fw pos ctrl: rework landing abort status

abort boolean and reasons no longer separated, single status field with corresponding abort triggers

msg/position_controller_landing_status.msg

@@ -1,11 +1,16 @@
 uint64 timestamp # [us] time since system start
 float32 lateral_touchdown_offset # [m] lateral touchdown position offset manually commanded during landing
 bool flaring # true if the aircraft is flaring
-bool abort_landing # true if landing should be aborted
-int32 abort_reason # the singular abort criterion which triggered the landing abort
+
+# abort status is:
+# 0 if not aborted
+# >0 if aborted, with the singular abort criterion which triggered the landing abort enumerated by the following abort reasons
+uint8 abort_status
 
 # abort reasons
-# corresponds to individual bits of param FW_LND_ABORT
-uint8 kAbortReasonNone = 0
-uint8 kAbortReasonTerrainNotFound = 1 # (1 << 0)
-uint8 kAbortReasonTerrainTimeout = 2 # (1 << 1)
+# after the manual operator abort, corresponds to individual bits of param FW_LND_ABORT
+uint8 kNotAborted = 0
+uint8 kAbortedByOperator = 1
+uint8 kTerrainNotFound = 2 # FW_LND_ABORT (1 << 0)
+uint8 kTerrainTimeout = 3 # FW_LND_ABORT (1 << 1)
+uint8 kUnknownAbortCriterion = 4

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -234,7 +234,7 @@ FixedwingPositionControl::vehicle_command_poll()
 			    _pos_sp_triplet.current.valid &&
 			    (_pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_LAND)) {
 
-				abort_landing(true);
+				updateLandingAbortStatus(position_controller_landing_status_s::kAbortedByOperator);
 			}
 
 		} else if (vehicle_command.command == vehicle_command_s::VEHICLE_CMD_DO_CHANGE_SPEED) {
@@ -594,50 +594,57 @@ FixedwingPositionControl::status_publish()
 }
 
 void
-FixedwingPositionControl::landing_status_publish(const uint8_t abort_reason)
+FixedwingPositionControl::landing_status_publish()
 {
 	position_controller_landing_status_s pos_ctrl_landing_status = {};
 
 	pos_ctrl_landing_status.lateral_touchdown_offset = _lateral_touchdown_position_offset;
 	pos_ctrl_landing_status.flaring = _flaring;
-	pos_ctrl_landing_status.abort_landing = _land_abort;
-	pos_ctrl_landing_status.abort_reason = abort_reason;
+	pos_ctrl_landing_status.abort_status = _landing_abort_status;
 	pos_ctrl_landing_status.timestamp = hrt_absolute_time();
 
 	_pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status);
 }
 
 void
-FixedwingPositionControl::abort_landing(const bool abort, const uint8_t abort_reason)
+FixedwingPositionControl::updateLandingAbortStatus(const uint8_t new_abort_status)
 {
 	// only announce changes
-	if (abort && !_land_abort) {
+	if (new_abort_status > 0 && _landing_abort_status != new_abort_status) {
 
-		switch (abort_reason) {
-		case (position_controller_landing_status_s::kAbortReasonTerrainNotFound): {
+		switch (new_abort_status) {
+		case (position_controller_landing_status_s::kAbortedByOperator): {
+				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted by operator\t");
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_operator_abort"), events::Log::Critical,
+					     "Landing aborted by operator");
+				break;
+			}
+
+		case (position_controller_landing_status_s::kTerrainNotFound): {
 				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate not found\t");
-				events::send(events::ID("fixedwing_position_control_land_aborted_terrain_not_found"), events::Log::Critical,
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_not_found"), events::Log::Critical,
 					     "Landing aborted: terrain measurement not found");
 				break;
 			}
 
-		case (position_controller_landing_status_s::kAbortReasonTerrainTimeout): {
+		case (position_controller_landing_status_s::kTerrainTimeout): {
 				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: terrain estimate timed out\t");
-				events::send(events::ID("fixedwing_position_control_land_aborted_terrain_timeout"), events::Log::Critical,
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_terrain_timeout"), events::Log::Critical,
 					     "Landing aborted: terrain estimate timed out");
 				break;
 			}
 
 		default: {
-				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: reason unspecified\t");
-				events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical,
-					     "Landing aborted: reason unspecified");
+				mavlink_log_critical(&_mavlink_log_pub, "Landing aborted: unknown criterion\t");
+				events::send(events::ID("fixedwing_position_control_landing_abort_status_unknown_criterion"), events::Log::Critical,
+					     "Landing aborted: unknown criterion");
 			}
 		}
 	}
 
-	_land_abort = abort;
-	landing_status_publish(abort_reason);
+	_landing_abort_status = (new_abort_status >= position_controller_landing_status_s::kUnknownAbortCriterion) ?
+				position_controller_landing_status_s::kUnknownAbortCriterion : new_abort_status;
+	landing_status_publish();
 }
 
 void
@@ -1341,10 +1348,10 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons
 
 	float alt_sp = pos_sp_curr.alt;
 
-	if (_land_abort) {
+	if (_landing_abort_status) {
 		if (pos_sp_curr.alt - _current_altitude  < _param_fw_clmbout_diff.get()) {
 			// aborted landing complete, normal loiter over landing point
-			abort_landing(false);
+			updateLandingAbortStatus(position_controller_landing_status_s::kNotAborted);
 
 		} else {
 			// continue straight until vehicle has sufficient altitude
@@ -2357,9 +2364,9 @@ FixedwingPositionControl::reset_landing_state()
 	_last_time_terrain_alt_was_valid = 0;
 
 	// reset abort land, unless loitering after an abort
-	if (_land_abort && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {
+	if (_landing_abort_status && (_pos_sp_triplet.current.type != position_setpoint_s::SETPOINT_TYPE_LOITER)) {
 
-		abort_landing(false);
+		updateLandingAbortStatus(position_controller_landing_status_s::kNotAborted);
 	}
 }
 
@@ -2596,7 +2603,7 @@ FixedwingPositionControl::initializeAutoLanding(const hrt_abstime &now, const po
 			_landing_approach_entrance_offset_vector = Vector2f({cosf(_yaw), sinf(_yaw)}) * landing_approach_distance;
 		}
 
-		// save time at which we started landing and reset abort_landing
+		// save time at which we started landing and reset landing abort status
 		reset_landing_state();
 		_time_started_landing = now;
 	}
@@ -2662,11 +2669,11 @@ FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &n
 		if (_last_time_terrain_alt_was_valid == 0) {
 
 			const bool terrain_first_measurement_timed_out = (now - _time_started_landing) > TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT;
-			const bool abort_on_terrain_measurement_timeout = _param_fw_lnd_abort.get() &
-					position_controller_landing_status_s::kAbortReasonTerrainNotFound;
+			const bool abort_on_terrain_measurement_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(),
+					position_controller_landing_status_s::kTerrainNotFound);
 
 			if (terrain_first_measurement_timed_out && abort_on_terrain_measurement_timeout) {
-				abort_landing(true, position_controller_landing_status_s::kAbortReasonTerrainNotFound);
+				updateLandingAbortStatus(position_controller_landing_status_s::kTerrainNotFound);
 			}
 
 			return land_point_altitude;
@@ -2675,11 +2682,11 @@ FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &n
 		if (!_local_pos.dist_bottom_valid) {
 
 			const bool terrain_timed_out = (now - _last_time_terrain_alt_was_valid) > TERRAIN_ALT_TIMEOUT;
-			const bool abort_on_terrain_timeout = _param_fw_lnd_abort.get() &
-							      position_controller_landing_status_s::kAbortReasonTerrainTimeout;
+			const bool abort_on_terrain_timeout = checkLandingAbortBitMask(_param_fw_lnd_abort.get(),
+							      position_controller_landing_status_s::kTerrainTimeout);
 
 			if (terrain_timed_out && abort_on_terrain_timeout) {
-				abort_landing(true, position_controller_landing_status_s::kAbortReasonTerrainTimeout);
+				updateLandingAbortStatus(position_controller_landing_status_s::kTerrainTimeout);
 			}
 
 			return _last_valid_terrain_alt_estimate;
@@ -2689,6 +2696,20 @@ FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &n
 	return land_point_altitude;
 }
 
+bool FixedwingPositionControl::checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask,
+		uint8_t landing_abort_criterion)
+{
+	// landing abort status contains a manual criterion at abort_status==1, need to subtract 2 to directly compare
+	// to automatic criteria bits from the parameter FW_LND_ABORT
+	if (landing_abort_criterion <= 1) {
+		return false;
+	}
+
+	landing_abort_criterion -= 2;
+
+	return ((1 << landing_abort_criterion) & automatic_abort_criteria_bitmask) == (1 << landing_abort_criterion);
+}
+
 void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)
 {
 	if (_global_local_proj_ref.isInitialized()) {

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -338,7 +338,7 @@ private:
 	// [m] relative height above land point
 	float _landing_approach_entrance_rel_alt{0.0f};
 
-	bool _land_abort{false};
+	uint8_t _landing_abort_status{position_controller_landing_status_s::kNotAborted};
 
 	bool _flaring{false};
 	hrt_abstime _time_started_flaring{0}; // [us]
@@ -421,18 +421,25 @@ private:
 	void wind_poll();
 
 	void status_publish();
-	void landing_status_publish(const uint8_t abort_reason = position_controller_landing_status_s::kAbortReasonNone);
+	void landing_status_publish();
 	void tecs_status_publish();
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
 
 	/**
-	 * @brief Sets the aborted landing state and publishes landing status.
+	 * @brief Sets the landing abort status and publishes landing status.
 	 *
-	 * @param abort If true, the aircraft should abort the landing
-	 * @param abort_reason Singular bit which triggered the abort
+	 * @param new_abort_status Either 0 (not aborted) or the singular bit >0 which triggered the abort
 	 */
-	void abort_landing(const bool abort,
-			   const uint8_t abort_reason = position_controller_landing_status_s::kAbortReasonNone);
+	void updateLandingAbortStatus(const uint8_t new_abort_status = position_controller_landing_status_s::kNotAborted);
+
+	/**
+	 * @brief Checks if the automatic abort bitmask (from FW_LND_ABORT) contains the given abort criterion.
+	 *
+	 * @param automatic_abort_criteria_bitmask Bitmask containing all active abort criteria
+	 * @param landing_abort_criterion The specifc criterion we are checking for
+	 * @return true if the bitmask contains the criterion
+	 */
+	bool checkLandingAbortBitMask(const uint8_t automatic_abort_criteria_bitmask, uint8_t landing_abort_criterion);
 
 	/**
 	 * @brief Get a new waypoint based on heading and distance from current position

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -1053,8 +1053,8 @@ PARAM_DEFINE_INT32(FW_LND_NUDGE, 0);
  * Bit mask to set the automatic landing abort conditions.
  *
  * Terrain estimation:
- * 0: Abort if terrain is not found
- * 1: Abort if terrain times out (after a first successful measurement)
+ * bit 0: Abort if terrain is not found
+ * bit 1: Abort if terrain times out (after a first successful measurement)
  *
  * The last estimate is always used as ground, whether the last valid measurement or the land waypoint, depending on the
  * selected abort criteria, until an abort condition is entered. If FW_LND_USETER == 0, these bits are ignored.

src/modules/navigator/navigator_main.cpp

@@ -1407,7 +1407,7 @@ bool Navigator::abort_landing()
 			// landing status from position controller must be newer than navigator's last position setpoint
 			if (_pos_ctrl_landing_status_sub.copy(&landing_status)) {
 				if (landing_status.timestamp > _pos_sp_triplet.timestamp) {
-					should_abort = landing_status.abort_landing;
+					should_abort = (landing_status.abort_status > 0);
 				}
 			}
 		}