uORB: Add subscription with single field selection

CMakeLists.txt

@@ -256,7 +256,7 @@ endif()
 
 set(package-contact "px4users@googlegroups.com")
 
-set(CMAKE_CXX_STANDARD 14)
+set(CMAKE_CXX_STANDARD 17)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_C_STANDARD_REQUIRED ON)

platforms/common/uORB/Subscription.hpp

@@ -245,4 +245,71 @@ private:
 	T _data{};
 };
 
+template<auto member>
+class SubscriptionSelection;
+
+template <class T, class R, R T::*member>
+class SubscriptionSelection<member>: public Subscription
+{
+public:
+	/**
+	 * Constructor
+	 *
+	 * @param id The uORB metadata ORB_ID enum for the topic.
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionSelection(ORB_ID id, uint8_t instance = 0) :
+		Subscription(id, instance)
+	{
+		_copySelection();
+	}
+
+	/**
+	 * Constructor
+	 *
+	 * @param meta The uORB metadata (usually from the ORB_ID() macro) for the topic.
+	 * @param instance The instance for multi sub.
+	 */
+	SubscriptionSelection(const orb_metadata *meta, uint8_t instance = 0) :
+		Subscription(meta, instance)
+	{
+		_copySelection();
+	}
+
+	~SubscriptionSelection() = default;
+
+	// no copy, assignment, move, move assignment
+	SubscriptionSelection(const SubscriptionSelection &) = delete;
+	SubscriptionSelection &operator=(const SubscriptionSelection &) = delete;
+	SubscriptionSelection(SubscriptionSelection &&) = delete;
+	SubscriptionSelection &operator=(SubscriptionSelection &&) = delete;
+
+	// update the embedded struct.
+	bool update()
+	{
+		bool updated = Subscription::updated();
+
+		if (updated) {
+			_copySelection();
+		}
+
+		return updated;
+	}
+
+	const R &get() const { return _data; }
+
+private:
+	void _copySelection()
+	{
+		T full_data;
+
+		if (copy(&full_data)) {
+			_data = full_data.*member;
+		}
+	}
+
+	R _data{};
+};
+
+
 } // namespace uORB

src/drivers/rc_input/RCInput.cpp

@@ -325,13 +325,7 @@ void RCInput::Run()
 			updateParams();
 		}
 
-		if (_vehicle_status_sub.updated()) {
-			vehicle_status_s vehicle_status;
-
-			if (_vehicle_status_sub.copy(&vehicle_status)) {
-				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			}
-		}
+		_vehicle_status_arming_state_sub.update();
 
 		const hrt_abstime cycle_timestamp = hrt_absolute_time();
 
@@ -346,7 +340,7 @@ void RCInput::Run()
 				uint8_t cmd_ret = vehicle_command_ack_s::VEHICLE_CMD_RESULT_UNSUPPORTED;
 #if defined(SPEKTRUM_POWER)
 
-				if (!_rc_scan_locked && !_armed) {
+				if (!_rc_scan_locked && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 					if ((int)vcmd.param1 == 0) {
 						// DSM binding command
 						int dsm_bind_mode = (int)vcmd.param2;
@@ -758,7 +752,8 @@ void RCInput::Run()
 
 			_to_input_rc.publish(_rc_in);
 
-		} else if (!rc_updated && !_armed && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
+		} else if (!rc_updated && !(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)
+			   && (hrt_elapsed_time(&_rc_in.timestamp_last_signal) > 1_s)) {
 			_rc_scan_locked = false;
 		}
 
@@ -767,7 +762,7 @@ void RCInput::Run()
 		}
 
 		// set RC_INPUT_PROTO if RC successfully locked for > 3 seconds
-		if (!_armed && rc_updated && _rc_scan_locked
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && rc_updated && _rc_scan_locked
 		    && ((_rc_scan_begin != 0) && hrt_elapsed_time(&_rc_scan_begin) > 3_s)
 		    && (_param_rc_input_proto.get() < 0)
 		   ) {

src/drivers/rc_input/RCInput.hpp

@@ -138,16 +138,13 @@ private:
 
 	uORB::Subscription	_adc_report_sub{ORB_ID(adc_report)};
 	uORB::Subscription	_vehicle_cmd_sub{ORB_ID(vehicle_command)};
-	uORB::Subscription	_vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state>	_vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 
 	input_rc_s	_rc_in{};
 
 	float		_analog_rc_rssi_volt{-1.0f};
 	bool		_analog_rc_rssi_stable{false};
 
-	bool _armed{false};
-
-
 	uORB::PublicationMulti<input_rc_s>	_to_input_rc{ORB_ID(input_rc)};
 
 	int		_rcs_fd{-1};

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.cpp

@@ -106,14 +106,8 @@ void FwAutotuneAttitudeControl::Run()
 		return;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-			_nav_state = vehicle_status.nav_state;
-		}
-	}
+	_vehicle_status_armed_state_sub.update();
+	_vehicle_status_nav_state_sub.update();
 
 	if (_actuator_controls_status_sub.updated()) {
 		actuator_controls_status_s controls_status;
@@ -296,7 +290,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 			mavlink_log_info(&_mavlink_log_pub, "Autotune started");
 			_state = state::init;
 			_state_start_time = now;
-			_start_flight_mode = _nav_state;
+			_start_flight_mode = _vehicle_status_nav_state_sub.get();
 		}
 
 		break;
@@ -429,7 +423,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::wait_for_disarm:
-		if (!_armed) {
+		if (!(_vehicle_status_armed_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 			saveGainsToParams();
 			_state = state::complete;
 			_state_start_time = now;
@@ -481,7 +475,7 @@ void FwAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 	if (_state != state::wait_for_disarm && _state != state::idle && _state != state::fail && _state != state::complete) {
 		if (now - _state_start_time > 20_s
 		    || (_param_fw_at_man_aux.get() && !_aux_switch_en)
-		    || _start_flight_mode != _nav_state) {
+		    || _start_flight_mode != _vehicle_status_nav_state_sub.get()) {
 			orb_advert_t mavlink_log_pub = nullptr;
 			mavlink_log_critical(&mavlink_log_pub, "Autotune aborted before finishing");
 			_state = state::fail;

src/modules/fw_autotune_attitude_control/fw_autotune_attitude_control.hpp

@@ -114,7 +114,8 @@ private:
 	uORB::Subscription _actuator_controls_status_sub;
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_armed_state_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::nav_state> _vehicle_status_nav_state_sub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -142,8 +143,6 @@ private:
 	uint8_t _max_steps{5};
 	int8_t _signal_sign{0};
 
-	bool _armed{false};
-	uint8_t _nav_state{0};
 	uint8_t _start_flight_mode{0};
 	bool _aux_switch_en{false};
 

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.cpp

@@ -96,13 +96,7 @@ void McAutotuneAttitudeControl::Run()
 		return;
 	}
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-		}
-	}
+	_vehicle_status_arming_state_sub.update();
 
 	if (_actuator_controls_status_sub.updated()) {
 		actuator_controls_status_s controls_status;
@@ -399,7 +393,7 @@ void McAutotuneAttitudeControl::updateStateMachine(hrt_abstime now)
 		break;
 
 	case state::wait_for_disarm:
-		if (!_armed) {
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 			saveGainsToParams();
 			_state = state::complete;
 			_state_start_time = now;

src/modules/mc_autotune_attitude_control/mc_autotune_attitude_control.hpp

@@ -108,7 +108,7 @@ private:
 	uORB::Subscription _actuator_controls_status_sub{ORB_ID(actuator_controls_status_0)};
 	uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
 	uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 
 	uORB::PublicationData<autotune_attitude_control_status_s> _autotune_attitude_control_status_pub{ORB_ID(autotune_attitude_control_status)};
 
@@ -136,8 +136,6 @@ private:
 	uint8_t _max_steps{5};
 	int8_t _signal_sign{0};
 
-	bool _armed{false};
-
 	matrix::Vector3f _kid{};
 	matrix::Vector3f _rate_k{};
 	matrix::Vector3f _rate_i{};

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp

@@ -151,18 +151,13 @@ void MulticopterHoverThrustEstimator::Run()
 
 	perf_begin(_cycle_perf);
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-		}
-	}
+	_vehicle_status_arming_state_sub.update();
 
 	const float dt = (local_pos.timestamp - _timestamp_last) * 1e-6f;
 	_timestamp_last = local_pos.timestamp;
 
-	if (_armed && _in_air && (dt > 0.001f) && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
+	if ((_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) && _in_air && (dt > 0.001f)
+	    && (dt < 1.f) && PX4_ISFINITE(local_pos.az)) {
 
 		_hover_thrust_ekf.predict(dt);
 
@@ -198,7 +193,7 @@ void MulticopterHoverThrustEstimator::Run()
 	} else {
 		_valid_hysteresis.set_state_and_update(false, hrt_absolute_time());
 
-		if (!_armed) {
+		if (!(_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED)) {
 			reset();
 		}
 

src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.hpp

@@ -102,12 +102,11 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 	uORB::Subscription _vehicle_local_position_setpoint_sub{ORB_ID(vehicle_local_position_setpoint)};
 
 	hrt_abstime _timestamp_last{0};
 
-	bool _armed{false};
 	bool _landed{false};
 	bool _in_air{false};
 

src/modules/sensors/vehicle_imu/VehicleIMU.cpp

@@ -181,13 +181,7 @@ void VehicleIMU::Run()
 	ScheduleDelayed(_backup_schedule_timeout_us);
 
 	// check vehicle status for changes to armed state
-	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode;
-
-		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_armed = vehicle_control_mode.flag_armed;
-		}
-	}
+	_vehicle_control_mode_armed_sub.update();
 
 	// reset data gap monitor
 	_data_gap = false;
@@ -254,7 +248,7 @@ void VehicleIMU::Run()
 	}
 
 	if (_param_sens_imu_autocal.get() && !parameters_updated) {
-		if ((_armed || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
+		if ((_vehicle_control_mode_armed_sub.get() || !_accel_calibration.calibrated() || !_gyro_calibration.calibrated())
 		    && (now_us > _in_flight_calibration_check_timestamp_last + 1_s)) {
 
 			SensorCalibrationUpdate();

src/modules/sensors/vehicle_imu/VehicleIMU.hpp

@@ -109,7 +109,8 @@ private:
 	uORB::Subscription _sensor_accel_sub;
 	uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub;
 
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::SubscriptionSelection<&vehicle_control_mode_s::flag_armed>
+	_vehicle_control_mode_armed_sub{ORB_ID(vehicle_control_mode)};
 
 	calibration::Accelerometer _accel_calibration{};
 	calibration::Gyroscope _gyro_calibration{};

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -142,7 +142,7 @@ bool VehicleMagnetometer::ParametersUpdate(bool force)
 		_mag_comp_type = mag_comp_typ;
 
 
-		if (!_armed) {
+		if (!_vehicle_control_mode_armed_sub.get()) {
 			bool calibration_updated = false;
 
 			// update mag priority (CAL_MAGx_PRIO)
@@ -219,7 +219,7 @@ void VehicleMagnetometer::UpdateMagBiasEstimate()
 					_calibration_estimator_bias[mag_index] = bias;
 
 					// set initial mag calibration if disarmed, mag uncalibrated, and valid estimated bias available
-					if (_param_sens_mag_autocal.get() && !_armed && mag_bias_est.stable[mag_index]
+					if (_param_sens_mag_autocal.get() && !_vehicle_control_mode_armed_sub.get() && mag_bias_est.stable[mag_index]
 					    && (_calibration[mag_index].device_id() != 0) && !_calibration[mag_index].calibrated()) {
 
 						const Vector3f old_offset = _calibration[mag_index].offset();
@@ -263,7 +263,7 @@ void VehicleMagnetometer::UpdateMagCalibration()
 	static constexpr float min_var_allowed = magb_vref * 0.01f;
 	static constexpr float max_var_allowed = magb_vref * 500.f;
 
-	if (_armed) {
+	if (_vehicle_control_mode_armed_sub.get()) {
 		static constexpr uint8_t mag_cal_size = sizeof(_mag_cal) / sizeof(_mag_cal[0]);
 
 		for (int i = 0; i < math::min(_estimator_sensor_bias_subs.size(), mag_cal_size); i++) {
@@ -363,7 +363,7 @@ void VehicleMagnetometer::UpdatePowerCompensation()
 {
 	if (_mag_comp_type != MagCompensationType::Disabled) {
 		// update power signal for mag compensation
-		if (_armed && (_mag_comp_type == MagCompensationType::Throttle)) {
+		if (_vehicle_control_mode_armed_sub.get() && (_mag_comp_type == MagCompensationType::Throttle)) {
 			vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
 
 			if (_vehicle_thrust_setpoint_0_sub.update(&vehicle_thrust_setpoint)) {
@@ -403,14 +403,8 @@ void VehicleMagnetometer::Run()
 
 	const bool parameter_update = ParametersUpdate();
 
-	// check vehicle status for changes to armed state
-	if (_vehicle_control_mode_sub.updated()) {
-		vehicle_control_mode_s vehicle_control_mode;
-
-		if (_vehicle_control_mode_sub.copy(&vehicle_control_mode)) {
-			_armed = vehicle_control_mode.flag_armed;
-		}
-	}
+	// update armed state
+	_vehicle_control_mode_armed_sub.update();
 
 	UpdatePowerCompensation();
 
@@ -569,7 +563,7 @@ void VehicleMagnetometer::Run()
 		CheckFailover(time_now_us);
 	}
 
-	if (!_armed) {
+	if (!_vehicle_control_mode_armed_sub.get()) {
 		calcMagInconsistency();
 	}
 

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -113,7 +113,8 @@ private:
 	uORB::Subscription _vehicle_thrust_setpoint_0_sub{ORB_ID(vehicle_thrust_setpoint), 0};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
 	uORB::Subscription _magnetometer_bias_estimate_sub{ORB_ID(magnetometer_bias_estimate)};
-	uORB::Subscription _vehicle_control_mode_sub{ORB_ID(vehicle_control_mode)};
+	uORB::SubscriptionSelection<&vehicle_control_mode_s::flag_armed>
+	_vehicle_control_mode_armed_sub{ORB_ID(vehicle_control_mode)};
 
 	// Used to check, save and use learned magnetometer biases
 	uORB::SubscriptionMultiArray<estimator_sensor_bias_s> _estimator_sensor_bias_subs{ORB_ID::estimator_sensor_bias};
@@ -171,8 +172,6 @@ private:
 
 	int8_t _selected_sensor_sub_index{-1};
 
-	bool _armed{false};
-
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
 		(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,

src/modules/simulation/battery_simulator/BatterySimulator.cpp

@@ -72,19 +72,13 @@ void BatterySimulator::Run()
 
 	updateCommands();
 
-	if (_vehicle_status_sub.updated()) {
-		vehicle_status_s vehicle_status;
-
-		if (_vehicle_status_sub.copy(&vehicle_status)) {
-			_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
-		}
-	}
-
 	const hrt_abstime now_us = hrt_absolute_time();
 
 	const float discharge_interval_us = _param_sim_bat_drain.get() * 1000 * 1000;
 
-	if (_armed) {
+	_vehicle_status_arming_state_sub.update();
+
+	if (_vehicle_status_arming_state_sub.get() == vehicle_status_s::ARMING_STATE_ARMED) {
 		if (_last_integration_us != 0) {
 			_battery_percentage -= (now_us - _last_integration_us) / discharge_interval_us;
 		}

src/modules/simulation/battery_simulator/BatterySimulator.hpp

@@ -77,7 +77,7 @@ private:
 	uORB::Publication<battery_status_s> _battery_pub{ORB_ID(battery_status)};
 
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
-	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};
+	uORB::SubscriptionSelection<&vehicle_status_s::arming_state> _vehicle_status_arming_state_sub{ORB_ID(vehicle_status)};
 
 	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
 	uORB::Publication<vehicle_command_ack_s> _command_ack_pub{ORB_ID(vehicle_command_ack)};
@@ -86,7 +86,6 @@ private:
 
 	uint64_t _last_integration_us{0};
 	float _battery_percentage{1.f};
-	bool _armed{false};
 
 	bool _force_empty_battery{false};