dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

position_estimator_inav: GPS delay compensation

This commit is contained in:
Anton Babushkin 2014-05-29 22:42:33 +02:00
parent 58c9d7a723
commit ead91f3259
3 changed files with 47 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@ -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;

3
src/modules/position_estimator_inav/position_estimator_inav_params.c
View File

@ -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;
}

2
src/modules/position_estimator_inav/position_estimator_inav_params.h
View File

@ -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;
};
/**