fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-28 03:50:34 +08:00
Code Issues 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.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