diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp
index 50db3a75114f23816ee3a908d135ae4c94fbedb1..880d69523d08b1cdf545319d22a50372ed321a05 100644
--- a/src/modules/commander/Commander.hpp
+++ b/src/modules/commander/Commander.hpp
@@ -87,6 +87,16 @@ public:
static bool preflight_check(bool report);
private:
+
+ DEFINE_PARAMETERS(
+ (ParamFloat<px4::params::COM_HOME_H_T>) _home_eph_threshold,
+ (ParamFloat<px4::params::COM_HOME_V_T>) _home_epv_threshold,
+
+ (ParamFloat<px4::params::COM_POS_FS_EPH>) _eph_threshold,
+ (ParamFloat<px4::params::COM_POS_FS_EPV>) _epv_threshold,
+ (ParamFloat<px4::params::COM_VEL_FS_EVH>) _evh_threshold,
+ )
+
bool handle_command(vehicle_status_s *status_local, const vehicle_command_s &cmd,
actuator_armed_s *armed_local, home_position_s *home, const vehicle_global_position_s &global_pos,
const vehicle_local_position_s &local_pos, orb_advert_t *home_pub, orb_advert_t *command_ack_pub, bool *changed);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 7fd16e27a3652c04ce053b8e9ec0ca5313f68b25..fb45c886783de80612c7b7a1f04951ad2e6f40f2 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -176,10 +176,6 @@ static uint64_t last_print_mode_reject_time = 0;
static systemlib::Hysteresis auto_disarm_hysteresis(false);
-static float eph_threshold = 5.0f; // Horizontal position error threshold (m)
-static float epv_threshold = 10.0f; // Vertivcal position error threshold (m)
-static float evh_threshold = 1.0f; // Horizontal velocity error threshold (m)
-
static hrt_abstime last_lpos_fail_time_us = 0; // Last time that the local position validity recovery check failed (usec)
static hrt_abstime last_gpos_fail_time_us = 0; // Last time that the global position validity recovery check failed (usec)
static hrt_abstime last_lvel_fail_time_us = 0; // Last time that the local velocity validity recovery check failed (usec)
@@ -1125,7 +1121,7 @@ Commander::set_home_position(orb_advert_t &homePub, home_position_s &home,
}
//Ensure that the GPS accuracy is good enough for intializing home
- if (globalPosition.eph > eph_threshold || globalPosition.epv > epv_threshold) {
+ if (globalPosition.eph > _home_eph_threshold.get() || globalPosition.epv > _home_epv_threshold.get()) {
return false;
}
@@ -1205,8 +1201,6 @@ Commander::run()
param_t _param_rc_in_off = param_find("COM_RC_IN_MODE");
param_t _param_rc_arm_hyst = param_find("COM_RC_ARM_HYST");
param_t _param_min_stick_change = param_find("COM_RC_STICK_OV");
- param_t _param_eph = param_find("COM_HOME_H_T");
- param_t _param_epv = param_find("COM_HOME_V_T");
param_t _param_geofence_action = param_find("GF_ACTION");
param_t _param_disarm_land = param_find("COM_DISARM_LAND");
param_t _param_low_bat_act = param_find("COM_LOW_BAT_ACT");
@@ -1613,10 +1607,6 @@ Commander::run()
param_get(_param_arm_mission_required, &arm_mission_required_param);
arm_requirements |= (arm_mission_required_param & (ARM_REQ_MISSION_BIT | ARM_REQ_ARM_AUTH_BIT));
- /* EPH / EPV */
- param_get(_param_eph, &eph_threshold);
- param_get(_param_epv, &epv_threshold);
-
/* flight mode slots */
param_get(_param_fmode_1, &_flight_mode_slots[0]);
param_get(_param_fmode_2, &_flight_mode_slots[1]);
@@ -1873,7 +1863,7 @@ Commander::run()
} else {
// use global position message to determine validity
- check_posvel_validity(true, global_position.eph, eph_threshold, global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed);
+ check_posvel_validity(true, global_position.eph, _eph_threshold.get(), global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, &status_changed);
}
}
}
@@ -1894,8 +1884,8 @@ Commander::run()
} else {
// use local position message to determine validity
- check_posvel_validity(local_position.xy_valid, local_position.eph, eph_threshold, local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, &status_changed);
- check_posvel_validity(local_position.v_xy_valid, local_position.evh, evh_threshold, local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, &status_changed);
+ check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold.get(), local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, &status_changed);
+ check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, &status_changed);
}
}
}
@@ -3417,9 +3407,9 @@ Commander::reset_posvel_validity(const vehicle_global_position_s &global_positio
lvel_probation_time_us = POSVEL_PROBATION_MIN;
// recheck validity
- check_posvel_validity(true, global_position.eph, eph_threshold, global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed);
- check_posvel_validity(local_position.xy_valid, local_position.eph, eph_threshold, local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
- check_posvel_validity(local_position.v_xy_valid, local_position.evh, evh_threshold, local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed);
+ check_posvel_validity(true, global_position.eph, _eph_threshold.get(), global_position.timestamp, &last_gpos_fail_time_us, &gpos_probation_time_us, &status_flags.condition_global_position_valid, changed);
+ check_posvel_validity(local_position.xy_valid, local_position.eph, _eph_threshold.get(), local_position.timestamp, &last_lpos_fail_time_us, &lpos_probation_time_us, &status_flags.condition_local_position_valid, changed);
+ check_posvel_validity(local_position.v_xy_valid, local_position.evh, _evh_threshold.get(), local_position.timestamp, &last_lvel_fail_time_us, &lvel_probation_time_us, &status_flags.condition_local_velocity_valid, changed);
}
bool
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 6398d2ab573d6dbac04fc0ad98f6407320278dec..bcc410faef61d70cc7fbf690d751da31ec5563b2 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -714,6 +714,36 @@ PARAM_DEFINE_INT32(COM_POS_FS_PROB, 30);
*/
PARAM_DEFINE_INT32(COM_POS_FS_GAIN, 10);
+/**
+ * Horizontal position error threshold.
+ *
+ * This is the horizontal position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ *
+ * @unit m
+ * @group Commander
+ */
+PARAM_DEFINE_FLOAT(COM_POS_FS_EPH, 5);
+
+/**
+ * Vertical position error threshold.
+ *
+ * This is the vertical position error (EPV) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ *
+ * @unit m
+ * @group Commander
+ */
+PARAM_DEFINE_FLOAT(COM_POS_FS_EPV, 10);
+
+/**
+ * Horizontal velocity error threshold.
+ *
+ * This is the horizontal velocity error (EVH) threshold that will trigger a failsafe. The default is appropriate for a multicopter. Can be increased for a fixed-wing.
+ *
+ * @unit m
+ * @group Commander
+ */
+PARAM_DEFINE_FLOAT(COM_VEL_FS_EVH, 1);
+
/**
* Next flight UUID
*