aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/commander/commander.cpp23
-rw-r--r--src/modules/position_estimator_inav/position_estimator_inav_main.c49
2 files changed, 53 insertions, 19 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 17c085652..2f373c6db 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -153,6 +153,7 @@ 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 struct vehicle_status_s status;
static struct actuator_armed_s armed;
@@ -965,15 +966,25 @@ int commander_thread_main(int argc, char *argv[])
}
/* 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 */
- static float hdop_threshold_m = 4.0f;
- static float vdop_threshold_m = 8.0f;
/* update home position */
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.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 */
- 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);
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 */
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
home.lat = global_position.lat;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 40f7069ca..dc85f7482 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -168,13 +168,13 @@ int position_estimator_inav_main(int argc, char *argv[])
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");
if (f) {
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);
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);
@@ -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 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_num = 200;
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 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;
hrt_abstime flow_prev = 0; // time of last flow measurement
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 */
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;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} 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;
reset_est = true;
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;
}
- w_gps_xy = 2.0f / fmaxf(2.0f, gps.eph_m);
- w_gps_z = 4.0f / fmaxf(4.0f, gps.epv_m);
+ w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
+ w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
}
} else {
@@ -782,23 +790,34 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* inertial filter prediction for altitude */
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_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_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));
- memcpy(y_est_prev, y_est, sizeof(y_est));
+ } else {
+ memcpy(z_est_prev, z_est, sizeof(z_est));
+ }
if (can_estimate_xy) {
/* inertial filter prediction for position */
inertial_filter_predict(dt, x_est);
inertial_filter_predict(dt, y_est);
- if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
- 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);
+ 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, 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(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])) {
- 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);
+ 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, 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(y_est, y_est_prev, sizeof(y_est));
memset(corr_acc, 0, sizeof(corr_acc));
memset(corr_gps, 0, sizeof(corr_gps));
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));
}
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-06 17:48:50 +0000