Skip to content

Commit

Permalink
commander: introduced global position relaxed for fixed wing vehicles
Browse files Browse the repository at this point in the history 
- allow fixed wing vehicles to indefinitely navigate if they are at least
wind dead-reckoning

Signed-off-by: RomanBapst <[email protected]>
  • Loading branch information
@RomanBapst
RomanBapst committed Feb 13, 2025
1 parent 4c2e69b commit e55c4ab
Show file tree
Hide file tree
Showing 9 changed files with 71 additions and 12 deletions.
2 changes: 2 additions & 0 deletions msg/FailsafeFlags.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ uint32 mode_req_local_alt
uint32 mode_req_local_position
uint32 mode_req_local_position_relaxed
uint32 mode_req_global_position
uint32 mode_req_global_position_relaxed
uint32 mode_req_mission
uint32 mode_req_offboard_signal
uint32 mode_req_home_position
Expand All @@ -29,6 +30,7 @@ bool local_position_invalid # Local position estimate invalid
bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
bool local_velocity_invalid # Local velocity estimate invalid
bool global_position_invalid # Global position estimate invalid
bool global_position_invalid_relaxed # Global position estimate invalid with relaxed accuracy requirements
bool auto_mission_missing # No mission available
bool offboard_control_signal_lost # Offboard signal lost
bool home_position_invalid # No home position available
Expand Down
1 change: 1 addition & 0 deletions msg/versioned/ArmingCheckReply.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,7 @@ bool mode_req_local_alt
bool mode_req_local_position
bool mode_req_local_position_relaxed
bool mode_req_global_position
bool mode_req_global_position_relaxed
bool mode_req_mission
bool mode_req_home_position
bool mode_req_prevent_arming
Expand Down
1 change: 1 addition & 0 deletions src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu
_failsafe_flags.local_position_invalid_relaxed = true;
_failsafe_flags.local_velocity_invalid = true;
_failsafe_flags.global_position_invalid = true;
_failsafe_flags.global_position_invalid_relaxed = true;
_failsafe_flags.auto_mission_missing = true;
_failsafe_flags.offboard_control_signal_lost = true;
_failsafe_flags.home_position_invalid = true;
Expand Down
23 changes: 19 additions & 4 deletions src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -693,10 +693,18 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s
void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
const vehicle_local_position_s &lpos) const
{
const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid
&& (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());

if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy
bool position_valid_but_low_accuracy = false;

if ((reporter.failsafeFlags().mode_req_global_position && !reporter.failsafeFlags().global_position_invalid) ||
(reporter.failsafeFlags().mode_req_global_position_relaxed
&& !reporter.failsafeFlags().global_position_invalid_relaxed) ||
(reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_invalid)) {

position_valid_but_low_accuracy = (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
}

if (!reporter.failsafeFlags().local_position_accuracy_low && position_valid_but_low_accuracy
&& _param_com_pos_low_act.get()) {

// only report if armed
Expand All @@ -718,7 +726,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
}
}

reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy;
reporter.failsafeFlags().local_position_accuracy_low = position_valid_but_low_accuracy;
}

void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
Expand Down Expand Up @@ -758,6 +766,12 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
!checkPosVelValidity(now, global_pos_valid, gpos.eph, lpos_eph_threshold, gpos.timestamp,
_last_gpos_fail_time_us, !failsafe_flags.global_position_invalid);

// for relaxed global condition we don't have any accuracy requirement
const float pos_eph_relaxed_treshold = INFINITY;
failsafe_flags.global_position_invalid_relaxed = !checkPosVelValidity(now, global_pos_valid, gpos.eph,
pos_eph_relaxed_treshold, gpos.timestamp, _last_gpos_relaxed_fail_time_us,
!failsafe_flags.global_position_invalid_relaxed);

// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe
const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
Expand All @@ -772,6 +786,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
|| estimator_status_flags.cs_wind_dead_reckoning;

if (!failsafe_flags.global_position_invalid
&& failsafe_flags.mode_req_global_position
&& !_nav_failure_imminent_warned
&& gpos.eph > gpos_critical_warning_thrld
&& dead_reckoning) {
Expand Down
1 change: 1 addition & 0 deletions src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -96,6 +96,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};

hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec)
hrt_abstime _last_gpos_relaxed_fail_time_us{0}; ///< Last time that the global position relaxed validity recovery check failed (usec)
hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec)
hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec)
hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec)
Expand Down
2 changes: 2 additions & 0 deletions src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,8 @@ void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
reporter.failsafeFlags().mode_req_local_position_relaxed);
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_global_position);
setOrClearRequirementBits(reply.mode_req_global_position_relaxed, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_global_position_relaxed);
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
reporter.failsafeFlags().mode_req_mission);
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,
Expand Down
15 changes: 13 additions & 2 deletions src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -86,16 +86,27 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
reporter.clearCanRunBits(local_position_modes);
}

NavModes global_position_modes = NavModes::None;

if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
global_position_modes = (NavModes)reporter.failsafeFlags().mode_req_global_position;
}

if (reporter.failsafeFlags().global_position_invalid_relaxed
&& reporter.failsafeFlags().mode_req_global_position_relaxed != 0) {
global_position_modes = global_position_modes | (NavModes)reporter.failsafeFlags().mode_req_global_position_relaxed;
}

if (global_position_modes != NavModes::None) {
/* EVENT
* @description
* The available positioning data is not sufficient to execute the selected mode.
*/
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position,
reporter.armingCheckFailure(global_position_modes,
health_component_t::global_position_estimate,
events::ID("check_modes_global_pos"),
events::Log::Error, "Navigation error: No valid global position estimate");
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
reporter.clearCanRunBits(global_position_modes);
}

if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {
Expand Down
37 changes: 31 additions & 6 deletions src/modules/commander/ModeUtil/mode_requirements.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -50,6 +50,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
flags.mode_req_local_position = 0;
flags.mode_req_local_position_relaxed = 0;
flags.mode_req_global_position = 0;
flags.mode_req_global_position_relaxed = 0;
flags.mode_req_local_alt = 0;
flags.mode_req_mission = 0;
flags.mode_req_offboard_signal = 0;
Expand Down Expand Up @@ -85,25 +86,49 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
// NAVIGATION_STATE_AUTO_MISSION
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);

if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position_relaxed);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position_relaxed);

} else {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position);
}

setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance);

// NAVIGATION_STATE_AUTO_LOITER
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);

if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position_relaxed);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position_relaxed);

} else {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
}

setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);

// NAVIGATION_STATE_AUTO_RTL
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position);

if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position_relaxed);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position_relaxed);

} else {
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position);
}

setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_alt);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position);
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming);
Expand Down
1 change: 1 addition & 0 deletions src/modules/commander/failsafe/framework.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -695,6 +695,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
(!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
(!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
(!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
(!status_flags.global_position_invalid_relaxed || ((status_flags.mode_req_global_position_relaxed & mode_mask) == 0)) &&
(!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&
Expand Down

0 comments on commit e55c4ab

Please sign in to comment.