diff options
Diffstat (limited to 'src/modules/position_estimator_inav')
3 files changed, 47 insertions, 8 deletions
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 d7503e42d..d6a44304d 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -71,6 +71,8 @@ #include "inertial_filter.h" #define MIN_VALID_W 0.00001f +#define PUB_INTERVAL 10000 // limit publish rate to 100 Hz +#define EST_BUF_SIZE 500000 / PUB_INTERVAL // buffer size is 0.5s static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -83,7 +85,6 @@ static const hrt_abstime sonar_timeout = 150000; // sonar timeout = 150ms static const hrt_abstime sonar_valid_timeout = 1000000; // estimate sonar distance during this time after sonar loss static const hrt_abstime xy_src_timeout = 2000000; // estimate position during this time after position sources loss static const uint32_t updates_counter_len = 1000000; -static const uint32_t pub_interval = 10000; // limit publish rate to 100 Hz static const float max_flow = 1.0f; // max flow value that can be used, rad/s __EXPORT int position_estimator_inav_main(int argc, char *argv[]); @@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]); static void usage(const char *reason); +static inline int min(int val1, int val2) +{ + return (val1 < val2) ? val1 : val2; +} + +static inline int max(int val1, int val2) +{ + return (val1 > val2) ? val1 : val2; +} + /** * Print the correct usage. */ @@ -199,6 +210,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) float y_est[2] = { 0.0f, 0.0f }; // pos, vel float z_est[2] = { 0.0f, 0.0f }; // pos, vel + float est_buf[EST_BUF_SIZE][3][2]; + memset(est_buf, 0, sizeof(est_buf)); + int est_buf_ptr = 0; + float eph = 1.0; float epv = 1.0; @@ -657,16 +672,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) y_est[1] = gps.vel_e_m_s; } + /* calculate index of estimated values in buffer */ + int est_i = est_buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL))); + if (est_i < 0) { + est_i += EST_BUF_SIZE; + } + /* calculate correction for position */ - corr_gps[0][0] = gps_proj[0] - x_est[0]; - corr_gps[1][0] = gps_proj[1] - y_est[0]; - corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0]; + corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0]; + corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0]; + corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0]; /* calculate correction for velocity */ if (gps.vel_ned_valid) { - corr_gps[0][1] = gps.vel_n_m_s - x_est[1]; - corr_gps[1][1] = gps.vel_e_m_s - y_est[1]; - corr_gps[2][1] = gps.vel_d_m_s - z_est[1]; + corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1]; + corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1]; + corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1]; } else { corr_gps[0][1] = 0.0f; @@ -910,8 +931,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } } - if (t > pub_last + pub_interval) { + if (t > pub_last + PUB_INTERVAL) { pub_last = t; + + /* push current estimate to FIFO buffer */ + est_buf[est_buf_ptr][0][0] = x_est[0]; + est_buf[est_buf_ptr][0][1] = x_est[1]; + est_buf[est_buf_ptr][1][0] = y_est[0]; + est_buf[est_buf_ptr][1][1] = y_est[1]; + est_buf[est_buf_ptr][2][0] = z_est[0]; + est_buf[est_buf_ptr][2][1] = z_est[1]; + est_buf_ptr++; + if (est_buf_ptr >= EST_BUF_SIZE) { + est_buf_ptr = 0; + } + /* publish local position */ local_pos.xy_valid = can_estimate_xy; local_pos.v_xy_valid = can_estimate_xy; 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 8aa08b6f2..8509d15cb 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,6 +55,7 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f); PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f); PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f); PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f); +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.1f); int parameters_init(struct position_estimator_inav_param_handles *h) { @@ -73,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h) h->land_t = param_find("INAV_LAND_T"); h->land_disp = param_find("INAV_LAND_DISP"); h->land_thr = param_find("INAV_LAND_THR"); + h->delay_gps = param_find("INAV_DELAY_GPS"); return OK; } @@ -94,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str param_get(h->land_t, &(p->land_t)); param_get(h->land_disp, &(p->land_disp)); param_get(h->land_thr, &(p->land_thr)); + param_get(h->delay_gps, &(p->delay_gps)); return OK; } diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h index 08ec996a1..df1cfaa71 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.h +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h @@ -56,6 +56,7 @@ struct position_estimator_inav_params { float land_t; float land_disp; float land_thr; + float delay_gps; }; struct position_estimator_inav_param_handles { @@ -74,6 +75,7 @@ struct position_estimator_inav_param_handles { param_t land_t; param_t land_disp; param_t land_thr; + param_t delay_gps; }; /** |