diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 89fc54b0d92e..1ae27ed2f9f1 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -54,8 +54,6 @@ typedef enum { } gps_driver_mode_t; - - /* * ObjDev tag for GPS data. */ diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 49b94fc2e726..450b3091bd79 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -33,7 +33,7 @@ /** * @file gps.cpp - * Driver for the GPS on UART6 + * Driver for the GPS on a serial port */ #include @@ -107,46 +107,41 @@ class GPS : public device::CDev private: - int _task_should_exit; - int _serial_fd; ///< serial interface to GPS - unsigned _baudrate; - unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; - uint8_t _send_buffer[SEND_BUFFER_LENGTH]; - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; - perf_counter_t _buffer_overflows; - volatile int _task; ///< worker task + bool _task_should_exit; ///< flag to make the main worker task exit + int _serial_fd; ///< serial interface to GPS + unsigned _baudrate; ///< current baudrate + unsigned _baudrates_to_try[NUMBER_OF_BAUDRATES]; ///< try different baudrates that GPS could be set to + volatile int _task; ///< worker task + bool _config_needed; ///< flag to signal that configuration of GPS is needed + bool _baudrate_changed; ///< flag to signal that the baudrate with the GPS has changed + bool _mode_changed; ///< flag that the GPS mode has changed + gps_driver_mode_t _mode; ///< current mode + GPS_Helper *_Helper; ///< Class for either UBX, MTK or NMEA helper + struct vehicle_gps_position_s _report; ///< uORB topic for gps position + orb_advert_t _report_pub; ///< uORB pub for gps position + float _rate; ///< position update rate - bool _response_received; - bool _config_needed; - bool _baudrate_changed; - bool _mode_changed; - bool _healthy; - gps_driver_mode_t _mode; - unsigned _messages_received; - float _rate; ///< position update rate - - GPS_Helper *_Helper; ///< class for either UBX, MTK or NMEA helper - - struct vehicle_gps_position_s _report; ///< the current GPS report - orb_advert_t _report_pub; ///< the publication topic, only valid on first valid GPS module message - - void recv(); + /** + * Try to configure the GPS, handle outgoing communication to the GPS + */ void config(); + /** + * Trampoline to the worker task + */ static void task_main_trampoline(void *arg); /** - * Worker task. + * Worker task: main GPS thread that configures the GPS and parses incoming data, always running */ void task_main(void); /** - * Set the module baud rate + * Set the baudrate of the UART to the GPS */ - int set_baudrate(unsigned baud); + int set_baudrate(unsigned baud); /** * Send a reset command to the GPS @@ -176,12 +171,10 @@ GPS::GPS() : _config_needed(true), _baudrate_changed(false), _mode_changed(true), - _healthy(false), _mode(GPS_DRIVER_MODE_UBX), - _messages_received(0), _Helper(nullptr), - _rate(0.0f), - _report_pub(-1) + _report_pub(-1), + _rate(0.0f) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -216,7 +209,7 @@ GPS::init() if (CDev::init() != OK) goto out; - /* start the IO interface task */ + /* start the GPS driver worker task */ _task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr); if (_task < 0) { @@ -275,17 +268,22 @@ void GPS::config() { int length = 0; + uint8_t send_buffer[SEND_BUFFER_LENGTH]; - _Helper->configure(_config_needed, _baudrate_changed, _baudrate, _send_buffer, length, SEND_BUFFER_LENGTH); + _Helper->configure(_config_needed, _baudrate_changed, _baudrate, send_buffer, length, SEND_BUFFER_LENGTH); - /* the message needs to be sent at the old baudrate */ + /* The config message is sent sent at the old baudrate */ if (length > 0) { - if (length != ::write(_serial_fd, _send_buffer, length)) { + + if (length != ::write(_serial_fd, send_buffer, length)) { debug("write config failed"); return; } } + /* Sometimes the baudrate needs to be changed, for instance UBX with factory settings are changed + * from 9600 to 38400 + */ if (_baudrate_changed) { set_baudrate(_baudrate); _baudrate_changed = false; @@ -304,11 +302,10 @@ GPS::task_main() log("starting"); /* open the serial port */ - _serial_fd = ::open("/dev/ttyS3", O_RDWR); + _serial_fd = ::open("/dev/ttyS3", O_RDWR); //TODO make the device dynamic depending on startup parameters - uint8_t buf[256]; - int baud_i = 0; - uint64_t time_before_configuration; + /* buffer to read from the serial port */ + uint8_t buf[32]; if (_serial_fd < 0) { log("failed to open serial port: %d", errno); @@ -326,16 +323,16 @@ GPS::task_main() /* lock against the ioctl handler */ lock(); - + unsigned baud_i = 0; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); - time_before_configuration = hrt_absolute_time(); + uint64_t time_before_configuration = hrt_absolute_time(); uint64_t last_rate_measurement = hrt_absolute_time(); unsigned last_rate_count = 0; - /* loop handling received serial bytes */ + /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { if (_mode_changed) { @@ -364,14 +361,17 @@ GPS::task_main() _mode_changed = false; } + /* If a configuration does not finish in the config timeout, change the baudrate */ if (_config_needed && time_before_configuration + CONFIG_TIMEOUT < hrt_absolute_time()) { baud_i = (baud_i+1)%NUMBER_OF_BAUDRATES; _baudrate = _baudrates_to_try[baud_i]; set_baudrate(_baudrate); + _Helper->reset(); time_before_configuration = hrt_absolute_time(); } - + /* during configuration, the timeout should be small, so that we can send config messages in between parsing, + * but during normal operation, it should never timeout because updates should arrive with 5Hz */ int poll_timeout; if (_config_needed) { poll_timeout = 50; @@ -409,11 +409,21 @@ GPS::task_main() /* pass received bytes to the packet decoder */ int j; + int ret_parse = 0; for (j = 0; j < count; j++) { - _messages_received += _Helper->parse(buf[j], &_report); + ret_parse += _Helper->parse(buf[j], &_report); } - if (_messages_received > 0) { - if (_config_needed == true) { + + if (ret_parse < 0) { + /* This means something went wrong in the parser, let's reconfigure */ + if (!_config_needed) { + _config_needed = true; + debug("Lost GPS module"); + } + config(); + } else if (ret_parse > 0) { + /* Looks like we got a valid position update, stop configuring and publish it */ + if (_config_needed) { _config_needed = false; debug("Found GPS module"); } @@ -424,8 +434,6 @@ GPS::task_main() } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } - - _messages_received = 0; last_rate_count++; /* measure update rate every 5 seconds */ @@ -435,10 +443,10 @@ GPS::task_main() last_rate_count = 0; } } + /* else if ret_parse == 0: just keep parsing */ } } } -out: debug("exiting"); ::close(_serial_fd); @@ -469,7 +477,6 @@ GPS::set_baudrate(unsigned baud) warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } - struct termios uart_config; int termios_state; @@ -482,24 +489,26 @@ GPS::set_baudrate(unsigned baud) uart_config.c_cflag &= ~(CSTOPB | PARENB); /* set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - warnx("ERROR setting config: %d (cfsetispeed, cfsetospeed)\n", termios_state); + if ((termios_state = cfsetispeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state); + return -1; + } + if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { + warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); return -1; } - - if ((termios_state = tcsetattr(_serial_fd, TCSANOW, &uart_config)) < 0) { warnx("ERROR setting baudrate (tcsetattr)\n"); return -1; } - /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ + return 0; } void GPS::cmd_reset() { - _healthy = false; + _config_needed = true; } void @@ -524,10 +533,12 @@ GPS::print_info() warnx("baudrate: %d, status: %s", _baudrate, (_config_needed) ? "NOT OK" : "OK"); if (_report.timestamp != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - ((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f)); + (double)((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); } + + usleep(100000); } /** @@ -583,6 +594,9 @@ start() errx(1, "driver start failed"); } +/** + * Stop the driver. + */ void stop() { diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index baacf7cb4725..8a6b0148b7b9 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -40,8 +40,9 @@ class GPS_Helper { public: - virtual void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned) = 0; - virtual int parse(uint8_t, struct vehicle_gps_position_s*) = 0; + virtual void reset() = 0; + virtual void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) = 0; + virtual int parse(uint8_t b, struct vehicle_gps_position_s *gps_position) = 0; }; #endif /* GPS_HELPER_H */ diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 1105e0da4051..66a891da4f19 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -48,36 +48,41 @@ UBX::UBX() : -_waited(0), _config_state(UBX_CONFIG_STATE_PRT), +_waiting_for_ack(false), _new_nav_posllh(false), _new_nav_timeutc(false), _new_nav_dop(false), _new_nav_sol(false), _new_nav_velned(false) { - decodeInit(); - _waiting_for_ack = false; + reset(); } UBX::~UBX() { } +void +UBX::reset() +{ + decodeInit(); + _config_state = UBX_CONFIG_STATE_PRT; + _waiting_for_ack = false; +} + void UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length) { - /* make sure the buffer to write the message is long enough */ + /* make sure the buffer, where the message is written to, is long enough */ assert(sizeof(type_gps_bin_cfg_prt_packet_t)+2 <= max_length); - /* try again after 10 times */ - if (_waited > 10) { - _waiting_for_ack = false; - } - + /* Only send a new config message when we got the ACK of the last one, + * otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command + * reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs. + */ if (!_waiting_for_ack) { _waiting_for_ack = true; - _waited = 0; if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { config_needed = false; length = 0; @@ -85,10 +90,15 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, _waiting_for_ack = false; return; } else if (_config_state == UBX_CONFIG_STATE_PRT) { - /* send a CFT-PRT message to define set ubx protocol and leave the baudrate as is, we just want an ACK-ACK from this */ + + /* Send a CFG-PRT message to set the UBX protocol for in and out + * and leave the baudrate as it is, we just want an ACK-ACK from this + */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; + /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); + /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; @@ -98,18 +108,20 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + /* Calculate the checksum now */ addChecksumToMessage((uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* Start with the two sync bytes */ buffer[0] = UBX_SYNC1; buffer[1] = UBX_SYNC2; + /* Copy it to the buffer that will be written back in the main gps driver */ memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); + /* Set the length of the packet (plus the 2 sync bytes) */ length = sizeof(cfg_prt_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { -// printf("Send change of baudrate now\n"); - - /* send a CFT-PRT message to define set ubx protocol and and baudrate, now let's try to switch the baudrate */ + /* Send a CFG-PRT message again, this time change the baudrate */ type_gps_bin_cfg_prt_packet_t cfg_prt_packet; memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); @@ -129,18 +141,15 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, memcpy(&(buffer[2]), &cfg_prt_packet, sizeof(cfg_prt_packet)); length = sizeof(cfg_prt_packet)+2; - /* detection when the baudrate has been changed in the first step */ + /* If the new baudrate will be different from the current one, we should report that back to the driver */ if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { - /* set a flag and exit */ baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; baudrate_changed = true; - _config_state = UBX_CONFIG_STATE_RATE; - _waiting_for_ack = false; - return; + /* Don't wait for an ACK, we're switching baudrate and we might never get, + * after that, start fresh */ + reset(); } - - } else if (_config_state == UBX_CONFIG_STATE_RATE) { /* send a CFT-RATE message to define update rate */ @@ -162,9 +171,9 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, length = sizeof(cfg_rate_packet)+2; } else if (_config_state == UBX_CONFIG_STATE_NAV5) { - /* send a NAV5 message to set the options for the internal estimator */ + /* send a NAV5 message to set the options for the internal filter */ type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); //set everything to 0 + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); cfg_nav5_packet.clsID = UBX_CLASS_CFG; cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; @@ -181,15 +190,16 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, length = sizeof(cfg_nav5_packet)+2; } else { - /* catch the remaining config states here */ + /* Catch the remaining config states here, they all need the same packet type */ type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); //set everything to 0 + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); cfg_msg_packet.clsID = UBX_CLASS_CFG; cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1; + /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; switch (_config_state) { case UBX_CONFIG_STATE_MSG_NAV_POSLLH: @@ -207,7 +217,8 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, case UBX_CONFIG_STATE_MSG_NAV_SVINFO: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - cfg_msg_packet.rate[1] = 5; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; break; case UBX_CONFIG_STATE_MSG_NAV_SOL: cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; @@ -232,16 +243,14 @@ UBX::configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, memcpy(&(buffer[2]), &cfg_msg_packet, sizeof(cfg_msg_packet)); length = sizeof(cfg_msg_packet)+2; } - } else { - _waited++; } } int -UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) +UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position) { + /* if no error happens and no new report is ready yet, ret will stay 0 */ int ret = 0; - //printf("Received char: %c\n", b); switch (_decode_state) { /* First, look for sync1 */ @@ -373,6 +382,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); decodeInit(); + ret = -1; break; } break; @@ -415,8 +425,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -441,8 +449,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -466,8 +472,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -501,8 +505,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -594,8 +596,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -615,7 +615,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) gps_position->counter++; - _new_nav_velned = true; } else { @@ -624,8 +623,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; - break; } @@ -718,7 +715,6 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) // Reset state machine to decode next packet decodeInit(); - return ret; break; } @@ -730,7 +726,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) //Check if checksum is valid if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - warnx("UBX: NO ACK"); + warnx("UBX: Received: Not Acknowledged"); ret = 1; } else { @@ -751,11 +747,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) break; } } // end if _rx_count high enough - if (_rx_count < RECV_BUFFER_SIZE) { + else if (_rx_count < RECV_BUFFER_SIZE) { _rx_count++; } else { - warnx("buffer overflow"); + warnx("buffer full, restarting"); decodeInit(); + ret = -1; } break; default: @@ -765,13 +762,16 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s* gps_position) /* 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) { + /* Add timestamp to finish the report */ gps_position->timestamp = hrt_absolute_time(); - ret = 1; + /* Reset the flags */ _new_nav_posllh = false; - // _new_nav_timeutc = false; - // _new_nav_dop = false; - // _new_nav_sol = false; - // _new_nav_velned = false; + _new_nav_timeutc = false; + _new_nav_dop = false; + _new_nav_sol = false; + _new_nav_velned = false; + + ret = 1; } return ret; @@ -807,6 +807,7 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } + /* The checksum is written to the last to bytes of a message */ message[length-2] = ck_a; message[length-1] = ck_b; } diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index dff25a518f0b..0cac10f0a310 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -40,16 +40,17 @@ #include "gps_helper.h" -#define UBX_NO_OF_MESSAGES 7 /**< Read 7 UBX GPS messages */ #define UBX_SYNC1 0xB5 #define UBX_SYNC2 0x62 -//UBX Protocol definitions (this is the subset of the messages that are parsed) +/* ClassIDs (the ones that are used) */ #define UBX_CLASS_NAV 0x01 //#define UBX_CLASS_RXM 0x02 #define UBX_CLASS_ACK 0x05 #define UBX_CLASS_CFG 0x06 + +/* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 #define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_TIMEUTC 0x21 @@ -65,11 +66,11 @@ #define UBX_MESSAGE_CFG_RATE 0x08 #define UBX_CFG_PRT_LENGTH 20 -#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< port 1 */ +#define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ #define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ -#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< ubx in */ -#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< ubx out */ +#define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ +#define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ #define UBX_CFG_RATE_LENGTH 6 #define UBX_CFG_RATE_PAYLOAD_MEASRATE 200 /**< 200ms for 5Hz */ @@ -83,30 +84,30 @@ #define UBX_CFG_NAV5_PAYLOAD_FIXMODE 2 /**< 1: 2D only, 2: 3D only, 3: Auto 2D/3D */ #define UBX_CFG_MSG_LENGTH 8 -#define UBX_CFG_MSG_PAYLOAD_RATE1 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} UART1 chosen */ - +#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ +#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ // ************ /** the structures of the binary packets */ #pragma pack(push, 1) typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t lon; // Longitude * 1e-7, deg - int32_t lat; // Latitude * 1e-7, deg - int32_t height; // Height above Ellipsoid, mm - int32_t height_msl; // Height above mean sea level, mm - uint32_t hAcc; // Horizontal Accuracy Estimate, mm - uint32_t vAcc; // Vertical Accuracy Estimate, mm + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t lon; /**< Longitude * 1e-7, deg */ + int32_t lat; /**< Latitude * 1e-7, deg */ + int32_t height; /**< Height above Ellipsoid, mm */ + int32_t height_msl; /**< Height above mean sea level, mm */ + uint32_t hAcc; /**< Horizontal Accuracy Estimate, mm */ + uint32_t vAcc; /**< Vertical Accuracy Estimate, mm */ uint8_t ck_a; uint8_t ck_b; } gps_bin_nav_posllh_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - int32_t time_nanoseconds; // Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 - int16_t week; // GPS week (GPS time) - uint8_t gpsFix; //GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + int32_t time_nanoseconds; /**< Fractional Nanoseconds remainder of rounded ms above, range -500000 .. 500000 */ + int16_t week; /**< GPS week (GPS time) */ + uint8_t gpsFix; /**< GPS Fix: 0 = No fix, 1 = Dead Reckoning only, 2 = 2D fix, 3 = 3d-fix, 4 = GPS + dead reckoning, 5 = time only fix */ uint8_t flags; int32_t ecefX; int32_t ecefY; @@ -125,50 +126,50 @@ typedef struct { } gps_bin_nav_sol_packet_t; typedef struct { - uint32_t time_milliseconds; // GPS Millisecond Time of Week - uint32_t time_accuracy; //Time Accuracy Estimate, ns - int32_t time_nanoseconds; //Nanoseconds of second, range -1e9 .. 1e9 (UTC) - uint16_t year; //Year, range 1999..2099 (UTC) - uint8_t month; //Month, range 1..12 (UTC) - uint8_t day; //Day of Month, range 1..31 (UTC) - uint8_t hour; //Hour of Day, range 0..23 (UTC) - uint8_t min; //Minute of Hour, range 0..59 (UTC) - uint8_t sec; //Seconds of Minute, range 0..59 (UTC) - uint8_t valid_flag; //Validity Flags (see ubx documentation) + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint32_t time_accuracy; /**< Time Accuracy Estimate, ns */ + int32_t time_nanoseconds; /**< Nanoseconds of second, range -1e9 .. 1e9 (UTC) */ + uint16_t year; /**< Year, range 1999..2099 (UTC) */ + uint8_t month; /**< Month, range 1..12 (UTC) */ + uint8_t day; /**< Day of Month, range 1..31 (UTC) */ + uint8_t hour; /**< Hour of Day, range 0..23 (UTC) */ + uint8_t min; /**< Minute of Hour, range 0..59 (UTC) */ + uint8_t sec; /**< Seconds of Minute, range 0..59 (UTC) */ + uint8_t valid_flag; /**< Validity Flags (see ubx documentation) */ uint8_t ck_a; 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) + 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 - uint8_t numCh; //Number of channels + uint32_t time_milliseconds; /**< GPS Millisecond Time of Week */ + uint8_t numCh; /**< Number of channels */ uint8_t globalFlags; uint16_t reserved2; } gps_bin_nav_svinfo_part1_packet_t; typedef struct { - uint8_t chn; //Channel number, 255 for SVs not assigned to a channel - uint8_t svid; //Satellite ID + uint8_t chn; /**< Channel number, 255 for SVs not assigned to a channel */ + uint8_t svid; /**< Satellite ID */ uint8_t flags; uint8_t quality; - uint8_t cno; //Carrier to Noise Ratio (Signal Strength), dbHz - int8_t elev; //Elevation in integer degrees - int16_t azim; //Azimuth in integer degrees - int32_t prRes; //Pseudo range residual in centimetres + uint8_t cno; /**< Carrier to Noise Ratio (Signal Strength), dbHz */ + int8_t elev; /**< Elevation in integer degrees */ + int16_t azim; /**< Azimuth in integer degrees */ + int32_t prRes; /**< Pseudo range residual in centimetres */ } gps_bin_nav_svinfo_part2_packet_t; @@ -192,9 +193,9 @@ typedef struct { } gps_bin_nav_velned_packet_t; //typedef struct { -// int32_t time_milliseconds; // Measurement integer millisecond GPS time of week -// int16_t week; //Measurement GPS week number -// uint8_t numVis; //Number of visible satellites +// int32_t time_milliseconds; /**< Measurement integer millisecond GPS time of week */ +// int16_t week; /**< Measurement GPS week number */ +// uint8_t numVis; /**< Number of visible satellites */ // // //... rest of package is not used in this implementation // @@ -210,7 +211,6 @@ typedef struct { typedef struct { uint8_t clsID; uint8_t msgID; - uint8_t ck_a; uint8_t ck_b; } gps_bin_ack_nak_packet_t; @@ -340,17 +340,27 @@ class UBX : public GPS_Helper public: UBX(); ~UBX(); - - void configure(bool&, bool&, unsigned&, uint8_t*, int&, const unsigned); + void reset(void); + void configure(bool &config_needed, bool &baudrate_changed, unsigned &baudrate, uint8_t *buffer, int &length, const unsigned max_length); int parse(uint8_t, struct vehicle_gps_position_s*); private: + /** + * Reset the parse state machine for a fresh start + */ void decodeInit(void); + + /** + * While parsing add every byte (except the sync bytes) to the checksum + */ void addByteToChecksum(uint8_t); + + /** + * Add the two checksum bytes to an outgoing message + */ void addChecksumToMessage(uint8_t*, const unsigned); - unsigned _waited; - bool _waiting_for_ack; ubx_config_state_t _config_state; + bool _waiting_for_ack; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count;