From ead91f325989cda312424ec3b2cc7fd11913f5c8 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 29 May 2014 22:42:33 +0200 Subject: [PATCH 1/7] position_estimator_inav: GPS delay compensation --- .../position_estimator_inav_main.c | 50 ++++++++++++++++--- .../position_estimator_inav_params.c | 3 ++ .../position_estimator_inav_params.h | 2 + 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 d7503e42da..d6a44304d2 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 8aa08b6f2c..8509d15cba 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 08ec996a13..df1cfaa710 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; }; /** From fdd5d7b8b8835dd3e2655cd104f7440c9f56ba27 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 11:03:06 +0200 Subject: [PATCH 2/7] position_estimator_inav: add buffer for rotation matrix to do accel bias correction properly --- .../position_estimator_inav_main.c | 61 ++++++++++++++----- 1 file changed, 45 insertions(+), 16 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 d6a44304d2..aa533c777f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -72,7 +72,7 @@ #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 +#define EST_BUF_SIZE 250000 / PUB_INTERVAL // buffer size is 0.5s static bool thread_should_exit = false; /**< Deamon exit flag */ static bool thread_running = false; /**< Deamon status flag */ @@ -146,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[]) thread_should_exit = false; position_estimator_inav_task = task_spawn_cmd("position_estimator_inav", - SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000, + SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000, position_estimator_inav_thread_main, (argv) ? (const char **) &argv[2] : (const char **) NULL); exit(0); @@ -210,9 +210,13 @@ 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]; + float est_buf[EST_BUF_SIZE][3][2]; // estimated position buffer + float R_buf[EST_BUF_SIZE][3][3]; // rotation matrix buffer + float R_gps[3][3]; // rotation matrix for GPS correction moment memset(est_buf, 0, sizeof(est_buf)); - int est_buf_ptr = 0; + memset(R_buf, 0, sizeof(R_buf)); + memset(R_gps, 0, sizeof(R_gps)); + int buf_ptr = 0; float eph = 1.0; float epv = 1.0; @@ -673,7 +677,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) } /* 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))); + int est_i = 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; } @@ -695,6 +699,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_gps[2][1] = 0.0f; } + /* save rotation matrix at this moment */ + memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); + eph = fminf(eph, gps.eph_m); epv = fminf(epv, gps.epv_m); @@ -784,7 +791,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) corr_baro += offs_corr; } - /* accelerometer bias correction */ + /* accelerometer bias correction for GPS (use buffered rotation matrix) */ float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f }; if (use_gps_xy) { @@ -798,6 +805,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p; } + /* transform error vector from NED frame to body frame */ + for (int i = 0; i < 3; i++) { + float c = 0.0f; + + for (int j = 0; j < 3; j++) { + c += R_gps[j][i] * accel_bias_corr[j]; + } + + if (isfinite(c)) { + acc_bias[i] += c * params.w_acc_bias * dt; + } + } + + /* accelerometer bias correction for flow and baro (assume that there is no delay) */ + accel_bias_corr[0] = 0.0f; + accel_bias_corr[1] = 0.0f; + accel_bias_corr[2] = 0.0f; + if (use_flow) { accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow; accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow; @@ -934,16 +959,20 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) 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; + /* push current estimate to buffer */ + est_buf[buf_ptr][0][0] = x_est[0]; + est_buf[buf_ptr][0][1] = x_est[1]; + est_buf[buf_ptr][1][0] = y_est[0]; + est_buf[buf_ptr][1][1] = y_est[1]; + est_buf[buf_ptr][2][0] = z_est[0]; + est_buf[buf_ptr][2][1] = z_est[1]; + + /* push current rotation matrix to buffer */ + memcpy(R_buf[buf_ptr], att.R, sizeof(att.R)); + + buf_ptr++; + if (buf_ptr >= EST_BUF_SIZE) { + buf_ptr = 0; } /* publish local position */ From 0bdde089243bbf7e3651f9535f5c847f894e0ac7 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:18:03 +0200 Subject: [PATCH 3/7] position_estimator_inav: default GPS delay changed to 0.2s --- .../position_estimator_inav/position_estimator_inav_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8509d15cba..d88419c258 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_params.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c @@ -55,7 +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); +PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f); int parameters_init(struct position_estimator_inav_param_handles *h) { From 47c9d326209034dc373ab54647d29df4e63b5285 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:23:26 +0200 Subject: [PATCH 4/7] ubx: disable all debug messages --- src/drivers/gps/ubx.cpp | 170 ++++++++++++++++++++-------------------- 1 file changed, 85 insertions(+), 85 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 7502809bd7..cbfb61d00a 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -219,19 +219,19 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: NAV SVINFO"); - return 1; - } - - configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); - - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("MSG CFG FAIL: MON HW"); - return 1; - } +// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); +// +// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { +// warnx("MSG CFG FAIL: NAV SVINFO"); +// return 1; +// } +// +// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); +// +// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { +// warnx("MSG CFG FAIL: MON HW"); +// return 1; +// } _configured = true; return 0; @@ -486,61 +486,61 @@ UBX::handle_message() break; } - case UBX_MESSAGE_NAV_SVINFO: { - //printf("GOT NAV_SVINFO\n"); - const int length_part1 = 8; - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - - uint8_t satellites_used = 0; - int i; - - //printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { - /* set pointer to sattelite_i information */ - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); - - /* write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - /* count SVs used for NAV */ - satellites_used++; - } - - /* record info for all channels, whether or not the SV is used for NAV */ - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); - } - - for (i = packet_part1->numCh; i < 20; i++) { - /* unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - - } else { - _gps_position->satellite_info_available = false; - } - - _gps_position->timestamp_satellites = hrt_absolute_time(); - - ret = 1; - break; - } +// case UBX_MESSAGE_NAV_SVINFO: { +// //printf("GOT NAV_SVINFO\n"); +// const int length_part1 = 8; +// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; +// const int length_part2 = 12; +// gps_bin_nav_svinfo_part2_packet_t *packet_part2; +// +// uint8_t satellites_used = 0; +// int i; +// +// //printf("Number of Channels: %d\n", packet_part1->numCh); +// for (i = 0; i < packet_part1->numCh; i++) { +// /* set pointer to sattelite_i information */ +// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); +// +// /* write satellite information to global storage */ +// uint8_t sv_used = packet_part2->flags & 0x01; +// +// if (sv_used) { +// /* count SVs used for NAV */ +// satellites_used++; +// } +// +// /* record info for all channels, whether or not the SV is used for NAV */ +// _gps_position->satellite_used[i] = sv_used; +// _gps_position->satellite_snr[i] = packet_part2->cno; +// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); +// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); +// _gps_position->satellite_prn[i] = packet_part2->svid; +// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); +// } +// +// for (i = packet_part1->numCh; i < 20; i++) { +// /* unused channels have to be set to zero for e.g. MAVLink */ +// _gps_position->satellite_prn[i] = 0; +// _gps_position->satellite_used[i] = 0; +// _gps_position->satellite_snr[i] = 0; +// _gps_position->satellite_elevation[i] = 0; +// _gps_position->satellite_azimuth[i] = 0; +// } +// +// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones +// +// if (packet_part1->numCh > 0) { +// _gps_position->satellite_info_available = true; +// +// } else { +// _gps_position->satellite_info_available = false; +// } +// +// _gps_position->timestamp_satellites = hrt_absolute_time(); +// +// ret = 1; +// break; +// } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -573,23 +573,23 @@ UBX::handle_message() break; } - case UBX_CLASS_MON: { - switch (_message_id) { - case UBX_MESSAGE_MON_HW: { - - struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; - - _gps_position->noise_per_ms = p->noisePerMS; - _gps_position->jamming_indicator = p->jamInd; - - ret = 1; - break; - } - - default: - break; - } - } +// case UBX_CLASS_MON: { +// switch (_message_id) { +// case UBX_MESSAGE_MON_HW: { +// +// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; +// +// _gps_position->noise_per_ms = p->noisePerMS; +// _gps_position->jamming_indicator = p->jamInd; +// +// ret = 1; +// break; +// } +// +// default: +// break; +// } +// } default: break; From efb44969d584e1b2613d96a795fc94a3ef728d9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 30 May 2014 21:57:48 +0200 Subject: [PATCH 5/7] ubx: send update only if got POSLLH & VELNED & TIMEUTC --- src/drivers/gps/ubx.cpp | 184 +++++++++++++++++++++------------------- src/drivers/gps/ubx.h | 3 + 2 files changed, 100 insertions(+), 87 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index cbfb61d00a..c143eeb0c2 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -69,6 +69,9 @@ UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _gps_position(gps_position), _configured(false), _waiting_for_ack(false), + _got_posllh(false), + _got_velned(false), + _got_timeutc(false), _disable_cmd_last(0) { decode_init(); @@ -219,19 +222,19 @@ UBX::configure(unsigned &baudrate) return 1; } -// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); -// -// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { -// warnx("MSG CFG FAIL: NAV SVINFO"); -// return 1; -// } -// -// configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); -// -// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { -// warnx("MSG CFG FAIL: MON HW"); -// return 1; -// } + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: NAV SVINFO"); + return 1; + } + + configure_message_rate(UBX_CLASS_MON, UBX_MESSAGE_MON_HW, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("MSG CFG FAIL: MON HW"); + return 1; + } _configured = true; return 0; @@ -275,9 +278,10 @@ UBX::receive(unsigned timeout) bool handled = false; while (true) { + bool ready_to_return = _configured ? (_got_posllh && _got_velned && _got_timeutc) : handled; /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ - int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), ready_to_return ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ @@ -286,7 +290,10 @@ UBX::receive(unsigned timeout) } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - if (handled) { + if (ready_to_return) { + _got_posllh = false; + _got_velned = false; + _got_timeutc = false; return 1; } else { @@ -438,6 +445,7 @@ UBX::handle_message() _rate_count_lat_lon++; + _got_posllh = true; ret = 1; break; } @@ -482,65 +490,66 @@ UBX::handle_message() _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); _gps_position->timestamp_time = hrt_absolute_time(); + _got_timeutc = true; ret = 1; break; } -// case UBX_MESSAGE_NAV_SVINFO: { -// //printf("GOT NAV_SVINFO\n"); -// const int length_part1 = 8; -// gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; -// const int length_part2 = 12; -// gps_bin_nav_svinfo_part2_packet_t *packet_part2; -// -// uint8_t satellites_used = 0; -// int i; -// -// //printf("Number of Channels: %d\n", packet_part1->numCh); -// for (i = 0; i < packet_part1->numCh; i++) { -// /* set pointer to sattelite_i information */ -// packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); -// -// /* write satellite information to global storage */ -// uint8_t sv_used = packet_part2->flags & 0x01; -// -// if (sv_used) { -// /* count SVs used for NAV */ -// satellites_used++; -// } -// -// /* record info for all channels, whether or not the SV is used for NAV */ -// _gps_position->satellite_used[i] = sv_used; -// _gps_position->satellite_snr[i] = packet_part2->cno; -// _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); -// _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); -// _gps_position->satellite_prn[i] = packet_part2->svid; -// //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); -// } -// -// for (i = packet_part1->numCh; i < 20; i++) { -// /* unused channels have to be set to zero for e.g. MAVLink */ -// _gps_position->satellite_prn[i] = 0; -// _gps_position->satellite_used[i] = 0; -// _gps_position->satellite_snr[i] = 0; -// _gps_position->satellite_elevation[i] = 0; -// _gps_position->satellite_azimuth[i] = 0; -// } -// -// _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones -// -// if (packet_part1->numCh > 0) { -// _gps_position->satellite_info_available = true; -// -// } else { -// _gps_position->satellite_info_available = false; -// } -// -// _gps_position->timestamp_satellites = hrt_absolute_time(); -// -// ret = 1; -// break; -// } + case UBX_MESSAGE_NAV_SVINFO: { + //printf("GOT NAV_SVINFO\n"); + const int length_part1 = 8; + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + + uint8_t satellites_used = 0; + int i; + + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); + + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + /* count SVs used for NAV */ + satellites_used++; + } + + /* record info for all channels, whether or not the SV is used for NAV */ + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); + } + + for (i = packet_part1->numCh; i < 20; i++) { + /* unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; + } + + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; + } case UBX_MESSAGE_NAV_VELNED: { // printf("GOT NAV_VELNED\n"); @@ -557,6 +566,7 @@ UBX::handle_message() _rate_count_vel++; + _got_velned = true; ret = 1; break; } @@ -573,23 +583,23 @@ UBX::handle_message() break; } -// case UBX_CLASS_MON: { -// switch (_message_id) { -// case UBX_MESSAGE_MON_HW: { -// -// struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; -// -// _gps_position->noise_per_ms = p->noisePerMS; -// _gps_position->jamming_indicator = p->jamInd; -// -// ret = 1; -// break; -// } -// -// default: -// break; -// } -// } + case UBX_CLASS_MON: { + switch (_message_id) { + case UBX_MESSAGE_MON_HW: { + + struct gps_bin_mon_hw_packet *p = (struct gps_bin_mon_hw_packet*) _rx_buffer; + + _gps_position->noise_per_ms = p->noisePerMS; + _gps_position->jamming_indicator = p->jamInd; + + ret = 1; + break; + } + + default: + break; + } + } default: break; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5cf47b60b5..43d6888936 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -397,6 +397,9 @@ private: struct vehicle_gps_position_s *_gps_position; bool _configured; bool _waiting_for_ack; + bool _got_posllh; + bool _got_velned; + bool _got_timeutc; uint8_t _message_class_needed; uint8_t _message_id_needed; ubx_decode_state_t _decode_state; From 6b8248ee29f38ae0f2ff10ebc23b1816e1fc6829 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 31 May 2014 16:19:38 +0200 Subject: [PATCH 6/7] position_estimator_inav: more safe EPH/EPV estimation, minor changes --- .../position_estimator_inav_main.c | 47 ++++++++++++------- 1 file changed, 30 insertions(+), 17 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 aa533c777f..f908d7a3be 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -79,7 +79,7 @@ static bool thread_running = false; /**< Deamon status flag */ static int position_estimator_inav_task; /**< Handle of deamon task / thread */ static bool verbose_mode = false; -static const hrt_abstime gps_topic_timeout = 1000000; // GPS topic timeout = 1s +static const hrt_abstime gps_topic_timeout = 500000; // GPS topic timeout = 0.5s static const hrt_abstime flow_topic_timeout = 1000000; // optical flow topic timeout = 1s 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 @@ -218,8 +218,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) memset(R_gps, 0, sizeof(R_gps)); int buf_ptr = 0; - float eph = 1.0; - float epv = 1.0; + static const float min_eph_epv = 2.0f; // min EPH/EPV, used for weight calculation + static const float max_eph_epv = 20.0f; // max EPH/EPV acceptable for estimation + + float eph = max_eph_epv; + float epv = 1.0f; + + float eph_flow = 1.0f; float x_est_prev[2], y_est_prev[2], z_est_prev[2]; memset(x_est_prev, 0, sizeof(x_est_prev)); @@ -276,9 +281,6 @@ 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) @@ -555,9 +557,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) w_flow *= 0.05f; } - flow_valid = true; + /* under ideal conditions, on 1m distance assume EPH = 10cm */ + eph_flow = 0.1 / w_flow; - eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist)); // under ideal conditions, on 1m distance assume EPH = 10cm + flow_valid = true; } else { w_flow = 0.0f; @@ -616,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* hysteresis for GPS quality */ if (gps_valid) { - if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) { + if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) { gps_valid = false; mavlink_log_info(mavlink_fd, "[inav] GPS signal lost"); } } else { - if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) { + if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) { gps_valid = true; reset_est = true; mavlink_log_info(mavlink_fd, "[inav] GPS signal found"); @@ -702,9 +705,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* save rotation matrix at this moment */ memcpy(R_gps, R_buf[est_i], sizeof(R_gps)); - eph = fminf(eph, gps.eph_m); - epv = fminf(epv, 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); } @@ -745,8 +745,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) t_prev = t; /* increase EPH/EPV on each step */ - eph *= 1.0 + dt; - epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + if (eph < max_eph_epv) { + eph *= 1.0 + dt; + } + if (epv < max_eph_epv) { + epv += 0.005 * dt; // add 1m to EPV each 200s (baro drift) + } /* use GPS if it's valid and reference position initialized */ bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W; @@ -759,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) xy_src_time = t; } - bool can_estimate_xy = eph < max_eph_epv * 1.5; + bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow; bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout); @@ -853,7 +857,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* 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); + + if (use_gps_z) { + epv = fminf(epv, gps.epv_m); + + inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p); + } if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) { write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v); @@ -878,11 +887,15 @@ int position_estimator_inav_thread_main(int argc, char *argv[]) /* inertial filter correction for position */ if (use_flow) { + eph = fminf(eph, eph_flow); + inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow); inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow); } if (use_gps_xy) { + eph = fminf(eph, gps.eph_m); + inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p); inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p); From 85b2dfa0c662298d49f584e6bd774954b04cec35 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 8 Jun 2014 12:30:27 +0200 Subject: [PATCH 7/7] fix initialization of perfcounters in fw att controllers --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 4 ++-- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0a909d02f5..a0a18bc2e4 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -61,9 +61,9 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); } ECL_PitchController::~ECL_PitchController() diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 82903ef5aa..d2a231694d 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -59,9 +59,9 @@ ECL_RollController::ECL_RollController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f) + _bodyrate_setpoint(0.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control roll nonfinite input"); } ECL_RollController::~ECL_RollController() diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index e53ffc6448..79184e2cdd 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -58,9 +58,9 @@ ECL_YawController::ECL_YawController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _coordinated_min_speed(1.0f) + _coordinated_min_speed(1.0f), + _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { - perf_alloc(PC_COUNT, "fw att control yaw nonfinite input"); } ECL_YawController::~ECL_YawController()