diff --git a/msg/position_controller_landing_status.msg b/msg/position_controller_landing_status.msg index 71f06a6117..393e67f5ab 100644 --- a/msg/position_controller_landing_status.msg +++ b/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) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 84e6f1ecb1..e9dda181dd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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 - events::send(events::ID("fixedwing_position_control_land_aborted"), events::Log::Critical, "Landing aborted"); + + switch (abort_reason) { + 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; - terrain_alt = (_local_pos.ref_alt - terrain_vpos); - _last_valid_terrain_alt_estimate = terrain_alt; + + const float terrain_estimate = _local_pos.ref_alt + -_local_pos.z - _local_pos.dist_bottom; + _last_valid_terrain_alt_estimate = terrain_estimate; _last_time_terrain_alt_was_valid = now; - } else if (_last_time_terrain_alt_was_valid == 0) { - // 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; + return terrain_estimate; + } - } else { - // still no valid terrain, abort landing - terrain_alt = land_point_altitude; - abort_landing(true); + 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; + + 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) - || _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; + return land_point_altitude; + } - } else { - // terrain alt was not valid for long time, abort landing - terrain_alt = _last_valid_terrain_alt_estimate; - abort_landing(true); + 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; + + 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 ¤t_waypoint) diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 82780799db..153b7a473d 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/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 ¤t_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) _param_fw_lnd_fl_time, (ParamFloat) _param_fw_lnd_fl_sink, (ParamFloat) _param_fw_lnd_td_off, - (ParamInt) _param_fw_lnd_nudge + (ParamInt) _param_fw_lnd_nudge, + (ParamInt) _param_fw_lnd_abort ) }; diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9c845e29c1..43e409f028 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/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);