Skip to content

Commit e55c4ab

Browse files
RomanBapstRomanBapst
committed
commander: introduced global position relaxed for fixed wing vehicles
- allow fixed wing vehicles to indefinitely navigate if they are at least wind dead-reckoning Signed-off-by: RomanBapst <[email protected]>
1 parent 4c2e69b commit e55c4ab

File tree

9 files changed

+71
-12
lines changed

9 files changed

+71
-12
lines changed

‎msg/FailsafeFlags.msg

+2
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@ uint32 mode_req_local_alt
1212
uint32 mode_req_local_position
1313
uint32 mode_req_local_position_relaxed
1414
uint32 mode_req_global_position
15+
uint32 mode_req_global_position_relaxed
1516
uint32 mode_req_mission
1617
uint32 mode_req_offboard_signal
1718
uint32 mode_req_home_position
@@ -29,6 +30,7 @@ bool local_position_invalid # Local position estimate invalid
2930
bool local_position_invalid_relaxed # Local position with reduced accuracy requirements invalid (e.g. flying with optical flow)
3031
bool local_velocity_invalid # Local velocity estimate invalid
3132
bool global_position_invalid # Global position estimate invalid
33+
bool global_position_invalid_relaxed # Global position estimate invalid with relaxed accuracy requirements
3234
bool auto_mission_missing # No mission available
3335
bool offboard_control_signal_lost # Offboard signal lost
3436
bool home_position_invalid # No home position available

‎msg/versioned/ArmingCheckReply.msg

+1
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ bool mode_req_local_alt
2626
bool mode_req_local_position
2727
bool mode_req_local_position_relaxed
2828
bool mode_req_global_position
29+
bool mode_req_global_position_relaxed
2930
bool mode_req_mission
3031
bool mode_req_home_position
3132
bool mode_req_prevent_arming

‎src/modules/commander/HealthAndArmingChecks/HealthAndArmingChecks.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -45,6 +45,7 @@ HealthAndArmingChecks::HealthAndArmingChecks(ModuleParams *parent, vehicle_statu
4545
_failsafe_flags.local_position_invalid_relaxed = true;
4646
_failsafe_flags.local_velocity_invalid = true;
4747
_failsafe_flags.global_position_invalid = true;
48+
_failsafe_flags.global_position_invalid_relaxed = true;
4849
_failsafe_flags.auto_mission_missing = true;
4950
_failsafe_flags.offboard_control_signal_lost = true;
5051
_failsafe_flags.home_position_invalid = true;

‎src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.cpp

+19-4
Original file line numberDiff line numberDiff line change
@@ -693,10 +693,18 @@ void EstimatorChecks::checkGps(const Context &context, Report &reporter, const s
693693
void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &reporter,
694694
const vehicle_local_position_s &lpos) const
695695
{
696-
const bool local_position_valid_but_low_accuracy = !reporter.failsafeFlags().local_position_invalid
697-
&& (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
698696

699-
if (!reporter.failsafeFlags().local_position_accuracy_low && local_position_valid_but_low_accuracy
697+
bool position_valid_but_low_accuracy = false;
698+
699+
if ((reporter.failsafeFlags().mode_req_global_position && !reporter.failsafeFlags().global_position_invalid) ||
700+
(reporter.failsafeFlags().mode_req_global_position_relaxed
701+
&& !reporter.failsafeFlags().global_position_invalid_relaxed) ||
702+
(reporter.failsafeFlags().mode_req_local_position && !reporter.failsafeFlags().local_position_invalid)) {
703+
704+
position_valid_but_low_accuracy = (_param_com_low_eph.get() > FLT_EPSILON && lpos.eph > _param_com_low_eph.get());
705+
}
706+
707+
if (!reporter.failsafeFlags().local_position_accuracy_low && position_valid_but_low_accuracy
700708
&& _param_com_pos_low_act.get()) {
701709

702710
// only report if armed
@@ -718,7 +726,7 @@ void EstimatorChecks::lowPositionAccuracy(const Context &context, Report &report
718726
}
719727
}
720728

721-
reporter.failsafeFlags().local_position_accuracy_low = local_position_valid_but_low_accuracy;
729+
reporter.failsafeFlags().local_position_accuracy_low = position_valid_but_low_accuracy;
722730
}
723731

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

769+
// for relaxed global condition we don't have any accuracy requirement
770+
const float pos_eph_relaxed_treshold = INFINITY;
771+
failsafe_flags.global_position_invalid_relaxed = !checkPosVelValidity(now, global_pos_valid, gpos.eph,
772+
pos_eph_relaxed_treshold, gpos.timestamp, _last_gpos_relaxed_fail_time_us,
773+
!failsafe_flags.global_position_invalid_relaxed);
774+
761775
// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
762776
const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe
763777
const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
@@ -772,6 +786,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
772786
|| estimator_status_flags.cs_wind_dead_reckoning;
773787

774788
if (!failsafe_flags.global_position_invalid
789+
&& failsafe_flags.mode_req_global_position
775790
&& !_nav_failure_imminent_warned
776791
&& gpos.eph > gpos_critical_warning_thrld
777792
&& dead_reckoning) {

‎src/modules/commander/HealthAndArmingChecks/checks/estimatorCheck.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -96,6 +96,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
9696
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
9797

9898
hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec)
99+
hrt_abstime _last_gpos_relaxed_fail_time_us{0}; ///< Last time that the global position relaxed validity recovery check failed (usec)
99100
hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec)
100101
hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec)
101102
hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec)

‎src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp

+2
Original file line numberDiff line numberDiff line change
@@ -173,6 +173,8 @@ void ExternalChecks::checkAndReport(const Context &context, Report &reporter)
173173
reporter.failsafeFlags().mode_req_local_position_relaxed);
174174
setOrClearRequirementBits(reply.mode_req_global_position, nav_mode_id, replaces_nav_state,
175175
reporter.failsafeFlags().mode_req_global_position);
176+
setOrClearRequirementBits(reply.mode_req_global_position_relaxed, nav_mode_id, replaces_nav_state,
177+
reporter.failsafeFlags().mode_req_global_position_relaxed);
176178
setOrClearRequirementBits(reply.mode_req_mission, nav_mode_id, replaces_nav_state,
177179
reporter.failsafeFlags().mode_req_mission);
178180
setOrClearRequirementBits(reply.mode_req_home_position, nav_mode_id, replaces_nav_state,

‎src/modules/commander/HealthAndArmingChecks/checks/modeCheck.cpp

+13-2
Original file line numberDiff line numberDiff line change
@@ -86,16 +86,27 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
8686
reporter.clearCanRunBits(local_position_modes);
8787
}
8888

89+
NavModes global_position_modes = NavModes::None;
90+
8991
if (reporter.failsafeFlags().global_position_invalid && reporter.failsafeFlags().mode_req_global_position != 0) {
92+
global_position_modes = (NavModes)reporter.failsafeFlags().mode_req_global_position;
93+
}
94+
95+
if (reporter.failsafeFlags().global_position_invalid_relaxed
96+
&& reporter.failsafeFlags().mode_req_global_position_relaxed != 0) {
97+
global_position_modes = global_position_modes | (NavModes)reporter.failsafeFlags().mode_req_global_position_relaxed;
98+
}
99+
100+
if (global_position_modes != NavModes::None) {
90101
/* EVENT
91102
* @description
92103
* The available positioning data is not sufficient to execute the selected mode.
93104
*/
94-
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_global_position,
105+
reporter.armingCheckFailure(global_position_modes,
95106
health_component_t::global_position_estimate,
96107
events::ID("check_modes_global_pos"),
97108
events::Log::Error, "Navigation error: No valid global position estimate");
98-
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_global_position);
109+
reporter.clearCanRunBits(global_position_modes);
99110
}
100111

101112
if (reporter.failsafeFlags().local_altitude_invalid && reporter.failsafeFlags().mode_req_local_alt != 0) {

‎src/modules/commander/ModeUtil/mode_requirements.cpp

+31-6
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,7 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
5050
flags.mode_req_local_position = 0;
5151
flags.mode_req_local_position_relaxed = 0;
5252
flags.mode_req_global_position = 0;
53+
flags.mode_req_global_position_relaxed = 0;
5354
flags.mode_req_local_alt = 0;
5455
flags.mode_req_mission = 0;
5556
flags.mode_req_offboard_signal = 0;
@@ -85,25 +86,49 @@ void getModeRequirements(uint8_t vehicle_type, failsafe_flags_s &flags)
8586
// NAVIGATION_STATE_AUTO_MISSION
8687
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_angular_velocity);
8788
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_attitude);
88-
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position);
89-
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
89+
90+
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
91+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position_relaxed);
92+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position_relaxed);
93+
94+
} else {
95+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_global_position);
96+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_position);
97+
}
98+
9099
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_local_alt);
91100
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_mission);
92101
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION, flags.mode_req_wind_and_flight_time_compliance);
93102

94103
// NAVIGATION_STATE_AUTO_LOITER
95104
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_angular_velocity);
96105
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_attitude);
97-
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
98-
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
106+
107+
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
108+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position_relaxed);
109+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position_relaxed);
110+
111+
} else {
112+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_global_position);
113+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_position);
114+
}
115+
99116
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_local_alt);
100117
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER, flags.mode_req_wind_and_flight_time_compliance);
101118

102119
// NAVIGATION_STATE_AUTO_RTL
103120
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_angular_velocity);
104121
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_attitude);
105-
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position);
106-
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position);
122+
123+
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
124+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position_relaxed);
125+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position_relaxed);
126+
127+
} else {
128+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_global_position);
129+
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_position);
130+
}
131+
107132
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_local_alt);
108133
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_home_position);
109134
setRequirement(vehicle_status_s::NAVIGATION_STATE_AUTO_RTL, flags.mode_req_prevent_arming);

‎src/modules/commander/failsafe/framework.cpp

+1
Original file line numberDiff line numberDiff line change
@@ -695,6 +695,7 @@ bool FailsafeBase::modeCanRun(const failsafe_flags_s &status_flags, uint8_t mode
695695
(!status_flags.local_position_invalid || ((status_flags.mode_req_local_position & mode_mask) == 0)) &&
696696
(!status_flags.local_position_invalid_relaxed || ((status_flags.mode_req_local_position_relaxed & mode_mask) == 0)) &&
697697
(!status_flags.global_position_invalid || ((status_flags.mode_req_global_position & mode_mask) == 0)) &&
698+
(!status_flags.global_position_invalid_relaxed || ((status_flags.mode_req_global_position_relaxed & mode_mask) == 0)) &&
698699
(!status_flags.local_altitude_invalid || ((status_flags.mode_req_local_alt & mode_mask) == 0)) &&
699700
(!status_flags.auto_mission_missing || ((status_flags.mode_req_mission & mode_mask) == 0)) &&
700701
(!status_flags.offboard_control_signal_lost || ((status_flags.mode_req_offboard_signal & mode_mask) == 0)) &&

0 commit comments

Comments
 (0)