From ff6676ef8cde121df09c327821f3a8060e2ef013 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 19 Oct 2015 13:16:11 +0200 Subject: [PATCH] GPS: Fixed code style --- src/drivers/gps/ashtech.cpp | 40 +++--- src/drivers/gps/gps.cpp | 75 ++++++----- src/drivers/gps/mtk.cpp | 8 +- src/drivers/gps/ubx.cpp | 240 ++++++++++++++++++++++++------------ 4 files changed, 237 insertions(+), 126 deletions(-) diff --git a/src/drivers/gps/ashtech.cpp b/src/drivers/gps/ashtech.cpp index eca775745f..322d275549 100644 --- a/src/drivers/gps/ashtech.cpp +++ b/src/drivers/gps/ashtech.cpp @@ -37,7 +37,7 @@ ASHTECH::~ASHTECH() int ASHTECH::handle_message(int len) { - char * endp; + char *endp; if (len < 7) { return 0; } @@ -69,7 +69,8 @@ int ASHTECH::handle_message(int len) Fields 5 and 6 together yield the total offset. For example, if field 5 is -5 and field 6 is +15, local time is 5 hours and 15 minutes earlier than GMT. */ double ashtech_time = 0.0; - int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, local_time_off_min __attribute__((unused)) = 0; + int day = 0, month = 0, year = 0, local_time_off_hour __attribute__((unused)) = 0, + local_time_off_min __attribute__((unused)) = 0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -110,12 +111,14 @@ int ASHTECH::handle_message(int len) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = usecs * 1000; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += usecs; + } else { _gps_position->time_utc_usec = 0; } @@ -266,7 +269,8 @@ int ASHTECH::handle_message(int len) double ashtech_time __attribute__((unused)) = 0.0, lat = 0.0, lon = 0.0, alt = 0.0; int num_of_sv __attribute__((unused)) = 0, fix_quality = 0; double track_true = 0.0, ground_speed = 0.0 , age_of_corr __attribute__((unused)) = 0.0; - double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; + double hdop __attribute__((unused)) = 99.9, vdop __attribute__((unused)) = 99.9, pdop __attribute__((unused)) = 99.9, + tdop __attribute__((unused)) = 99.9, vertic_vel = 0.0; char ns = '?', ew = '?'; if (bufptr && *(++bufptr) != ',') { fix_quality = strtol(bufptr, &endp, 10); bufptr = endp; } @@ -281,7 +285,9 @@ int ASHTECH::handle_message(int len) * or strtod won't find anything and endp will point exactly where bufptr is. The same is for lon and alt. */ lat = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -289,7 +295,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { lon = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -297,7 +305,9 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt = strtod(bufptr, &endp); + if (bufptr != endp) {coordinatesFound++;} + bufptr = endp; } @@ -349,7 +359,8 @@ int ASHTECH::handle_message(int len) _gps_position->vel_n_m_s = velocity_north; /** GPS ground speed in m/s */ _gps_position->vel_e_m_s = velocity_east; /** GPS ground speed in m/s */ _gps_position->vel_d_m_s = static_cast(-vertic_vel); /** GPS ground speed in m/s */ - _gps_position->cog_rad = track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ + _gps_position->cog_rad = + track_rad; /** Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */ _gps_position->vel_ned_valid = true; /** Flag to indicate if NED speed is valid */ _gps_position->c_variance_rad = 0.1f; _gps_position->timestamp_velocity = hrt_absolute_time(); @@ -381,7 +392,8 @@ int ASHTECH::handle_message(int len) 9 The checksum data, always begins with * */ double ashtech_time __attribute__((unused)) = 0.0, lat_err = 0.0, lon_err = 0.0, alt_err = 0.0; - double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; + double min_err __attribute__((unused)) = 0.0, maj_err __attribute__((unused)) = 0.0, + deg_from_north __attribute__((unused)) = 0.0, rms_err __attribute__((unused)) = 0.0; if (bufptr && *(++bufptr) != ',') { ashtech_time = strtod(bufptr, &endp); bufptr = endp; } @@ -400,7 +412,7 @@ int ASHTECH::handle_message(int len) if (bufptr && *(++bufptr) != ',') { alt_err = strtod(bufptr, &endp); bufptr = endp; } _gps_position->eph = sqrtf(static_cast(lat_err) * static_cast(lat_err) - + static_cast(lon_err) * static_cast(lon_err)); + + static_cast(lon_err) * static_cast(lon_err)); _gps_position->epv = static_cast(alt_err); _gps_position->s_variance_m_s = 0; @@ -571,7 +583,7 @@ int ASHTECH::parse_char(uint8_t b) int iRet = 0; switch (_decode_state) { - /* First, look for sync1 */ + /* First, look for sync1 */ case NME_DECODE_UNINIT: if (b == '$') { _decode_state = NME_DECODE_GOT_SYNC1; @@ -636,13 +648,13 @@ void ASHTECH::decode_init(void) */ const char comm[] = "$PASHS,POP,20\r\n"\ - "$PASHS,NME,ZDA,B,ON,3\r\n"\ - "$PASHS,NME,GGA,B,OFF\r\n"\ - "$PASHS,NME,GST,B,ON,3\r\n"\ - "$PASHS,NME,POS,B,ON,0.05\r\n"\ - "$PASHS,NME,GSV,B,ON,3\r\n"\ - "$PASHS,SPD,A,8\r\n"\ - "$PASHS,SPD,B,9\r\n"; + "$PASHS,NME,ZDA,B,ON,3\r\n"\ + "$PASHS,NME,GGA,B,OFF\r\n"\ + "$PASHS,NME,GST,B,ON,3\r\n"\ + "$PASHS,NME,POS,B,ON,0.05\r\n"\ + "$PASHS,NME,GSV,B,ON,3\r\n"\ + "$PASHS,SPD,A,8\r\n"\ + "$PASHS,SPD,B,9\r\n"; int ASHTECH::configure(unsigned &baudrate) { diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 35ee8658a0..05ff3e5b91 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -211,8 +211,9 @@ GPS::~GPS() } /* well, kill it anyway, though this will probably crash */ - if (_task != -1) + if (_task != -1) { task_delete(_task); + } g_dev = nullptr; @@ -224,12 +225,13 @@ GPS::init() int ret = ERROR; /* do regular cdev init */ - if (CDev::init() != OK) + if (CDev::init() != OK) { goto out; + } /* start the GPS driver worker task */ _task = px4_task_spawn_cmd("gps", SCHED_DEFAULT, - SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); + SCHED_PRIORITY_SLOW_DRIVER, 1200, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { warnx("task start failed: %d", errno); @@ -305,9 +307,10 @@ GPS::task_main() _report_gps_pos.vel_n_m_s = 0.0f; _report_gps_pos.vel_e_m_s = 0.0f; _report_gps_pos.vel_d_m_s = 0.0f; - _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); + _report_gps_pos.vel_m_s = sqrtf(_report_gps_pos.vel_n_m_s * _report_gps_pos.vel_n_m_s + _report_gps_pos.vel_e_m_s * + _report_gps_pos.vel_e_m_s + _report_gps_pos.vel_d_m_s * _report_gps_pos.vel_d_m_s); _report_gps_pos.cog_rad = 0.0f; - _report_gps_pos.vel_ned_valid = true; + _report_gps_pos.vel_ned_valid = true; /* no time and satellite information simulated */ @@ -392,14 +395,16 @@ GPS::task_main() } int helper_ret; + while ((helper_ret = _Helper->receive(TIMEOUT_5HZ)) > 0 && !_task_should_exit) { - // lock(); + // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (!(_pub_blocked)) { if (helper_ret & 1) { orb_publish(ORB_ID(vehicle_gps_position), _report_gps_pos_pub, &_report_gps_pos); } + if (_p_report_sat_info && (helper_ret & 2)) { if (_report_sat_info_pub != nullptr) { orb_publish(ORB_ID(satellite_info), _report_sat_info_pub, _p_report_sat_info); @@ -508,7 +513,7 @@ void GPS::print_info() { //GPS Mode - if(_fake_gps) { + if (_fake_gps) { warnx("protocol: SIMULATED"); } @@ -522,27 +527,27 @@ GPS::print_info() warnx("protocol: MTK"); break; - case GPS_DRIVER_MODE_ASHTECH: - warnx("protocol: ASHTECH"); - break; + case GPS_DRIVER_MODE_ASHTECH: + warnx("protocol: ASHTECH"); + break; default: break; - } + } } warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); - warnx("sat info: %s, noise: %d, jamming detected: %s", - (_p_report_sat_info != nullptr) ? "enabled" : "disabled", - _report_gps_pos.noise_per_ms, - _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); + warnx("sat info: %s, noise: %d, jamming detected: %s", + (_p_report_sat_info != nullptr) ? "enabled" : "disabled", + _report_gps_pos.noise_per_ms, + _report_gps_pos.jamming_indicator == 255 ? "YES" : "NO"); if (_report_gps_pos.timestamp_position != 0) { warnx("position lock: %dD, satellites: %d, last update: %8.4fms ago", (int)_report_gps_pos.fix_type, - _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); + _report_gps_pos.satellites_used, (double)(hrt_absolute_time() - _report_gps_pos.timestamp_position) / 1000.0); warnx("lat: %d, lon: %d, alt: %d", _report_gps_pos.lat, _report_gps_pos.lon, _report_gps_pos.alt); warnx("vel: %.2fm/s, %.2fm/s, %.2fm/s", (double)_report_gps_pos.vel_n_m_s, - (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); + (double)_report_gps_pos.vel_e_m_s, (double)_report_gps_pos.vel_d_m_s); warnx("eph: %.2fm, epv: %.2fm", (double)_report_gps_pos.eph, (double)_report_gps_pos.epv); warnx("rate position: \t%6.2f Hz", (double)_Helper->get_position_update_rate()); warnx("rate velocity: \t%6.2f Hz", (double)_Helper->get_velocity_update_rate()); @@ -575,17 +580,20 @@ start(const char *uart_path, bool fake_gps, bool enable_sat_info) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver */ g_dev = new GPS(uart_path, fake_gps, enable_sat_info); - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } - if (OK != g_dev->init()) + if (OK != g_dev->init()) { goto fail; + } /* set the poll rate to default, starts automatic data collection */ fd = open(GPS0_DEVICE_PATH, O_RDONLY); @@ -639,11 +647,13 @@ reset() { int fd = open(GPS0_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "reset failed"); + } exit(0); } @@ -654,8 +664,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "not running"); + } g_dev->print_info(); @@ -690,39 +701,45 @@ gps_main(int argc, char *argv[]) /* Detect fake gps option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-f")) + if (!strcmp(argv[i], "-f")) { fake_gps = true; + } } /* Detect sat info option */ for (int i = 2; i < argc; i++) { - if (!strcmp(argv[i], "-s")) + if (!strcmp(argv[i], "-s")) { enable_sat_info = true; + } } gps::start(device_name, fake_gps, enable_sat_info); } - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { gps::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { gps::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { gps::reset(); + } /* * Print driver status. */ - if (!strcmp(argv[1], "status")) + if (!strcmp(argv[1], "status")) { gps::info(); + } out: errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status'\n [-d /dev/ttyS0-n][-f (for enabling fake)][-s (to enable sat info)]"); diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 371f17c1a9..25991b090a 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -66,8 +66,9 @@ int MTK::configure(unsigned &baudrate) { /* set baudrate first */ - if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) + if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0) { return -1; + } baudrate = MTK_BAUDRATE; @@ -207,8 +208,9 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } else if (_decode_state == MTK_DECODE_GOT_CK_B) { // Add to checksum - if (_rx_count < 33) + if (_rx_count < 33) { add_byte_to_checksum(b); + } // Fill packet buffer ((uint8_t *)(&packet))[_rx_count] = b; @@ -288,12 +290,14 @@ MTK::handle_message(gps_mtk_packet_t &packet) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = timeinfo_conversion_temp * 1000000ULL; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += timeinfo_conversion_temp * 1000ULL; + } else { _gps_position->time_utc_usec = 0; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index a7247a262d..643b339be3 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -199,6 +199,7 @@ UBX::configure(unsigned &baudrate) if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } + #endif /* configure message rates */ @@ -207,41 +208,50 @@ UBX::configure(unsigned &baudrate) /* try to set rate for NAV-PVT */ /* (implemented for ubx7+ modules only, use NAV-SOL, NAV-POSLLH, NAV-VELNED and NAV-TIMEUTC for ubx6) */ configure_message_rate(UBX_MSG_NAV_PVT, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { _use_nav_pvt = false; + } else { _use_nav_pvt = true; } + UBX_DEBUG("%susing NAV-PVT", _use_nav_pvt ? "" : "not "); if (!_use_nav_pvt) { configure_message_rate(UBX_MSG_NAV_TIMEUTC, 5); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_POSLLH, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_SOL, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_NAV_VELNED, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } } configure_message_rate(UBX_MSG_NAV_SVINFO, (_satellite_info != nullptr) ? 5 : 0); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } configure_message_rate(UBX_MSG_MON_HW, 1); + if (wait_for_ack(UBX_MSG_CFG_MSG, UBX_CONFIG_TIMEOUT, true) < 0) { return 1; } @@ -269,9 +279,11 @@ UBX::wait_for_ack(const uint16_t msg, const unsigned timeout, const bool report) if (_ack_state == UBX_ACK_GOT_ACK) { ret = 0; // ACK received ok + } else if (report) { if (_ack_state == UBX_ACK_GOT_NAK) { UBX_DEBUG("ubx msg 0x%04x NAK", SWAP16((unsigned)msg)); + } else { UBX_DEBUG("ubx msg 0x%04x ACK timeout", SWAP16((unsigned)msg)); } @@ -359,6 +371,7 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("\nA"); _decode_state = UBX_DECODE_SYNC2; } + break; /* Expecting Sync2 */ @@ -370,6 +383,7 @@ UBX::parse_char(const uint8_t b) } else { // Sync1 not followed by Sync2: reset parser decode_init(); } + break; /* Expecting Class */ @@ -401,38 +415,48 @@ UBX::parse_char(const uint8_t b) UBX_TRACE_PARSER("F"); add_byte_to_checksum(b); _rx_payload_length |= b << 8; // calculate payload size + if (payload_rx_init() != 0) { // start payload reception // payload will not be handled, discard message decode_init(); + } else { _decode_state = (_rx_payload_length > 0) ? UBX_DECODE_PAYLOAD : UBX_DECODE_CHKSUM1; } + break; /* Expecting payload */ case UBX_DECODE_PAYLOAD: UBX_TRACE_PARSER("."); add_byte_to_checksum(b); + switch (_rx_msg) { case UBX_MSG_NAV_SVINFO: ret = payload_rx_add_nav_svinfo(b); // add a NAV-SVINFO payload byte break; + case UBX_MSG_MON_VER: ret = payload_rx_add_mon_ver(b); // add a MON-VER payload byte break; + default: ret = payload_rx_add(b); // add a payload byte break; } + if (ret < 0) { // payload not handled, discard message decode_init(); + } else if (ret > 0) { // payload complete, expecting checksum _decode_state = UBX_DECODE_CHKSUM1; + } else { // expecting more payload, stay in state UBX_DECODE_PAYLOAD } + ret = 0; break; @@ -441,18 +465,22 @@ UBX::parse_char(const uint8_t b) if (_rx_ck_a != b) { UBX_WARN("ubx checksum err"); decode_init(); + } else { _decode_state = UBX_DECODE_CHKSUM2; } + break; /* Expecting second checksum byte */ case UBX_DECODE_CHKSUM2: if (_rx_ck_b != b) { UBX_WARN("ubx checksum err"); + } else { ret = payload_rx_done(); // finish payload processing } + decode_init(); break; @@ -475,83 +503,116 @@ UBX::payload_rx_init() switch (_rx_msg) { case UBX_MSG_NAV_PVT: - if ( (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ - && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) /* u-blox 8+ msg format */ + if ((_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX7) /* u-blox 7 msg format */ + && (_rx_payload_length != UBX_PAYLOAD_RX_NAV_PVT_SIZE_UBX8)) { /* u-blox 8+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (!_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if not using NAV-PVT + } + break; case UBX_MSG_NAV_POSLLH: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_posllh_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SOL: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_sol_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_TIMEUTC: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_timeutc_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_NAV_SVINFO: - if (_satellite_info == nullptr) - _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else - memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + if (_satellite_info == nullptr) { + _rx_state = UBX_RXMSG_DISABLE; // disable if sat info not requested + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else { + memset(_satellite_info, 0, sizeof(*_satellite_info)); // initialize sat info + } + break; case UBX_MSG_NAV_VELNED: - if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_nav_velned_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured - else if (_use_nav_pvt) - _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (_use_nav_pvt) { + _rx_state = UBX_RXMSG_DISABLE; // disable if using NAV-PVT instead + } + break; case UBX_MSG_MON_VER: break; // unconditionally handle this message case UBX_MSG_MON_HW: - if ( (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ - && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) /* u-blox 7+ msg format */ + if ((_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx6_t)) /* u-blox 6 msg format */ + && (_rx_payload_length != sizeof(ubx_payload_rx_mon_hw_ubx7_t))) { /* u-blox 7+ msg format */ _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (!_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + + } else if (!_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if not _configured + } + break; case UBX_MSG_ACK_ACK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_ack_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; case UBX_MSG_ACK_NAK: - if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) + if (_rx_payload_length != sizeof(ubx_payload_rx_ack_nak_t)) { _rx_state = UBX_RXMSG_ERROR_LENGTH; - else if (_configured) - _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + + } else if (_configured) { + _rx_state = UBX_RXMSG_IGNORE; // ignore if _configured + } + break; default: @@ -624,32 +685,39 @@ UBX::payload_rx_add_nav_svinfo(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_nav_svinfo_part1_t)) { // Part 1 complete: decode Part 1 buffer _satellite_info->count = MIN(_buf.payload_rx_nav_svinfo_part1.numCh, satellite_info_s::SAT_INFO_MAX_SATELLITES); - UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); + UBX_TRACE_SVINFO("SVINFO len %u numCh %u\n", (unsigned)_rx_payload_length, + (unsigned)_buf.payload_rx_nav_svinfo_part1.numCh); } - if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof(ubx_payload_rx_nav_svinfo_part2_t)) { + + if (_rx_payload_index < sizeof(ubx_payload_rx_nav_svinfo_part1_t) + _satellite_info->count * sizeof( + ubx_payload_rx_nav_svinfo_part2_t)) { // Still room in _satellite_info: fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) % sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_nav_svinfo_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer - unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof(ubx_payload_rx_nav_svinfo_part2_t); + unsigned sat_index = (_rx_payload_index - sizeof(ubx_payload_rx_nav_svinfo_part1_t)) / sizeof( + ubx_payload_rx_nav_svinfo_part2_t); _satellite_info->used[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.flags & 0x01); _satellite_info->snr[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.cno); _satellite_info->elevation[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.elev); _satellite_info->azimuth[sat_index] = (uint8_t)((float)_buf.payload_rx_nav_svinfo_part2.azim * 255.0f / 360.0f); _satellite_info->svid[sat_index] = (uint8_t)(_buf.payload_rx_nav_svinfo_part2.svid); UBX_TRACE_SVINFO("SVINFO #%02u used %u snr %3u elevation %3u azimuth %3u svid %3u\n", - (unsigned)sat_index + 1, - (unsigned)_satellite_info->used[sat_index], - (unsigned)_satellite_info->snr[sat_index], - (unsigned)_satellite_info->elevation[sat_index], - (unsigned)_satellite_info->azimuth[sat_index], - (unsigned)_satellite_info->svid[sat_index] - ); + (unsigned)sat_index + 1, + (unsigned)_satellite_info->used[sat_index], + (unsigned)_satellite_info->snr[sat_index], + (unsigned)_satellite_info->elevation[sat_index], + (unsigned)_satellite_info->azimuth[sat_index], + (unsigned)_satellite_info->svid[sat_index] + ); } } } @@ -672,6 +740,7 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) if (_rx_payload_index < sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Fill Part 1 buffer _buf.raw[_rx_payload_index] = b; + } else { if (_rx_payload_index == sizeof(ubx_payload_rx_mon_ver_part1_t)) { // Part 1 complete: decode Part 1 buffer and calculate hash for SW&HW version strings @@ -681,9 +750,12 @@ UBX::payload_rx_add_mon_ver(const uint8_t b) UBX_DEBUG("VER hw \"%10s\"", _buf.payload_rx_mon_ver_part1.hwVersion); UBX_DEBUG("VER sw \"%30s\"", _buf.payload_rx_mon_ver_part1.swVersion); } + // fill Part 2 buffer - unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof(ubx_payload_rx_mon_ver_part2_t); + unsigned buf_index = (_rx_payload_index - sizeof(ubx_payload_rx_mon_ver_part1_t)) % sizeof( + ubx_payload_rx_mon_ver_part2_t); _buf.raw[buf_index] = b; + if (buf_index == sizeof(ubx_payload_rx_mon_ver_part2_t) - 1) { // Part 2 complete: decode Part 2 buffer UBX_DEBUG("VER ext \" %30s\"", _buf.payload_rx_mon_ver_part2.extension); @@ -717,13 +789,11 @@ UBX::payload_rx_done(void) UBX_TRACE_RXMSG("Rx NAV-PVT\n"); //Check if position fix flag is good - if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) - { + if ((_buf.payload_rx_nav_pvt.flags & UBX_RX_NAV_PVT_FLAGS_GNSSFIXOK) == 1) { _gps_position->fix_type = _buf.payload_rx_nav_pvt.fixType; _gps_position->vel_ned_valid = true; - } - else - { + + } else { _gps_position->fix_type = 0; _gps_position->vel_ned_valid = false; } @@ -748,10 +818,9 @@ UBX::payload_rx_done(void) _gps_position->c_variance_rad = (float)_buf.payload_rx_nav_pvt.headAcc * M_DEG_TO_RAD_F * 1e-5f; //Check if time and date fix flags are good - if( (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) - && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) - { + if ((_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDDATE) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_VALIDTIME) + && (_buf.payload_rx_nav_pvt.valid & UBX_RX_NAV_PVT_VALID_FULLYRESOLVED)) { /* convert to unix timestamp */ struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_pvt.year - 1900; @@ -770,12 +839,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_pvt.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -827,8 +898,7 @@ UBX::payload_rx_done(void) case UBX_MSG_NAV_TIMEUTC: UBX_TRACE_RXMSG("Rx NAV-TIMEUTC\n"); - if(_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) - { + if (_buf.payload_rx_nav_timeutc.valid & UBX_RX_NAV_TIMEUTC_VALID_VALIDUTC) { // convert to unix timestamp struct tm timeinfo; timeinfo.tm_year = _buf.payload_rx_nav_timeutc.year - 1900; @@ -849,12 +919,14 @@ UBX::payload_rx_done(void) timespec ts; ts.tv_sec = epoch; ts.tv_nsec = _buf.payload_rx_nav_timeutc.nano; + if (clock_settime(CLOCK_REALTIME, &ts)) { warn("failed setting clock"); } _gps_position->time_utc_usec = static_cast(epoch) * 1000000ULL; _gps_position->time_utc_usec += _buf.payload_rx_nav_timeutc.nano / 1000; + } else { _gps_position->time_utc_usec = 0; } @@ -922,6 +994,7 @@ UBX::payload_rx_done(void) ret = 0; // don't handle message break; } + break; case UBX_MSG_ACK_ACK: @@ -999,39 +1072,44 @@ UBX::send_message(const uint16_t msg, const uint8_t *payload, const uint16_t len header.length = length; // Calculate checksum - calc_checksum(((uint8_t*)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes - if (payload != nullptr) + calc_checksum(((uint8_t *)&header) + 2, sizeof(header) - 2, &checksum); // skip 2 sync bytes + + if (payload != nullptr) { calc_checksum(payload, length, &checksum); + } // Send message write(_fd, (const void *)&header, sizeof(header)); - if (payload != nullptr) + + if (payload != nullptr) { write(_fd, (const void *)payload, length); + } + write(_fd, (const void *)&checksum, sizeof(checksum)); } uint32_t UBX::fnv1_32_str(uint8_t *str, uint32_t hval) { - uint8_t *s = str; + uint8_t *s = str; - /* - * FNV-1 hash each octet in the buffer - */ - while (*s) { + /* + * FNV-1 hash each octet in the buffer + */ + while (*s) { - /* multiply by the 32 bit FNV magic prime mod 2^32 */ + /* multiply by the 32 bit FNV magic prime mod 2^32 */ #if defined(NO_FNV_GCC_OPTIMIZATION) - hval *= FNV1_32_PRIME; + hval *= FNV1_32_PRIME; #else - hval += (hval<<1) + (hval<<4) + (hval<<7) + (hval<<8) + (hval<<24); + hval += (hval << 1) + (hval << 4) + (hval << 7) + (hval << 8) + (hval << 24); #endif - /* xor the bottom with the current octet */ - hval ^= (uint32_t)*s++; - } + /* xor the bottom with the current octet */ + hval ^= (uint32_t) * s++; + } - /* return our new hash value */ - return hval; + /* return our new hash value */ + return hval; }