Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 9 Packages Projects Releases Wiki Activity

commander add parameters for eph, epv, evh requirements

Browse Source
This commit is contained in:
Daniel Agar
2018-01-28 13:19:59 -05:00
parent 255cc12c48
commit b70e7433dc
3 changed files with 47 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.hpp
+10
View File
@@ -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);
src/modules/commander/commander.cpp
+7 -17
View File
@@ -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
src/modules/commander/commander_params.c
+30
View File
@@ -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
*