drivers/px4io move to uORB::Subscription

Author: dagar

src/drivers/px4io/px4io.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2019 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
@@ -68,13 +68,15 @@
 
 #include <rc/dsm.h>
 
+#include <lib/mathlib/mathlib.h>
 #include <lib/mixer/mixer.h>
 #include <perf/perf_counter.h>
 #include <systemlib/err.h>
 #include <parameters/param.h>
 #include <circuit_breaker/circuit_breaker.h>
 #include <systemlib/mavlink_log.h>
 
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
@@ -101,7 +103,9 @@
 #define PX4IO_REBOOT_BOOTLOADER		_IOC(0xff00, 2)
 #define PX4IO_CHECK_CRC			_IOC(0xff00, 3)
 
-#define UPDATE_INTERVAL_MIN		2			// 2 ms	-> 500 Hz
+static constexpr unsigned UPDATE_INTERVAL_MIN{2};	// 2 ms	-> 500 Hz
+static constexpr unsigned UPDATE_INTERVAL_MAX{100};	// 100 ms -> 10 Hz
+
 #define ORB_CHECK_INTERVAL		200000		// 200 ms -> 5 Hz
 #define IO_POLL_INTERVAL		20000		// 20 ms -> 50 Hz
 
@@ -245,14 +249,16 @@ class PX4IO : public cdev::CDev
 
 	/* subscribed topics */
 	int			_t_actuator_controls_0;	///< actuator controls group 0 topic
-	int			_t_actuator_controls_1;	///< actuator controls group 1 topic
-	int			_t_actuator_controls_2;	///< actuator controls group 2 topic
-	int			_t_actuator_controls_3;	///< actuator controls group 3 topic
-	int			_t_actuator_armed;	///< system armed control topic
-	int 			_t_vehicle_control_mode;///< vehicle control mode topic
-	int			_t_param;		///< parameter update topic
+
+	uORB::Subscription	_t_actuator_controls_1{ORB_ID(actuator_controls_1)};	///< actuator controls group 1 topic
+	uORB::Subscription	_t_actuator_controls_2{ORB_ID(actuator_controls_2)};;	///< actuator controls group 2 topic
+	uORB::Subscription	_t_actuator_controls_3{ORB_ID(actuator_controls_3)};;	///< actuator controls group 3 topic
+	uORB::Subscription	_t_actuator_armed{ORB_ID(actuator_armed)};		///< system armed control topic
+	uORB::Subscription 	_t_vehicle_control_mode{ORB_ID(vehicle_control_mode)};	///< vehicle control mode topic
+	uORB::Subscription	_t_param{ORB_ID(parameter_update)};			///< parameter update topic
+	uORB::Subscription	_t_vehicle_command{ORB_ID(vehicle_command)};		///< vehicle command topic
+
 	bool			_param_update_force;	///< force a parameter update
-	int			_t_vehicle_command;	///< vehicle command topic
 
 	/* advertised topics */
 	orb_advert_t 		_to_input_rc;		///< rc inputs from io
@@ -466,14 +472,7 @@ PX4IO::PX4IO(device::Device *interface) :
 	_last_written_arming_s(0),
 	_last_written_arming_c(0),
 	_t_actuator_controls_0(-1),
-	_t_actuator_controls_1(-1),
-	_t_actuator_controls_2(-1),
-	_t_actuator_controls_3(-1),
-	_t_actuator_armed(-1),
-	_t_vehicle_control_mode(-1),
-	_t_param(-1),
 	_param_update_force(false),
-	_t_vehicle_command(-1),
 	_to_input_rc(nullptr),
 	_to_outputs(nullptr),
 	_to_servorail(nullptr),
@@ -683,20 +682,17 @@ PX4IO::init()
 		 * remains untouched (so manual override is still available).
 		 */
 
-		int safety_sub = orb_subscribe(ORB_ID(actuator_armed));
+		uORB::Subscription actuator_armed_sub{ORB_ID(actuator_armed)};
+
 		/* fill with initial values, clear updated flag */
-		struct actuator_armed_s safety;
+		actuator_armed_s actuator_armed{};
 		uint64_t try_start_time = hrt_absolute_time();
-		bool updated = false;
 
 		/* keep checking for an update, ensure we got a arming information,
 		   not something that was published a long time ago. */
 		do {
-			orb_check(safety_sub, &updated);
-
-			if (updated) {
-				/* got data, copy and exit loop */
-				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
+			if (actuator_armed_sub.update(&actuator_armed)) {
+				// updated data, exit loop
 				break;
 			}
 
@@ -746,11 +742,7 @@ PX4IO::init()
 
 			/* spin here until IO's state has propagated into the system */
 			do {
-				orb_check(safety_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
-				}
+				actuator_armed_sub.update(&actuator_armed);
 
 				/* wait 50 ms */
 				px4_usleep(50000);
@@ -762,13 +754,13 @@ PX4IO::init()
 				}
 
 				/* re-send if necessary */
-				if (!safety.force_failsafe) {
+				if (!actuator_armed.force_failsafe) {
 					orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
 					PX4_WARN("re-sending flight termination cmd");
 				}
 
 				/* keep waiting for state change for 2 s */
-			} while (!safety.force_failsafe);
+			} while (!actuator_armed.force_failsafe);
 		}
 
 		/* send command to arm system via command API */
@@ -781,11 +773,7 @@ PX4IO::init()
 
 		/* spin here until IO's state has propagated into the system */
 		do {
-			orb_check(safety_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(actuator_armed), safety_sub, &safety);
-			}
+			actuator_armed_sub.update(&actuator_armed);
 
 			/* wait 50 ms */
 			px4_usleep(50000);
@@ -797,13 +785,13 @@ PX4IO::init()
 			}
 
 			/* re-send if necessary */
-			if (!safety.armed) {
+			if (!actuator_armed.armed) {
 				orb_publish(ORB_ID(vehicle_command), pub, &vcmd);
 				PX4_WARN("re-sending arm cmd");
 			}
 
 			/* keep waiting for state change for 2 s */
-		} while (!safety.armed);
+		} while (!actuator_armed.armed);
 
 		/* Indicate restart type is in-flight */
 		sys_restart_val = DM_INIT_REASON_IN_FLIGHT;
@@ -902,23 +890,9 @@ PX4IO::task_main()
 	 */
 	_t_actuator_controls_0 = orb_subscribe(ORB_ID(actuator_controls_0));
 	orb_set_interval(_t_actuator_controls_0, 20);		/* default to 50Hz */
-	_t_actuator_controls_1 = orb_subscribe(ORB_ID(actuator_controls_1));
-	orb_set_interval(_t_actuator_controls_1, 33);		/* default to 30Hz */
-	_t_actuator_controls_2 = orb_subscribe(ORB_ID(actuator_controls_2));
-	orb_set_interval(_t_actuator_controls_2, 33);		/* default to 30Hz */
-	_t_actuator_controls_3 = orb_subscribe(ORB_ID(actuator_controls_3));
-	orb_set_interval(_t_actuator_controls_3, 33);		/* default to 30Hz */
-	_t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed));
-	_t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_t_param = orb_subscribe(ORB_ID(parameter_update));
-	_t_vehicle_command = orb_subscribe(ORB_ID(vehicle_command));
-
-	if ((_t_actuator_controls_0 < 0) ||
-	    (_t_actuator_armed < 0) ||
-	    (_t_vehicle_control_mode < 0) ||
-	    (_t_param < 0) ||
-	    (_t_vehicle_command < 0)) {
-		warnx("subscription(s) failed");
+
+	if (_t_actuator_controls_0 < 0) {
+		PX4_ERR("actuator subscription failed");
 		goto out;
 	}
 
@@ -940,13 +914,7 @@ PX4IO::task_main()
 
 		/* adjust update interval */
 		if (_update_interval != 0) {
-			if (_update_interval < UPDATE_INTERVAL_MIN) {
-				_update_interval = UPDATE_INTERVAL_MIN;
-			}
-
-			if (_update_interval > 100) {
-				_update_interval = 100;
-			}
+			_update_interval = math::constrain(_update_interval, UPDATE_INTERVAL_MIN, UPDATE_INTERVAL_MAX);
 
 			orb_set_interval(_t_actuator_controls_0, _update_interval);
 			/*
@@ -995,10 +963,10 @@ PX4IO::task_main()
 			bool updated = false;
 
 			/* arming state */
-			orb_check(_t_actuator_armed, &updated);
+			updated = _t_actuator_armed.updated();
 
 			if (!updated) {
-				orb_check(_t_vehicle_control_mode, &updated);
+				updated = _t_vehicle_control_mode.updated();
 			}
 
 			if (updated) {
@@ -1010,15 +978,10 @@ PX4IO::task_main()
 			/* run at 5Hz */
 			orb_check_last = now;
 
-			/* check updates on uORB topics and handle it */
-			bool updated = false;
-
 			/* vehicle command */
-			orb_check(_t_vehicle_command, &updated);
-
-			if (updated) {
-				struct vehicle_command_s cmd;
-				orb_copy(ORB_ID(vehicle_command), _t_vehicle_command, &cmd);
+			if (_t_vehicle_command.updated()) {
+				vehicle_command_s cmd{};
+				_t_vehicle_command.copy(&cmd);
 
 				// Check for a DSM pairing command
 				if (((unsigned int)cmd.command == vehicle_command_s::VEHICLE_CMD_START_RX_PAIR) && ((int)cmd.param1 == 0)) {
@@ -1031,12 +994,10 @@ PX4IO::task_main()
 			 *
 			 * XXX this may be a bit spammy
 			 */
-			orb_check(_t_param, &updated);
-
-			if (updated || _param_update_force) {
+			if (_t_param.updated() || _param_update_force) {
 				_param_update_force = false;
 				parameter_update_s pupdate;
-				orb_copy(ORB_ID(parameter_update), _t_param, &pupdate);
+				_t_param.copy(&pupdate);
 
 				if (!_rc_handling_disabled) {
 					/* re-upload RC input config as it may have changed */
@@ -1295,8 +1256,7 @@ PX4IO::io_set_control_groups()
 int
 PX4IO::io_set_control_state(unsigned group)
 {
-	actuator_controls_s	controls;	///< actuator outputs
-	uint16_t 		regs[_max_actuators];
+	actuator_controls_s	controls{};	///< actuator outputs
 
 	/* get controls */
 	bool changed = false;
@@ -1312,31 +1272,16 @@ PX4IO::io_set_control_state(unsigned group)
 		}
 		break;
 
-	case 1: {
-			orb_check(_t_actuator_controls_1, &changed);
-
-			if (changed) {
-				orb_copy(ORB_ID(actuator_controls_1), _t_actuator_controls_1, &controls);
-			}
-		}
+	case 1:
+		changed = _t_actuator_controls_1.update(&controls);
 		break;
 
-	case 2: {
-			orb_check(_t_actuator_controls_2, &changed);
-
-			if (changed) {
-				orb_copy(ORB_ID(actuator_controls_2), _t_actuator_controls_2, &controls);
-			}
-		}
+	case 2:
+		changed = _t_actuator_controls_2.update(&controls);
 		break;
 
-	case 3: {
-			orb_check(_t_actuator_controls_3, &changed);
-
-			if (changed) {
-				orb_copy(ORB_ID(actuator_controls_3), _t_actuator_controls_3, &controls);
-			}
-		}
+	case 3:
+		changed = _t_actuator_controls_3.update(&controls);
 		break;
 	}
 
@@ -1351,17 +1296,11 @@ PX4IO::io_set_control_state(unsigned group)
 		controls.control[3] = 1.0f;
 	}
 
-	for (unsigned i = 0; i < _max_controls; i++) {
+	uint16_t regs[_max_actuators];
 
+	for (unsigned i = 0; i < _max_controls; i++) {
 		/* ensure FLOAT_TO_REG does not produce an integer overflow */
-		float ctrl = controls.control[i];
-
-		if (ctrl < -1.0f) {
-			ctrl = -1.0f;
-
-		} else if (ctrl > 1.0f) {
-			ctrl = 1.0f;
-		}
+		const float ctrl = math::constrain(controls.control[i], -1.0f, 1.0f);
 
 		regs[i] = FLOAT_TO_REG(ctrl);
 	}
@@ -1375,21 +1314,15 @@ PX4IO::io_set_control_state(unsigned group)
 	}
 }
 
-
 int
 PX4IO::io_set_arming_state()
 {
-	actuator_armed_s	armed;		///< system armed state
-	vehicle_control_mode_s	control_mode;	///< vehicle_control_mode
-
-	int have_armed = orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed);
-	int have_control_mode = orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode);
-	_in_esc_calibration_mode = armed.in_esc_calibration_mode;
-
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	if (have_armed == OK) {
+	actuator_armed_s armed;
+
+	if (_t_actuator_armed.copy(&armed)) {
 		_in_esc_calibration_mode = armed.in_esc_calibration_mode;
 
 		if (armed.armed || _in_esc_calibration_mode) {
@@ -1433,7 +1366,9 @@ PX4IO::io_set_arming_state()
 		}
 	}
 
-	if (have_control_mode == OK) {
+	vehicle_control_mode_s control_mode;
+
+	if (_t_vehicle_control_mode.copy(&control_mode)) {
 		if (control_mode.flag_external_manual_override_ok) {
 			set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK;
 			_override_available = true;
@@ -2913,19 +2848,10 @@ PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
 int
 PX4IO::set_update_rate(int rate)
 {
-	int interval_ms = 1000 / rate;
+	unsigned interval_ms = 1000 / rate;
 
-	if (interval_ms < UPDATE_INTERVAL_MIN) {
-		interval_ms = UPDATE_INTERVAL_MIN;
-		warnx("update rate too high, limiting interval to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
-	}
-
-	if (interval_ms > 100) {
-		interval_ms = 100;
-		warnx("update rate too low, limiting to %d ms (%d Hz).", interval_ms, 1000 / interval_ms);
-	}
+	_update_interval = math::constrain(interval_ms, UPDATE_INTERVAL_MIN, UPDATE_INTERVAL_MAX);
 
-	_update_interval = interval_ms;
 	return 0;
 }