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

position_estimator_inav: increase acceptable EPH/EPV, in commander use EPH/EPV to decide if global position valid

Browse Source
This commit is contained in:
Anton Babushkin
2014-04-07 17:16:43 +04:00
parent 79d2247b44
commit b770c9fc1e
2 changed files with 53 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/commander.cpp
+17 -6
View File
@@ -153,6 +153,7 @@ static bool on_usb_power = false;
static float takeoff_alt = 5.0f; static float takeoff_alt = 5.0f;
static int parachute_enabled = 0; static int parachute_enabled = 0;
static float eph_epv_threshold = 5.0f;
static struct vehicle_status_s status; static struct vehicle_status_s status;
static struct actuator_armed_s armed; static struct actuator_armed_s armed;
@@ -965,15 +966,25 @@ int commander_thread_main(int argc, char *argv[])
} }
/* update condition_global_position_valid */ /* update condition_global_position_valid */
check_valid(global_position.timestamp, POSITION_TIMEOUT, true, &(status.condition_global_position_valid), &status_changed); /* hysteresis for EPH/EPV*/
bool eph_epv_good;
if (status.condition_global_position_valid) {
if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
eph_epv_good = false;
}
} else {
if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
eph_epv_good = true;
}
}
check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);
/* check if GPS fix is ok */ /* check if GPS fix is ok */
static float hdop_threshold_m = 4.0f;
static float vdop_threshold_m = 8.0f;
/* update home position */ /* update home position */
if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed && if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
(global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
home.lat = global_position.lat; home.lat = global_position.lat;
home.lon = global_position.lon; home.lon = global_position.lon;
@@ -1004,7 +1015,7 @@ int commander_thread_main(int argc, char *argv[])
} }
/* update condition_local_position_valid and condition_local_altitude_valid */ /* update condition_local_position_valid and condition_local_altitude_valid */
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid, &(status.condition_local_position_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed); check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
static bool published_condition_landed_fw = false; static bool published_condition_landed_fw = false;
@@ -1322,7 +1333,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 */ /* 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 && if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
(global_position.eph < hdop_threshold_m) && (global_position.epv < vdop_threshold_m)) { (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
// TODO remove code duplication // TODO remove code duplication
home.lat = global_position.lat; home.lat = global_position.lat;
src/modules/position_estimator_inav/position_estimator_inav_main.c
+36 -13
View File
@@ -168,13 +168,13 @@ int position_estimator_inav_main(int argc, char *argv[])
exit(1); exit(1);
} }
void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v) void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
{ {
FILE *f = fopen("/fs/microsd/inav.log", "a"); FILE *f = fopen("/fs/microsd/inav.log", "a");
if (f) { if (f) {
char *s = malloc(256); char *s = malloc(256);
unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]); unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v); n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
fwrite(s, 1, n, f); fwrite(s, 1, n, f);
@@ -199,6 +199,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float y_est[3] = { 0.0f, 0.0f, 0.0f }; float y_est[3] = { 0.0f, 0.0f, 0.0f };
float z_est[3] = { 0.0f, 0.0f, 0.0f }; float z_est[3] = { 0.0f, 0.0f, 0.0f };
float x_est_prev[3], y_est_prev[3], z_est_prev[3];
memset(x_est_prev, 0, sizeof(x_est_prev));
memset(y_est_prev, 0, sizeof(y_est_prev));
memset(z_est_prev, 0, sizeof(z_est_prev));
int baro_init_cnt = 0; int baro_init_cnt = 0;
int baro_init_num = 200; int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
@@ -249,6 +254,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float corr_flow[] = { 0.0f, 0.0f }; // N E float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f; float w_flow = 0.0f;
static float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation
static float max_eph_epv = 10.0f; // max EPH/EPV acceptable for estimation
float sonar_prev = 0.0f; float sonar_prev = 0.0f;
hrt_abstime flow_prev = 0; // time of last flow measurement hrt_abstime flow_prev = 0; // time of last flow measurement
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered) hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
@@ -584,13 +592,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* hysteresis for GPS quality */ /* hysteresis for GPS quality */
if (gps_valid) { if (gps_valid) {
if (gps.eph_m > 10.0f || gps.epv_m > 20.0f || gps.fix_type < 3) { if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
gps_valid = false; gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
} }
} else { } else {
if (gps.eph_m < 5.0f && gps.epv_m < 10.0f && gps.fix_type >= 3) { if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
gps_valid = true; gps_valid = true;
reset_est = true; reset_est = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -665,8 +673,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
corr_gps[2][1] = 0.0f; corr_gps[2][1] = 0.0f;
} }
w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m); w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m); w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
} }
} else { } else {
@@ -782,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter prediction for altitude */ /* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est); inertial_filter_predict(dt, z_est);
if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
}
/* inertial filter correction for altitude */ /* inertial filter correction for altitude */
inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro); inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc); inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
float x_est_prev[3], y_est_prev[3]; if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps));
corr_baro = 0;
memcpy(x_est_prev, x_est, sizeof(x_est)); } else {
memcpy(y_est_prev, y_est, sizeof(y_est)); memcpy(z_est_prev, z_est, sizeof(z_est));
}
if (can_estimate_xy) { if (can_estimate_xy) {
/* inertial filter prediction for position */ /* inertial filter prediction for position */
inertial_filter_predict(dt, x_est); inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est); inertial_filter_predict(dt, y_est);
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est)); memcpy(y_est, y_est_prev, sizeof(y_est));
} }
@@ -822,13 +841,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} }
} }
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) { if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v); write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
memcpy(x_est, x_est_prev, sizeof(x_est)); memcpy(x_est, x_est_prev, sizeof(x_est));
memcpy(y_est, y_est_prev, sizeof(y_est)); memcpy(y_est, y_est_prev, sizeof(y_est));
memset(corr_acc, 0, sizeof(corr_acc)); memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps)); memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_flow, 0, sizeof(corr_flow)); memset(corr_flow, 0, sizeof(corr_flow));
} else {
memcpy(x_est_prev, x_est, sizeof(x_est));
memcpy(y_est_prev, y_est, sizeof(y_est));
} }
} }