diff options
Diffstat (limited to 'src/modules/position_estimator_inav')
3 files changed, 40 insertions, 8 deletions
diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk index 0658d3f09..45c876299 100644 --- a/src/modules/position_estimator_inav/module.mk +++ b/src/modules/position_estimator_inav/module.mk @@ -41,3 +41,6 @@ SRCS = position_estimator_inav_main.c \ inertial_filter.c MODULE_STACKSIZE = 1200 + +EXTRACFLAGS = -Wframe-larger-than=3500 + 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 4bdb4d539..6fb87ae7e 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -152,7 +152,7 @@ int position_estimator_inav_main(int argc, char *argv[]) position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, - (argv) ? (const char **) &argv[2] : (const char **) NULL); + (argv) ? (char * const *) &argv[2] : (char * const *) NULL); exit(0); } @@ -234,8 +234,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float eph_flow = 1.0f; - float eph_vision = 0.5f; - float epv_vision = 0.5f; + float eph_vision = 0.2f; + float epv_vision = 0.2f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); @@ -641,6 +641,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) if (updated) { orb_copy(ORB_ID(vision_position_estimate), vision_position_estimate_sub, &vision); + static float last_vision_x = 0.0f; + static float last_vision_y = 0.0f; + static float last_vision_z = 0.0f; + /* reset position estimate on first vision update */ if (!vision_valid) { x_est[0] = vision.x; @@ -655,6 +659,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } vision_valid = true; + + last_vision_x = vision.x; + last_vision_y = vision.y; + last_vision_z = vision.z; + warnx("VISION estimate valid"); mavlink_log_info(mavlink_fd, "[inav] VISION estimate valid"); } @@ -664,10 +673,30 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_vision[1][0] = vision.y - y_est[0]; corr_vision[2][0] = vision.z - z_est[0]; - /* calculate correction for velocity */ - corr_vision[0][1] = vision.vx - x_est[1]; - corr_vision[1][1] = vision.vy - y_est[1]; - corr_vision[2][1] = vision.vz - z_est[1]; + static hrt_abstime last_vision_time = 0; + + float vision_dt = (vision.timestamp_boot - last_vision_time) / 1e6f; + last_vision_time = vision.timestamp_boot; + + if (vision_dt > 0.000001f && vision_dt < 0.2f) { + vision.vx = (vision.x - last_vision_x) / vision_dt; + vision.vy = (vision.y - last_vision_y) / vision_dt; + vision.vz = (vision.z - last_vision_z) / vision_dt; + + last_vision_x = vision.x; + last_vision_y = vision.y; + last_vision_z = vision.z; + + /* calculate correction for velocity */ + corr_vision[0][1] = vision.vx - x_est[1]; + corr_vision[1][1] = vision.vy - y_est[1]; + corr_vision[2][1] = vision.vz - z_est[1]; + } else { + /* assume zero motion */ + corr_vision[0][1] = 0.0f - x_est[1]; + corr_vision[1][1] = 0.0f - y_est[1]; + corr_vision[2][1] = 0.0f - z_est[1]; + } } } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c index cc0fb4155..b7f6509d7 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -116,7 +116,7 @@ PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f); * @max 10.0 * @group Position Estimator INAV */ -PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 5.0f); +PARAM_DEFINE_FLOAT(INAV_W_XY_VIS_P, 7.0f); /** * XY axis weight for vision velocity |