From 12f4cb2dc38303b58750a8f3b120738a291ac8d1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 16:13:17 +0100 Subject: Tuned GPS update rates --- apps/mavlink/mavlink.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) (limited to 'apps/mavlink') diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index ceb7c2554..09abcf269 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -625,7 +625,9 @@ int mavlink_thread_main(int argc, char *argv[]) /* 20 Hz / 50 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); - /* 2 Hz */ + /* 10 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 100); + /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); } else if (baudrate >= 115200) { @@ -634,8 +636,10 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); - /* 5 Hz / 100 ms */ + /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); + /* 5 Hz / 200 ms */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -651,6 +655,8 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 500); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); + /* 2 Hz */ + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ -- cgit v1.2.3 From 30f028908a69db058c05b9af7e31c8c52c3bc339 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 16:15:48 +0100 Subject: Fixed typo --- apps/mavlink/mavlink.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) (limited to 'apps/mavlink') diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 09abcf269..b958d5f96 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -626,7 +626,7 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 10 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 100); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 100); /* 10 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100); @@ -639,7 +639,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 5 Hz / 200 ms */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 200); /* 5 Hz / 200 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 200); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 200); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); @@ -656,7 +656,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 500); /* 2 Hz */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW, 500); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_GPS_RAW_INT, 500); } else { /* very low baud rate, limit to 1 Hz / 1000 ms */ -- cgit v1.2.3 From cb0fd834ae15c45cf6993f8c9eea9c99b2b153dd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 4 Feb 2013 18:14:55 +0100 Subject: Minor polishing, fixed rate and last measurement indication --- apps/drivers/gps/gps.cpp | 6 +++--- apps/drivers/gps/ubx.cpp | 6 ------ apps/mavlink/orb_listener.c | 2 +- 3 files changed, 4 insertions(+), 10 deletions(-) (limited to 'apps/mavlink') diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index bd77e8969..49b94fc2e 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -430,7 +430,7 @@ GPS::task_main() /* measure update rate every 5 seconds */ if (hrt_absolute_time() - last_rate_measurement > 5000000) { - _rate = last_rate_count / (float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f; + _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; } @@ -523,8 +523,8 @@ GPS::print_info() } warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { - warnx("position lock: %dD, last update %d seconds ago", (int)_report.fix_type, - int((hrt_absolute_time() - _report.timestamp) / 1000000)); + warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, + ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index fe4758d8c..1105e0da4 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -429,7 +429,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->fix_type = packet->gpsFix; - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; gps_position->s_variance = packet->sAcc; gps_position->p_variance = packet->pAcc; @@ -457,7 +456,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->eph = packet->hDOP; gps_position->epv = packet->vDOP; - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; _new_nav_dop = true; @@ -493,7 +491,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; _new_nav_timeutc = true; @@ -587,7 +584,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) } gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -617,7 +613,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->vel_ned_valid = true; gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - gps_position->timestamp = hrt_absolute_time(); gps_position->counter++; @@ -645,7 +640,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { // // gps_position->satellites_visible = packet->numVis; -// gps_position->timestamp = hrt_absolute_time(); // gps_position->counter++; // _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); // ret = 1; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index ec5149745..18da70f61 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -698,7 +698,7 @@ uorb_receive_start(void) /* --- GPS VALUE --- */ mavlink_subs.gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - orb_set_interval(mavlink_subs.gps_sub, 1000); /* 1Hz updates */ + orb_set_interval(mavlink_subs.gps_sub, 200); /* 5Hz updates */ /* --- HOME POSITION --- */ mavlink_subs.home_sub = orb_subscribe(ORB_ID(home_position)); -- cgit v1.2.3 From fc4be3e7280db480b67b7c6cec11e35481969bbb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 6 Feb 2013 12:41:05 -0800 Subject: Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps --- apps/commander/commander.c | 14 ++-- apps/drivers/gps/gps.cpp | 4 +- apps/drivers/gps/ubx.cpp | 110 +++++++++++++++----------------- apps/drivers/gps/ubx.h | 30 ++++----- apps/examples/kalman_demo/KalmanNav.cpp | 23 +++---- apps/mavlink/mavlink_receiver.c | 24 +++---- apps/mavlink/orb_listener.c | 10 +-- apps/uORB/topics/home_position.h | 8 +-- apps/uORB/topics/vehicle_gps_position.h | 55 ++++++++-------- 9 files changed, 139 insertions(+), 139 deletions(-) (limited to 'apps/mavlink') diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 3a6fecf74..f19f1d0e6 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1678,8 +1678,8 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ - float hdop_m = gps_position.eph / 100.0f; - float vdop_m = gps_position.epv / 100.0f; + float hdop_m = gps_position.eph_m; + float vdop_m = gps_position.epv_m; /* check if gps fix is ok */ // XXX magic number @@ -1697,7 +1697,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < dop_threshold_m) && (vdop_m < dop_threshold_m) && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp < 2000000) + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { warnx("setting home position"); @@ -1706,11 +1706,11 @@ int commander_thread_main(int argc, char *argv[]) home.lon = gps_position.lon; home.alt = gps_position.alt; - home.eph = gps_position.eph; - home.epv = gps_position.epv; + home.eph_m = gps_position.eph_m; + home.epv_m = gps_position.epv_m; - home.s_variance = gps_position.s_variance; - home.p_variance = gps_position.p_variance; + home.s_variance_m_s = gps_position.s_variance_m_s; + home.p_variance_m = gps_position.p_variance_m; /* announce new home position */ if (home_pub > 0) { diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 28dc949d4..8c2775adb 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -543,9 +543,9 @@ GPS::print_info() break; } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK"); - if (_report.timestamp != 0) { + if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("update rate: %6.2f Hz", (double)_rate); } diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 440ec74c5..eec1df980 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -52,7 +53,7 @@ _config_state(UBX_CONFIG_STATE_PRT), _waiting_for_ack(false), _new_nav_posllh(false), _new_nav_timeutc(false), -_new_nav_dop(false), +//_new_nav_dop(false), _new_nav_sol(false), _new_nav_velned(false) { @@ -211,10 +212,10 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; - break; +// case UBX_CONFIG_STATE_MSG_NAV_DOP: +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; +// break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; @@ -316,10 +317,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = NAV_TIMEUTC; break; - case UBX_MESSAGE_NAV_DOP: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_DOP; - break; +// case UBX_MESSAGE_NAV_DOP: +// _decode_state = UBX_DECODE_GOT_MESSAGEID; +// _message_id = NAV_DOP; +// break; case UBX_MESSAGE_NAV_SVINFO: _decode_state = UBX_DECODE_GOT_MESSAGEID; @@ -412,11 +413,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->lon = packet->lon; gps_position->alt = packet->height_msl; - gps_position->counter_pos_valid++; - gps_position->counter++; + gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m + gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m /* Add timestamp to finish the report */ - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_position = hrt_absolute_time(); _new_nav_posllh = true; /* set flag to trigger publishing of new position */ @@ -439,11 +440,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { gps_position->fix_type = packet->gpsFix; - gps_position->s_variance = packet->sAcc; - gps_position->p_variance = packet->pAcc; + gps_position->s_variance_m_s = packet->sAcc; + gps_position->p_variance_m = packet->pAcc; - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_variance = hrt_absolute_time(); _new_nav_sol = true; @@ -457,29 +457,28 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; } - case NAV_DOP: { -// printf("GOT NAV_DOP MESSAGE\n"); - gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; - - //Check if checksum is valid and the store the gps information - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - gps_position->eph = packet->hDOP; - gps_position->epv = packet->vDOP; - - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); - - _new_nav_dop = true; - - } else { - warnx("NAV_DOP: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; +// +// //Check if checksum is valid and the store the gps information +// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { +// +// gps_position->eph_m = packet->hDOP; +// gps_position->epv = packet->vDOP; +// +// gps_position->timestamp_posdilution = hrt_absolute_time(); +// +// _new_nav_dop = true; +// +// } else { +// warnx("NAV_DOP: checksum invalid"); +// } +// +// // Reset state machine to decode next packet +// decodeInit(); +// break; +// } case NAV_TIMEUTC: { // printf("GOT NAV_TIMEUTC MESSAGE\n"); @@ -501,8 +500,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_time = hrt_absolute_time(); _new_nav_timeutc = true; @@ -593,8 +591,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_satellites = hrt_absolute_time(); // as this message arrives only with 1Hz and is not essential, we don't take it into account for the report @@ -614,18 +611,17 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ //Check if checksum is valid and the store the gps information if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - gps_position->vel = (uint16_t)packet->speed; - gps_position->vel_n = packet->velN / 100.0f; - gps_position->vel_e = packet->velE / 100.0f; - gps_position->vel_d = packet->velD / 100.0f; + gps_position->vel_m_s = (float)packet->speed * 1e-2f; + gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; + gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; + gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; + gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; gps_position->vel_ned_valid = true; - gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f); - - gps_position->counter++; - gps_position->timestamp = hrt_absolute_time(); + gps_position->timestamp_velocity = hrt_absolute_time(); _new_nav_velned = true; + } else { warnx("NAV_VELNED: checksum invalid"); } @@ -690,13 +686,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC; break; case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_DOP; - break; - case UBX_CONFIG_STATE_MSG_NAV_DOP: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; break; +// case UBX_CONFIG_STATE_MSG_NAV_DOP: +// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) +// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO; +// break; case UBX_CONFIG_STATE_MSG_NAV_SVINFO: if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) _config_state = UBX_CONFIG_STATE_MSG_NAV_SOL; @@ -766,7 +762,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ } /* return 1 when position updates and the remaining packets updated at least once */ - if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) { + if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) { /* we have received everything, this most probably means that the configuration is fine */ config_needed = false; @@ -774,7 +770,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ /* Reset the flags */ _new_nav_posllh = false; _new_nav_timeutc = false; - _new_nav_dop = false; +// _new_nav_dop = false; _new_nav_sol = false; _new_nav_velned = false; diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index d3c6c6d4c..43cded02f 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -54,7 +54,7 @@ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 -#define UBX_MESSAGE_NAV_DOP 0x04 +//#define UBX_MESSAGE_NAV_DOP 0x04 #define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 @@ -140,18 +140,18 @@ typedef struct { uint8_t ck_b; } gps_bin_nav_timeutc_packet_t; -typedef struct { - uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ - uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ - uint16_t pDOP; /**< Position DOP (scaling 0.01) */ - uint16_t tDOP; /**< Time DOP (scaling 0.01) */ - uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ - uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ - uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ - uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ - uint8_t ck_a; - uint8_t ck_b; -} gps_bin_nav_dop_packet_t; +//typedef struct { +// uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ +// uint16_t gDOP; /**< Geometric DOP (scaling 0.01) */ +// uint16_t pDOP; /**< Position DOP (scaling 0.01) */ +// uint16_t tDOP; /**< Time DOP (scaling 0.01) */ +// uint16_t vDOP; /**< Vertical DOP (scaling 0.01) */ +// uint16_t hDOP; /**< Horizontal DOP (scaling 0.01) */ +// uint16_t nDOP; /**< Northing DOP (scaling 0.01) */ +// uint16_t eDOP; /**< Easting DOP (scaling 0.01) */ +// uint8_t ck_a; +// uint8_t ck_b; +//} gps_bin_nav_dop_packet_t; typedef struct { uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ @@ -311,7 +311,7 @@ typedef enum { NAV_POSLLH, NAV_SOL, NAV_TIMEUTC, - NAV_DOP, +// NAV_DOP, NAV_SVINFO, NAV_VELNED, // RXM_SVSI, @@ -371,7 +371,7 @@ private: unsigned _payload_size; bool _new_nav_posllh; bool _new_nav_timeutc; - bool _new_nav_dop; +// bool _new_nav_dop; bool _new_nav_sol; bool _new_nav_velned; }; diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 7e89dffb2..b7f651d8a 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -190,11 +190,12 @@ void KalmanNav::update() if (!_positionInitialized && _attitudeInitialized && // wait for attitude first gpsUpdate && - _gps.fix_type > 2 && - _gps.counter_pos_valid > 10) { - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + _gps.fix_type > 2 + //&& _gps.counter_pos_valid > 10 + ) { + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); @@ -259,7 +260,7 @@ void KalmanNav::updatePublications() // position publication _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp; + _pos.time_gps_usec = _gps.timestamp_position; _pos.valid = true; _pos.lat = getLatDegE7(); _pos.lon = getLonDegE7(); @@ -631,8 +632,8 @@ int KalmanNav::correctPos() // residual Vector y(5); - y(0) = _gps.vel_n - vN; - y(1) = _gps.vel_e - vE; + y(0) = _gps.vel_n_m_s - vN; + y(1) = _gps.vel_e_m_s - vE; y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; y(4) = double(_gps.alt) / 1.0e3 - alt; @@ -651,9 +652,9 @@ int KalmanNav::correctPos() // abort correction and return printf("[kalman_demo] numerical failure in gps correction\n"); // fallback to GPS - vN = _gps.vel_n; - vE = _gps.vel_e; - vD = _gps.vel_d; + vN = _gps.vel_n_m_s; + vE = _gps.vel_e_m_s; + vD = _gps.vel_d_m_s; setLatDegE7(_gps.lat); setLonDegE7(_gps.lon); setAltE3(_gps.alt); diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c index 86732d07c..b3b4b1e0b 100644 --- a/apps/mavlink/mavlink_receiver.c +++ b/apps/mavlink/mavlink_receiver.c @@ -387,22 +387,22 @@ handle_message(mavlink_message_t *msg) static uint64_t old_timestamp = 0; /* gps */ - hil_gps.timestamp = gps.time_usec; - hil_gps.counter = hil_counter++; + hil_gps.timestamp_position = gps.time_usec; +// hil_gps.counter = hil_counter++; hil_gps.time_gps_usec = gps.time_usec; hil_gps.lat = gps.lat; hil_gps.lon = gps.lon; hil_gps.alt = gps.alt; - hil_gps.counter_pos_valid = hil_counter++; - hil_gps.eph = gps.eph; - hil_gps.epv = gps.epv; - hil_gps.s_variance = 100; - hil_gps.p_variance = 100; - hil_gps.vel = gps.vel; - hil_gps.vel_n = gps.vel / 100.0f * cosf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_e = gps.vel / 100.0f * sinf(gps.cog / M_RAD_TO_DEG_F / 100.0f); - hil_gps.vel_d = 0.0f; - hil_gps.cog = gps.cog; +// hil_gps.counter_pos_valid = hil_counter++; + hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m + hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m + hil_gps.s_variance_m_s = 100; // XXX 100 m/s variance? + hil_gps.p_variance_m = 100; // XXX 100 m variance? + hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s + hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(gps.cog * M_DEG_TO_RAD_F * 1e-2f); + hil_gps.vel_d_m_s = 0.0f; + hil_gps.cog_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f; // from deg*100 to rad hil_gps.fix_type = gps.fix_type; hil_gps.satellites_visible = gps.satellites_visible; diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 18da70f61..9f85d5801 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -231,15 +231,15 @@ l_vehicle_gps_position(struct listener *l) /* GPS position */ mavlink_msg_gps_raw_int_send(MAVLINK_COMM_0, - gps.timestamp, + gps.timestamp_position, gps.fix_type, gps.lat, gps.lon, gps.alt, - gps.eph, - gps.epv, - gps.vel, - gps.cog, + (uint16_t)(gps.eph_m * 1e2f), // from m to cm + (uint16_t)(gps.epv_m * 1e2f), // from m to cm + (uint16_t)(gps.vel_m_s * 1e2f), // from m/s to cm/s + (uint16_t)(gps.cog_rad * M_RAD_TO_DEG_F * 1e2f), // from rad to deg * 100 gps.satellites_visible); if (gps.satellite_info_available && (gps_counter % 4 == 0)) { diff --git a/apps/uORB/topics/home_position.h b/apps/uORB/topics/home_position.h index dec34b6ab..7e1b82a0f 100644 --- a/apps/uORB/topics/home_position.h +++ b/apps/uORB/topics/home_position.h @@ -61,10 +61,10 @@ struct home_position_s int32_t lat; /**< Latitude in 1E7 degrees */ int32_t lon; /**< Longitude in 1E7 degrees */ int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; /**< speed accuracy estimate cm/s */ - float p_variance; /**< position accuracy estimate cm */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ }; /** diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h index db529da06..aa75c22ac 100644 --- a/apps/uORB/topics/vehicle_gps_position.h +++ b/apps/uORB/topics/vehicle_gps_position.h @@ -55,35 +55,38 @@ */ struct vehicle_gps_position_s { - uint64_t timestamp; /**< Timestamp (microseconds since system boot) */ - uint32_t counter; /**< Count of GPS messages */ - uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + uint64_t timestamp_position; /**< Timestamp for position information */ + int32_t lat; /**< Latitude in 1E7 degrees */ + int32_t lon; /**< Longitude in 1E7 degrees */ + int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL */ - int32_t lat; /**< Latitude in 1E7 degrees //LOGME */ - int32_t lon; /**< Longitude in 1E7 degrees //LOGME */ - int32_t alt; /**< Altitude in 1E3 meters (millimeters) above MSL //LOGME */ - uint16_t counter_pos_valid; /**< is only increased when new lat/lon/alt information was added */ - uint16_t eph; /**< GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 //LOGME */ - uint16_t epv; /**< GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 */ - float s_variance; /**< speed accuracy estimate cm/s */ - float p_variance; /**< position accuracy estimate cm */ - uint16_t vel; /**< GPS ground speed (m/s * 100). If unknown, set to: 65535 */ - float vel_n; /**< GPS ground speed in m/s */ - float vel_e; /**< GPS ground speed in m/s */ - float vel_d; /**< GPS ground speed in m/s */ - uint16_t cog; /**< Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 */ - uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint64_t timestamp_variance; + float s_variance_m_s; /**< speed accuracy estimate m/s */ + float p_variance_m; /**< position accuracy estimate m */ + uint8_t fix_type; /**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. */ - uint8_t satellite_prn[20]; /**< Global satellite ID */ - uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ - uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ - uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ - uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ - uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ + float eph_m; /**< GPS HDOP horizontal dilution of position in m */ + float epv_m; /**< GPS VDOP horizontal dilution of position in m */ - /* flags */ - float vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + uint64_t timestamp_velocity; /**< Timestamp for velocity informations */ + float vel_m_s; /**< GPS ground speed (m/s) */ + float vel_n_m_s; /**< GPS ground speed in m/s */ + float vel_e_m_s; /**< GPS ground speed in m/s */ + float vel_d_m_s; /**< GPS ground speed in m/s */ + float cog_rad; /**< Course over ground (NOT heading, but direction of movement) in rad */ + bool vel_ned_valid; /**< Flag to indicate if NED speed is valid */ + + uint64_t timestamp_time; /**< Timestamp for time information */ + uint64_t time_gps_usec; /**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module */ + + uint64_t timestamp_satellites; /**< Timestamp for sattelite information */ + uint8_t satellites_visible; /**< Number of satellites visible. If unknown, set to 255 */ + uint8_t satellite_prn[20]; /**< Global satellite ID */ + uint8_t satellite_used[20]; /**< 0: Satellite not used, 1: used for localization */ + uint8_t satellite_elevation[20]; /**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */ + uint8_t satellite_azimuth[20]; /**< Direction of satellite, 0: 0 deg, 255: 360 deg. */ + uint8_t satellite_snr[20]; /**< Signal to noise ratio of satellite */ + uint8_t satellite_info_available; /**< 0 for no info, 1 for info available */ }; /** -- cgit v1.2.3