fw pos ctrl: refactor terrain estimate and land abort

- improve terrain estiamte documentation
- add landing abort condition bitmasked parameter to choose abort conditions
- refactor terrain estimate getter with landing abort logic
- log abort status and inform user
- log flaring status

msg/position_controller_landing_status.msg

@@ -1,3 +1,11 @@
 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 reasons
+# corresponds to individual bits of param FW_LND_ABORT
+uint8 kAbortReasonNone = 0
+uint8 kAbortReasonTerrainNotFound = 1 # (1 << 0)
+uint8 kAbortReasonTerrainTimeout = 2 # (1 << 1)

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -600,29 +600,50 @@ FixedwingPositionControl::status_publish()
 }
 
 void
-FixedwingPositionControl::landing_status_publish()
+FixedwingPositionControl::landing_status_publish(const uint8_t abort_reason)
 {
 	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.timestamp = hrt_absolute_time();
 
 	_pos_ctrl_landing_status_pub.publish(pos_ctrl_landing_status);
 }
 
 void
-FixedwingPositionControl::abort_landing(bool abort)
+FixedwingPositionControl::abort_landing(const bool abort, const uint8_t abort_reason)
 {
 	// only announce changes
 	if (abort && !_land_abort) {
-		mavlink_log_critical(&_mavlink_log_pub, "Landing aborted\t");
+
-		// TODO: add reason
+		switch (abort_reason) {
-		events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical, "Landing aborted");
+		case (position_controller_landing_status_s::kAbortReasonTerrainNotFound): {
+				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,
+					     "Landing aborted: terrain measurement not found");
+				break;
+			}
+
+		case (position_controller_landing_status_s::kAbortReasonTerrainTimeout): {
+				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,
+					     "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");
+			}
+		}
 	}
 
 	_land_abort = abort;
-	landing_status_publish();
+	landing_status_publish(abort_reason);
 }
 
 void
@@ -2609,44 +2630,45 @@ FixedwingPositionControl::calculateLandingApproachVector() const
 float
 FixedwingPositionControl::getLandingTerrainAltitudeEstimate(const hrt_abstime &now, const float land_point_altitude)
 {
-	float terrain_alt = land_point_altitude;
+	if (_param_fw_lnd_useter.get()) {
 
-	if (_param_fw_lnd_useter.get() == 1) {
 		if (_local_pos.dist_bottom_valid) {
-			// all good, have valid terrain altitude
+
-			float terrain_vpos = _local_pos.dist_bottom + _local_pos.z;
+			const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom;
-			terrain_alt = (_local_pos.ref_alt - terrain_vpos);
+			_last_valid_terrain_alt_estimate = terrain_estimate;
-			_last_valid_terrain_alt_estimate = terrain_alt;
 			_last_time_terrain_alt_was_valid = now;
 
-		} else if (_last_time_terrain_alt_was_valid == 0) {
+			return terrain_estimate;
-			// we have started landing phase but don't have valid terrain
+		}
-			// wait for some time, maybe we will soon get a valid estimate
-			// until then just use the altitude of the landing waypoint
-			if ((now - _time_started_landing) < TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT) {
-				terrain_alt = land_point_altitude;
 
-			} else {
+		if (_last_time_terrain_alt_was_valid == 0) {
-				// still no valid terrain, abort landing
+
-				terrain_alt = land_point_altitude;
+			const bool terrain_first_measurement_timed_out = (now - _time_started_landing) > TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT;
-				abort_landing(true);
+			const bool abort_on_terrain_measurement_timeout = _param_fw_lnd_abort.get() &
+					position_controller_landing_status_s::kAbortReasonTerrainNotFound;
+
+			if (terrain_first_measurement_timed_out && abort_on_terrain_measurement_timeout) {
+				abort_landing(true, position_controller_landing_status_s::kAbortReasonTerrainNotFound);
 			}
 
-		} else if ((!_local_pos.dist_bottom_valid && (now - _last_time_terrain_alt_was_valid) < TERRAIN_ALT_TIMEOUT)
+			return land_point_altitude;
-			   || _flaring) {
+		}
-			// use previous terrain estimate for some time and hope to recover
-			// if we are already flaring (land_noreturn_vertical) then just
-			//  go with the old estimate
-			terrain_alt = _last_valid_terrain_alt_estimate;
 
-		} else {
+		if (!_local_pos.dist_bottom_valid) {
-			// terrain alt was not valid for long time, abort landing
+
-			terrain_alt = _last_valid_terrain_alt_estimate;
+			const bool terrain_timed_out = (now - _last_time_terrain_alt_was_valid) > TERRAIN_ALT_TIMEOUT;
-			abort_landing(true);
+			const bool abort_on_terrain_timeout = _param_fw_lnd_abort.get() &
+							      position_controller_landing_status_s::kAbortReasonTerrainTimeout;
+
+			if (terrain_timed_out && abort_on_terrain_timeout) {
+				abort_landing(true, position_controller_landing_status_s::kAbortReasonTerrainTimeout);
+			}
+
+			return _last_valid_terrain_alt_estimate;
 		}
 	}
 
-	return terrain_alt;
+	return land_point_altitude;
 }
 
 void FixedwingPositionControl::publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint)

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -117,10 +117,12 @@ static constexpr float HDG_HOLD_YAWRATE_THRESH = 0.15f;
 // [.] max manual roll/yaw normalized input from user which does not change the locked heading
 static constexpr float HDG_HOLD_MAN_INPUT_THRESH = 0.01f;
 
-// [us] time after which we abort landing if terrain estimate is not valid
+// [us] time after which we abort landing if terrain estimate is not valid. this timer start whenever the terrain altitude
+// was previously valid, and has changed to invalid.
 static constexpr hrt_abstime TERRAIN_ALT_TIMEOUT = 1_s;
 
-// [us] if no altimeter measurement is made within this timeout, the land waypoint altitude is used for terrain altitude
+// [us] within this timeout, if a distance sensor measurement not yet made, the land waypoint altitude is used for terrain
+// altitude. this timer starts at the beginning of the landing glide slope.
 static constexpr hrt_abstime TERRAIN_ALT_FIRST_MEASUREMENT_TIMEOUT = 10_s;
 
 // [.] max throttle from user which will not lead to motors spinning up in altitude controlled modes
@@ -323,7 +325,9 @@ private:
 		kNudgeApproachPath
 	};
 
-	hrt_abstime _time_started_landing{0}; // [us]
+	// [us] Start time of the landing approach. If a fixed-wing landing pattern is used, this timer starts *after any
+	// orbit to altitude only when the aircraft has entered the final *straight approach.
+	hrt_abstime _time_started_landing{0};
 
 	// [m] lateral touchdown position offset manually commanded during landing
 	float _lateral_touchdown_position_offset{0.0f};
@@ -417,11 +421,18 @@ private:
 	void wind_poll();
 
 	void status_publish();
-	void landing_status_publish();
+	void landing_status_publish(const uint8_t abort_reason = position_controller_landing_status_s::kAbortReasonNone);
 	void tecs_status_publish();
 	void publishLocalPositionSetpoint(const position_setpoint_s &current_waypoint);
 
-	void abort_landing(bool abort);
+	/**
+	 * @brief Sets the aborted landing state and publishes landing status.
+	 *
+	 * @param abort If true, the aircraft should abort the landing
+	 * @param abort_reason Singular bit which triggered the abort
+	 */
+	void abort_landing(const bool abort,
+			   const uint8_t abort_reason = position_controller_landing_status_s::kAbortReasonNone);
 
 	/**
 	 * @brief Get a new waypoint based on heading and distance from current position
@@ -836,7 +847,8 @@ private:
 		(ParamFloat<px4::params::FW_LND_FL_TIME>) _param_fw_lnd_fl_time,
 		(ParamFloat<px4::params::FW_LND_FL_SINK>) _param_fw_lnd_fl_sink,
 		(ParamFloat<px4::params::FW_LND_TD_OFF>) _param_fw_lnd_td_off,
-		(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge
+		(ParamInt<px4::params::FW_LND_NUDGE>) _param_fw_lnd_nudge,
+		(ParamInt<px4::params::FW_LND_ABORT>) _param_fw_lnd_abort
 	)
 
 };

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -1044,3 +1044,24 @@ PARAM_DEFINE_FLOAT(FW_LND_TD_OFF, 3.0);
  * @group FW L1 Control
  */
 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)
+ *
+ * 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.
+ *
+ * TODO: Extend automatic abort conditions
+ * e.g. glide slope tracking error (horizontal and vertical)
+ *
+ * @min 0
+ * @max 3
+ * @bit 0 Abort if terrain is not found
+ * @bit 1 Abort if terrain times out (after a first successful measurement)
+ * @group FW L1 Control
+ */
+PARAM_DEFINE_INT32(FW_LND_ABORT, 0);