Commit 6a4c589c authored by Daniel Agar's avatar Daniel Agar
Browse files

examples/work_item: add ModuleParams and more uORB::Subscription example usage

parent bf09089a
############################################################################
#
# Copyright (c) 2020 PX4 Development Team. All rights reserved.
# Copyright (c) 2021 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
......@@ -35,6 +35,8 @@ px4_add_module(
MODULE examples__work_item
MAIN work_item_example
COMPILE_FLAGS
#-DDEBUG_BUILD # uncomment for PX4_DEBUG output
#-O0 # uncomment when debugging
SRCS
WorkItemExample.cpp
WorkItemExample.hpp
......
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
......@@ -33,10 +33,6 @@
#include "WorkItemExample.hpp"
#include <drivers/drv_hrt.h>
using namespace time_literals;
WorkItemExample::WorkItemExample() :
ModuleParams(nullptr),
ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::test1)
......@@ -51,7 +47,14 @@ WorkItemExample::~WorkItemExample()
bool WorkItemExample::init()
{
ScheduleOnInterval(1000_us); // 1000 us interval, 1000 Hz rate
// execute Run() on every sensor_accel publication
if (!_sensor_accel_sub.registerCallback()) {
PX4_ERR("sensor_accel callback registration failed");
return false;
}
// alternatively, Run on fixed interval
// ScheduleOnInterval(5000_us); // 2000 us interval, 200 Hz rate
return true;
}
......@@ -67,27 +70,60 @@ void WorkItemExample::Run()
perf_begin(_loop_perf);
perf_count(_loop_interval_perf);
// Check if parameters have changed
if (_parameter_update_sub.updated()) {
// clear update
parameter_update_s param_update;
_parameter_update_sub.copy(&param_update);
updateParams(); // update module parameters (in DEFINE_PARAMETERS)
}
// Example
// update vehicle_status to check arming state
if (_vehicle_status_sub.updated()) {
vehicle_status_s vehicle_status;
// DO WORK
if (_vehicle_status_sub.copy(&vehicle_status)) {
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
if (armed && !_armed) {
PX4_WARN("vehicle armed due to %d", vehicle_status.latest_arming_reason);
} else if (!armed && _armed) {
PX4_INFO("vehicle disarmed due to %d", vehicle_status.latest_disarming_reason);
}
_armed = armed;
}
}
// Example
// grab latest accelerometer data
_sensor_accel_sub.update();
const sensor_accel_s &accel = _sensor_accel_sub.get();
// grab latest accelerometer data
if (_sensor_accel_sub.updated()) {
sensor_accel_s accel;
if (_sensor_accel_sub.copy(&accel)) {
// DO WORK
// access parameter value (SYS_AUTOSTART)
if (_param_sys_autostart.get() == 1234) {
// do something if SYS_AUTOSTART is 1234
}
}
}
// Example
// publish some data
// publish some data
orb_test_s data{};
data.val = 314159;
data.timestamp = hrt_absolute_time();
data.val = accel.device_id;
_orb_test_pub.publish(data);
perf_end(_loop_perf);
}
......
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
* Copyright (c) 2021 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
......@@ -33,17 +33,24 @@
#pragma once
#include <lib/perf/perf_counter.h>
#include <px4_platform_common/defines.h>
#include <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/posix.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/sensor_accel.h>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <uORB/Publication.hpp>
#include <uORB/Subscription.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/orb_test.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/vehicle_status.h>
using namespace time_literals;
class WorkItemExample : public ModuleBase<WorkItemExample>, public ModuleParams, public px4::ScheduledWorkItem
{
......@@ -67,10 +74,24 @@ public:
private:
void Run() override;
// Publications
uORB::Publication<orb_test_s> _orb_test_pub{ORB_ID(orb_test)};
uORB::SubscriptionData<sensor_accel_s> _sensor_accel_sub{ORB_ID(sensor_accel)};
// Subscriptions
uORB::SubscriptionCallbackWorkItem _sensor_accel_sub{this, ORB_ID(sensor_accel)}; // subscription that schedules WorkItemExample when updated
uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates
uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; // regular subscription for additional data
// Performance (perf) counters
perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
// Parameters
DEFINE_PARAMETERS(
(ParamInt<px4::params::SYS_AUTOSTART>) _param_sys_autostart, /**< example parameter */
(ParamInt<px4::params::SYS_AUTOCONFIG>) _param_sys_autoconfig /**< another parameter */
)
bool _armed{false};
};
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment