fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-30 14:04:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Differentiate between EPH and EPV. Do not declare position invalid because of EPV (because we use the baro anyway). No fundamental logic change / cleanup to ensure current approach and arming logic remains intact

Browse Source
This commit is contained in:
Lorenz Meier 2014-07-29 17:50:52 +02:00
parent 7f2799a78b
commit 0cbd943d09
1 changed files with 15 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

29
src/modules/commander/commander.cpp
View File

@ -163,7 +163,8 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static float eph_epv_threshold = 5.0f;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
static struct vehicle_status_s status;
static struct actuator_armed_s armed;
@ -1108,32 +1109,32 @@ int commander_thread_main(int argc, char *argv[])
/* update condition_global_position_valid */
/* hysteresis for EPH/EPV */
bool eph_epv_good;
bool eph_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.5f || global_position.epv > eph_epv_threshold * 2.5f) {
eph_epv_good = false;
if (global_position.eph > eph_threshold * 2.5f) {
eph_good = false;
} else {
eph_epv_good = true;
eph_good = true;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
if (global_position.eph < eph_threshold) {
eph_good = true;
} else {
eph_epv_good = false;
eph_good = false;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */
/* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
home.lat = global_position.lat;
home.lon = global_position.lon;
@ -1163,8 +1164,8 @@ int commander_thread_main(int argc, char *argv[])
/* hysteresis for EPH */
bool local_eph_good;
if (status.condition_global_position_valid) {
if (local_position.eph > eph_epv_threshold * 2.0f) {
if (status.condition_local_position_valid) {
if (local_position.eph > eph_threshold * 2.5f) {
local_eph_good = false;
} else {
@ -1172,7 +1173,7 @@ int commander_thread_main(int argc, char *argv[])
}
} else {
if (local_position.eph < eph_epv_threshold) {
if (local_position.eph < eph_threshold) {
local_eph_good = true;
} else {
@ -1504,7 +1505,7 @@ int commander_thread_main(int argc, char *argv[])
/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
(global_position.eph < eph_threshold) && (global_position.epv < epv_threshold)) {
// TODO remove code duplication
home.lat = global_position.lat;