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

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

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 <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);
 
-
-	// 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(&param_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);
 }
 

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 <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};
 };