diff --git a/msg/position_controller_landing_status.msg b/msg/position_controller_landing_status.msg index 393e67f5ab..e98ba16350 100644 --- a/msg/position_controller_landing_status.msg +++ b/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 diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 05fda4a900..e3eb839bcd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/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 ¤t_waypoint) { if (_global_local_proj_ref.isInitialized()) { diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index a37bb1ef62..cf4c9c9e95 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/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 ¤t_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 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 e4a245ebde..955b492620 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 @@ -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. diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 24b60c0b16..225c52d004 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/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); } } }