dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

fw pos ctrl: rework landing abort status 

abort boolean and reasons no longer separated, single status field with corresponding abort triggers
This commit is contained in:
Thomas Stastny 2022-07-18 10:51:11 +02:00 committed by Daniel Agar
parent 694d36050a
commit 121cc1fce8
5 changed files with 76 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
msg/position_controller_landing_status.msg
View File

@ -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

75
src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp
View File

@ -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()) {

21
src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp
View File

@ -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

4
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -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.

2
src/modules/navigator/navigator_main.cpp
View File

@ -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);
}
}
}