Skip to content

Commit d664ee5

Browse files
RomanBapstRomanBapst
committed
added mode_req_global_position_relaxed, which allows fixed wing auto modes
to only require relaxed global / local position (no accuracy requirement) Signed-off-by: RomanBapst <[email protected]>
1 parent 4c2e69b commit d664ee5

File tree

12 files changed

+95
-20
lines changed

12 files changed

+95
-20
lines changed

‎ROMFS/px4fmu_common/init.d/rc.fw_defaults

+1-1
Original file line numberDiff line numberDiff line change
@@ -14,7 +14,7 @@ param set-default MAV_TYPE 1
1414
# Default parameters for fixed wing UAVs.
1515
#
1616
# there is a 2.5 factor applied on the _FS thresholds if for invalidation
17-
param set-default COM_POS_FS_EPH 50
17+
param set-default COM_POS_R_FS_EPH 50
1818
param set-default COM_VEL_FS_EVH 3
1919

2020
param set-default COM_POS_LOW_EPH 50

‎ROMFS/px4fmu_common/init.d/rc.vtol_defaults

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@ set VEHICLE_TYPE vtol
1111
param set-default MAV_TYPE 22
1212

1313
# there is a 2.5 factor applied on COM_POS_FS_EPH if for invalidation
14-
param set-default COM_POS_FS_EPH 50
14+
param set-default COM_POS_R_FS_EPH 50
1515

1616
param set-default COM_POS_LOW_EPH 50
1717

‎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

+20-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,13 @@ 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 have a separate accuracy parameter
770+
const float pos_eph_relaxed_treshold = (_param_com_pos_relaxed_fs_eph.get() < 0) ? INFINITY :
771+
_param_com_pos_relaxed_fs_eph.get();
772+
failsafe_flags.global_position_invalid_relaxed = !checkPosVelValidity(now, global_pos_valid, gpos.eph,
773+
pos_eph_relaxed_treshold, gpos.timestamp, _last_gpos_relaxed_fail_time_us,
774+
!failsafe_flags.global_position_invalid_relaxed);
775+
761776
// Additional warning if the system is about to enter position-loss failsafe after dead-reckoning period
762777
const float eph_critical = 2.5f * lpos_eph_threshold; // threshold used to trigger the navigation failsafe
763778
const float gpos_critical_warning_thrld = math::max(0.9f * eph_critical, math::max(eph_critical - 10.f, 0.f));
@@ -772,6 +787,7 @@ void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_f
772787
|| estimator_status_flags.cs_wind_dead_reckoning;
773788

774789
if (!failsafe_flags.global_position_invalid
790+
&& failsafe_flags.mode_req_global_position
775791
&& !_nav_failure_imminent_warned
776792
&& gpos.eph > gpos_critical_warning_thrld
777793
&& dead_reckoning) {

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

+2
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)
@@ -117,6 +118,7 @@ class EstimatorChecks : public HealthAndArmingCheckBase
117118
(ParamInt<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
118119
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
119120
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
121+
(ParamFloat<px4::params::COM_POS_R_FS_EPH>) _param_com_pos_relaxed_fs_eph,
120122
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
121123
(ParamFloat<px4::params::COM_POS_LOW_EPH>) _param_com_low_eph,
122124
(ParamInt<px4::params::COM_POS_LOW_ACT>) _param_com_pos_low_act

‎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/commander_params.c

+20-6
Original file line numberDiff line numberDiff line change
@@ -513,14 +513,12 @@ PARAM_DEFINE_INT32(COM_ARM_AUTH_MET, 0);
513513
PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
514514

515515
/**
516-
* Horizontal position error threshold.
516+
* Multicopter Horizontal position error threshold
517517
*
518-
* This is the horizontal position error (EPH) threshold that will trigger a failsafe.
519-
* The default is appropriate for a multicopter. Can be increased for a fixed-wing.
520-
* If the previous position error was below this threshold, there is an additional
521-
* factor of 2.5 applied (threshold for invalidation 2.5 times the one for validation).
518+
* Horizontal position error (EPH) threshold for multicopter modes which rely on local / global position.
519+
* If mode is active, threshold is relaxed by factor 2.5.
522520
*
523-
* Set to -1 to disable.
521+
* Set -1 to disable.
524522
*
525523
* @unit m
526524
* @min -1
@@ -530,6 +528,22 @@ PARAM_DEFINE_FLOAT(COM_ARM_AUTH_TO, 1);
530528
*/
531529
PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5.f);
532530

531+
/**
532+
* Fixed-wing Horizontal position error threshold.
533+
*
534+
* Relaxed Horizontal position error (EPH) threshold for fixed-wing modes which rely on local / global position.
535+
* If mode is active, threshold is relaxed by factor 2.5.
536+
*
537+
* Set to -1 to disable.
538+
*
539+
* @unit m
540+
* @min -1
541+
* @max 400
542+
* @decimal 1
543+
* @group Commander
544+
*/
545+
PARAM_DEFINE_FLOAT(COM_POS_R_FS_EPH, 50.f);
546+
533547
/**
534548
* Horizontal velocity error threshold.
535549
*

‎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)