From 6a4c589c62fef78140e96acb6f2bad52c1dd7509 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Tue, 13 Apr 2021 11:29:15 -0400 Subject: [PATCH] examples/work_item: add ModuleParams and more uORB::Subscription example usage --- src/examples/work_item/CMakeLists.txt | 4 +- src/examples/work_item/WorkItemExample.cpp | 74 ++++++++++++++++------ src/examples/work_item/WorkItemExample.hpp | 31 +++++++-- 3 files changed, 84 insertions(+), 25 deletions(-) diff --git a/src/examples/work_item/CMakeLists.txt b/src/examples/work_item/CMakeLists.txt index 4b4f518c3e..433108ac89 100644 --- a/src/examples/work_item/CMakeLists.txt +++ b/src/examples/work_item/CMakeLists.txt @@ -1,6 +1,6 @@ ############################################################################ # -# 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 diff --git a/src/examples/work_item/WorkItemExample.cpp b/src/examples/work_item/WorkItemExample.cpp index 674c174228..33d2c4258b 100644 --- a/src/examples/work_item/WorkItemExample.cpp +++ b/src/examples/work_item/WorkItemExample.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * 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 - -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); - - // DO WORK - - - - // Example - // grab latest accelerometer data - _sensor_accel_sub.update(); - const sensor_accel_s &accel = _sensor_accel_sub.get(); + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); // update module parameters (in DEFINE_PARAMETERS) + } // Example - // publish some data + // update vehicle_status to check arming state + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; + + 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 + 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 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); } diff --git a/src/examples/work_item/WorkItemExample.hpp b/src/examples/work_item/WorkItemExample.hpp index a38e03ac3f..a51454f625 100644 --- a/src/examples/work_item/WorkItemExample.hpp +++ b/src/examples/work_item/WorkItemExample.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * 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 #include #include #include #include #include -#include -#include + +#include +#include + #include #include #include +#include +#include +#include +#include + +using namespace time_literals; class WorkItemExample : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { @@ -67,10 +74,24 @@ public: private: void Run() override; + // Publications uORB::Publication _orb_test_pub{ORB_ID(orb_test)}; - uORB::SubscriptionData _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) _param_sys_autostart, /**< example parameter */ + (ParamInt) _param_sys_autoconfig /**< another parameter */ + ) + + + bool _armed{false}; };