land detector: move arming state into base class & set param when disarming

Before that, different modules (ekf2, commander & land detector) changed
params upon different events:
- ekf2 & commander set params after disarm
- land detector set params on land detected
If the 2 events were several 100ms appart, it led to 2 param saves, and
the latter param set could have been blocked by an ongoing save. And if
the land detector was blocked, it could lead to mag timeouts.

This patch makes all modules use the same event, thus only a single param
save will happen.

If we want to have guarantees to never block, we should introduce a
param_try_set() API method.

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -62,7 +62,6 @@ FixedwingLandDetector::FixedwingLandDetector()
 
 void FixedwingLandDetector::_initialize_topics()
 {
-	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_airspeedSub = orb_subscribe(ORB_ID(airspeed));
 	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
@@ -70,7 +69,6 @@ void FixedwingLandDetector::_initialize_topics()
 
 void FixedwingLandDetector::_update_topics()
 {
-	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 	_orb_update(ORB_ID(airspeed), _airspeedSub, &_airspeed);
 	_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
 	_orb_update(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);

src/modules/land_detector/FixedwingLandDetector.h

@@ -42,7 +42,6 @@
 
 #pragma once
 
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/sensor_bias.h>
 #include <uORB/topics/vehicle_local_position.h>
@@ -86,12 +85,10 @@ private:
 		float maxIntVelocity;
 	} _params{};
 
-	int _armingSub{-1};
 	int _airspeedSub{-1};
 	int _sensor_bias_sub{-1};
 	int _local_pos_sub{-1};
 
-	actuator_armed_s _arming{};
 	airspeed_s _airspeed{};
 	sensor_bias_s _sensors{};
 	vehicle_local_position_s _local_pos{};

src/modules/land_detector/LandDetector.cpp

@@ -90,6 +90,7 @@ void LandDetector::_cycle()
 		_p_total_flight_time_low = param_find("LND_FLIGHT_T_LO");
 
 		// Initialize uORB topics.
+		_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 		_initialize_topics();
 
 		_check_params(true);
@@ -98,6 +99,7 @@ void LandDetector::_cycle()
 	}
 
 	_check_params(false);
+	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 	_update_topics();
 	_update_state();
 
@@ -107,6 +109,8 @@ void LandDetector::_cycle()
 	const bool ground_contactDetected = (_state == LandDetectionState::GROUND_CONTACT);
 	const float alt_max = _get_max_altitude();
 
+	const hrt_abstime now = hrt_absolute_time();
+
 	// Only publish very first time or when the result has changed.
 	if ((_landDetectedPub == nullptr) ||
 	    (_landDetected.landed != landDetected) ||
@@ -115,20 +119,9 @@ void LandDetector::_cycle()
 	    (_landDetected.ground_contact != ground_contactDetected) ||
 	    (fabsf(_landDetected.alt_max - alt_max) > FLT_EPSILON)) {
 
-		hrt_abstime now = hrt_absolute_time();
-
 		if (!landDetected && _landDetected.landed) {
 			// We did take off
 			_takeoff_time = now;
-
-		} else if (_takeoff_time != 0 && landDetected && !_landDetected.landed) {
-			// We landed
-			_total_flight_time += now - _takeoff_time;
-			_takeoff_time = 0;
-			uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
-			param_set_no_notification(_p_total_flight_time_high, &flight_time);
-			flight_time = _total_flight_time & 0xffffffff;
-			param_set_no_notification(_p_total_flight_time_low, &flight_time);
 		}
 
 		_landDetected.timestamp = hrt_absolute_time();
@@ -143,6 +136,19 @@ void LandDetector::_cycle()
 				 &instance, ORB_PRIO_DEFAULT);
 	}
 
+	// set the flight time when disarming (not necessarily when landed, because all param changes should
+	// happen on the same event and it's better to set/save params while not in armed state)
+	if (_takeoff_time != 0 && !_arming.armed && _previous_arming_state) {
+		_total_flight_time += now - _takeoff_time;
+		_takeoff_time = 0;
+		uint32_t flight_time = (_total_flight_time >> 32) & 0xffffffff;
+		param_set_no_notification(_p_total_flight_time_high, &flight_time);
+		flight_time = _total_flight_time & 0xffffffff;
+		param_set_no_notification(_p_total_flight_time_low, &flight_time);
+	}
+
+	_previous_arming_state = _arming.armed;
+
 	perf_end(_cycle_perf);
 
 	if (!should_exit()) {

src/modules/land_detector/LandDetector.h

@@ -48,6 +48,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
 #include <uORB/uORB.h>
+#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/vehicle_land_detected.h>
 
 namespace land_detector
@@ -150,6 +151,7 @@ protected:
 	vehicle_land_detected_s _landDetected{};
 
 	int _parameterSub{-1};
+	int _armingSub{-1};
 
 	LandDetectionState _state{LandDetectionState::LANDED};
 
@@ -158,6 +160,8 @@ protected:
 	systemlib::Hysteresis _maybe_landed_hysteresis{true};
 	systemlib::Hysteresis _ground_contact_hysteresis{true};
 
+	struct actuator_armed_s	_arming {};
+
 private:
 	static void _cycle_trampoline(void *arg);
 
@@ -175,6 +179,8 @@ private:
 	struct work_s	_work {};
 
 	perf_counter_t	_cycle_perf;
+
+	bool _previous_arming_state{false}; ///< stores the previous _arming.armed state
 };
 
 

src/modules/land_detector/MulticopterLandDetector.cpp

@@ -77,7 +77,6 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_vehicleLocalPositionSub(-1),
 	_vehicleLocalPositionSetpointSub(-1),
 	_actuatorsSub(-1),
-	_armingSub(-1),
 	_attitudeSub(-1),
 	_sensor_bias_sub(-1),
 	_vehicle_control_mode_sub(-1),
@@ -85,7 +84,6 @@ MulticopterLandDetector::MulticopterLandDetector() :
 	_vehicleLocalPosition{},
 	_vehicleLocalPositionSetpoint{},
 	_actuators{},
-	_arming{},
 	_vehicleAttitude{},
 	_sensors{},
 	_control_mode{},
@@ -113,12 +111,11 @@ MulticopterLandDetector::MulticopterLandDetector() :
 
 void MulticopterLandDetector::_initialize_topics()
 {
-	// subscribe to position, attitude, arming and velocity changes
+	// subscribe to position, attitude and velocity changes
 	_vehicleLocalPositionSub = orb_subscribe(ORB_ID(vehicle_local_position));
 	_vehicleLocalPositionSetpointSub = orb_subscribe(ORB_ID(vehicle_local_position_setpoint));
 	_attitudeSub = orb_subscribe(ORB_ID(vehicle_attitude));
 	_actuatorsSub = orb_subscribe(ORB_ID(actuator_controls_0));
-	_armingSub = orb_subscribe(ORB_ID(actuator_armed));
 	_parameterSub = orb_subscribe(ORB_ID(parameter_update));
 	_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
 	_vehicle_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
@@ -131,7 +128,6 @@ void MulticopterLandDetector::_update_topics()
 	_orb_update(ORB_ID(vehicle_local_position_setpoint), _vehicleLocalPositionSetpointSub, &_vehicleLocalPositionSetpoint);
 	_orb_update(ORB_ID(vehicle_attitude), _attitudeSub, &_vehicleAttitude);
 	_orb_update(ORB_ID(actuator_controls_0), _actuatorsSub, &_actuators);
-	_orb_update(ORB_ID(actuator_armed), _armingSub, &_arming);
 	_orb_update(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensors);
 	_orb_update(ORB_ID(vehicle_control_mode), _vehicle_control_mode_sub, &_control_mode);
 	_orb_update(ORB_ID(battery_status), _battery_sub, &_battery);

src/modules/land_detector/MulticopterLandDetector.h

@@ -49,7 +49,6 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/actuator_controls.h>
-#include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/parameter_update.h>
@@ -130,7 +129,6 @@ private:
 	int _vehicleLocalPositionSub;
 	int _vehicleLocalPositionSetpointSub;
 	int _actuatorsSub;
-	int _armingSub;
 	int _attitudeSub;
 	int _sensor_bias_sub;
 	int _vehicle_control_mode_sub;
@@ -139,7 +137,6 @@ private:
 	struct vehicle_local_position_s				_vehicleLocalPosition;
 	struct vehicle_local_position_setpoint_s	_vehicleLocalPositionSetpoint;
 	struct actuator_controls_s					_actuators;
-	struct actuator_armed_s						_arming;
 	struct vehicle_attitude_s					_vehicleAttitude;
 	struct sensor_bias_s					_sensors;
 	struct vehicle_control_mode_s				_control_mode;