diff options
Diffstat (limited to 'apps/drivers')
-rw-r--r-- | apps/drivers/gps/gps.cpp | 4 | ||||
-rw-r--r-- | apps/drivers/gps/ubx.cpp | 110 | ||||
-rw-r--r-- | apps/drivers/gps/ubx.h | 30 |
3 files changed, 70 insertions, 74 deletions
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 <unistd.h> #include <stdio.h> +#include <math.h> #include <string.h> #include <assert.h> #include <systemlib/err.h> @@ -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"); } @@ -691,12 +687,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ 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; }; |