From df6cf142e7d67fa8c868245ff144f4e1d4ebdfb3 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 8 Feb 2013 11:05:57 -0800 Subject: Another rewrite: most of the polling, reading and writing is now inside the GPS classes --- apps/drivers/drv_gps.h | 3 +- apps/drivers/gps/gps.cpp | 62 +-- apps/drivers/gps/gps_helper.cpp | 2 + apps/drivers/gps/gps_helper.h | 4 +- apps/drivers/gps/mtk.cpp | 69 +-- apps/drivers/gps/mtk.h | 10 +- apps/drivers/gps/ubx.cpp | 930 ++++++++++++++++++++-------------------- apps/drivers/gps/ubx.h | 32 +- 8 files changed, 564 insertions(+), 548 deletions(-) diff --git a/apps/drivers/drv_gps.h b/apps/drivers/drv_gps.h index 67248efcd..1dda8ef0b 100644 --- a/apps/drivers/drv_gps.h +++ b/apps/drivers/drv_gps.h @@ -49,7 +49,8 @@ #define GPS_DEVICE_PATH "/dev/gps" typedef enum { - GPS_DRIVER_MODE_UBX = 0, + GPS_DRIVER_MODE_NONE = 0, + GPS_DRIVER_MODE_UBX, GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_NMEA, } gps_driver_mode_t; diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp index 76b5f4533..6d7cfcc68 100644 --- a/apps/drivers/gps/gps.cpp +++ b/apps/drivers/gps/gps.cpp @@ -36,10 +36,7 @@ * Driver for the GPS on a serial port */ -#include - -#include - +#include #include #include #include @@ -54,30 +51,24 @@ #include #include #include - +#include #include -#include - #include - #include - +#include #include #include #include - #include - +#include #include -//#include "ubx.h" +#include "ubx.h" #include "mtk.h" -#define SEND_BUFFER_LENGTH 100 -#define TIMEOUT 1000000 //1s -#define NUMBER_OF_TRIES 5 -#define CONFIG_TIMEOUT 2000000 +#define TIMEOUT_5HZ 400 +#define RATE_MEASUREMENT_PERIOD 5000000 /* oddly, ERROR is not defined for c++ */ #ifdef ERROR @@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) : _task_should_exit(false), _healthy(false), _mode_changed(false), - _mode(GPS_DRIVER_MODE_MTK), + _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), _rate(0.0f) @@ -279,12 +270,11 @@ GPS::task_main() } switch (_mode) { -// case GPS_DRIVER_MODE_UBX: -// _Helper = new UBX(); -// break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; case GPS_DRIVER_MODE_MTK: - printf("try new mtk\n"); - _Helper = new MTK(); + _Helper = new MTK(_serial_fd, &_report); break; case GPS_DRIVER_MODE_NMEA: //_Helper = new NMEA(); //TODO: add NMEA @@ -292,11 +282,11 @@ GPS::task_main() default: break; } - // XXX unlock/lock makes sense? unlock(); - if (_Helper->configure(_serial_fd, _baudrate) == 0) { - - while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { + if (_Helper->configure(_baudrate) == 0) { + unlock(); + while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { +// lock(); /* opportunistic publishing - else invalid data would end up on the bus */ if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); @@ -307,7 +297,7 @@ GPS::task_main() last_rate_count++; /* measure update rate every 5 seconds */ - if (hrt_absolute_time() - last_rate_measurement > 5000000) { + if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) { _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); last_rate_measurement = hrt_absolute_time(); last_rate_count = 0; @@ -323,8 +313,26 @@ GPS::task_main() _healthy = false; _rate = 0.0f; } + + lock(); } lock(); + + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; + case GPS_DRIVER_MODE_MTK: + _mode = GPS_DRIVER_MODE_UBX; + break; + // case GPS_DRIVER_MODE_NMEA: + // _mode = GPS_DRIVER_MODE_UBX; + // break; + default: + break; + } + } debug("exiting"); diff --git a/apps/drivers/gps/gps_helper.cpp b/apps/drivers/gps/gps_helper.cpp index 2caa82af1..9c1fad569 100644 --- a/apps/drivers/gps/gps_helper.cpp +++ b/apps/drivers/gps/gps_helper.cpp @@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; + warnx("try baudrate: %d\n", speed); + default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; diff --git a/apps/drivers/gps/gps_helper.h b/apps/drivers/gps/gps_helper.h index 537ca819c..f3d3bc40b 100644 --- a/apps/drivers/gps/gps_helper.h +++ b/apps/drivers/gps/gps_helper.h @@ -44,8 +44,8 @@ class GPS_Helper { public: - virtual int configure(const int &fd, unsigned &baud) = 0; - virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; + virtual int configure(unsigned &baud) = 0; + virtual int receive(unsigned timeout) = 0; int set_baudrate(const int &fd, unsigned baud); }; diff --git a/apps/drivers/gps/mtk.cpp b/apps/drivers/gps/mtk.cpp index 4ccbbfbe4..4762bd503 100644 --- a/apps/drivers/gps/mtk.cpp +++ b/apps/drivers/gps/mtk.cpp @@ -47,7 +47,9 @@ #include "mtk.h" -MTK::MTK() : +MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _mtk_revision(0) { decode_init(); @@ -58,40 +60,40 @@ MTK::~MTK() } int -MTK::configure(const int &fd, unsigned &baudrate) +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; /* Write config messages, don't wait for an answer */ - if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { + if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { + if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { + if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) { + if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) { warnx("mtk: config write failed"); return -1; } usleep(10000); - if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { + if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { warnx("mtk: config write failed"); return -1; } @@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate) } int -MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) +MTK::receive(unsigned timeout) { /* poll descriptor */ pollfd fds[1]; - fds[0].fd = fd; + fds[0].fd = _fd; fds[0].events = POLLIN; uint8_t buf[32]; gps_mtk_packet_t packet; + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + int j = 0; ssize_t count = 0; @@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) /* pass received bytes to the packet decoder */ while (j < count) { if (parse_char(buf[j], packet) > 0) { - handle_message(packet, gps_position); + handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } j++; } /* everything is read */ @@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) } /* then poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { /* something went wrong when polling */ @@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(fd, buf, sizeof(buf)); + count = ::read(_fd, buf, sizeof(buf)); } } } @@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) } void -MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) +MTK::handle_message(gps_mtk_packet_t &packet) { if (_mtk_revision == 16) { - gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 - gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 + _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 } else if (_mtk_revision == 19) { - gps_position.lat = packet.latitude; // both degrees*1e7 - gps_position.lon = packet.longitude; // both degrees*1e7 + _gps_position->lat = packet.latitude; // both degrees*1e7 + _gps_position->lon = packet.longitude; // both degrees*1e7 } else { warnx("mtk: unknown revision"); - gps_position.lat = 0; - gps_position.lon = 0; + _gps_position->lat = 0; + _gps_position->lon = 0; } - gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm - gps_position.fix_type = packet.fix_type; - gps_position.eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit - gps_position.epv_m = 0.0; //unknown in mtk custom mode - gps_position.vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s - gps_position.cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad - gps_position.satellites_visible = packet.satellites; + _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm + _gps_position->fix_type = packet.fix_type; + _gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit + _gps_position->epv_m = 0.0; //unknown in mtk custom mode + _gps_position->vel_m_s = ((float)packet.ground_speed)*1e-2f; // from cm/s to m/s + _gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad + _gps_position->satellites_visible = packet.satellites; /* convert time and date information to unix timestamp */ struct tm timeinfo; //TODO: test this conversion @@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; time_t epoch = mktime(&timeinfo); - gps_position.time_gps_usec = epoch * 1e6; //TODO: test this - gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3; - gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time(); + _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this + _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3; + _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time(); return; } diff --git a/apps/drivers/gps/mtk.h b/apps/drivers/gps/mtk.h index 39063beab..d4e390b01 100644 --- a/apps/drivers/gps/mtk.h +++ b/apps/drivers/gps/mtk.h @@ -85,10 +85,10 @@ typedef struct { class MTK : public GPS_Helper { public: - MTK(); + MTK(const int &fd, struct vehicle_gps_position_s *gps_position); ~MTK(); - int receive(const int &fd, struct vehicle_gps_position_s &gps_position); - int configure(const int &fd, unsigned &baudrate); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: /** @@ -99,7 +99,7 @@ private: /** * Handle the package once it has arrived */ - void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position); + void handle_message(gps_mtk_packet_t &packet); /** * Reset the parse state machine for a fresh start @@ -111,6 +111,8 @@ private: */ void add_byte_to_checksum(uint8_t); + int _fd; + struct vehicle_gps_position_s *_gps_position; mtk_decode_state_t _decode_state; uint8_t _mtk_revision; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp index 8e2396564..74cbc5aaf 100644 --- a/apps/drivers/gps/ubx.cpp +++ b/apps/drivers/gps/ubx.cpp @@ -37,6 +37,7 @@ #include #include +#include #include #include #include @@ -47,172 +48,251 @@ #include "ubx.h" +#define UBX_CONFIG_TIMEOUT 100 -UBX::UBX() : -_config_state(UBX_CONFIG_STATE_PRT), +UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : +_fd(fd), +_gps_position(gps_position), _waiting_for_ack(false) { - reset(); + decode_init(); } UBX::~UBX() { } -void -UBX::reset() +int +UBX::configure(unsigned &baudrate) { - decodeInit(); - _config_state = UBX_CONFIG_STATE_PRT; - _waiting_for_ack = false; -} + _waiting_for_ack = true; + + /* try different baudrates */ + const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; + + for (int baud_i = 0; baud_i < 5; baud_i++) { + baudrate = baudrates_to_try[baud_i]; + set_baudrate(_fd, baudrate); + + /* 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)); + + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_PRT; + + /* 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; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = baudrate; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } -void -UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate) -{ - /* 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; - if (_config_state == UBX_CONFIG_STATE_CONFIGURED) { - /* This should never happen, the parser should set the flag, - * if it should be reconfigured, reset() should be called first. - */ - warnx("ubx: already configured"); - _waiting_for_ack = false; - return; - } else if (_config_state == UBX_CONFIG_STATE_PRT) { - - /* 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; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = baudrate; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - } else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_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)); - - cfg_prt_packet.clsID = UBX_CLASS_CFG; - cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; - cfg_prt_packet.length = UBX_CFG_PRT_LENGTH; - cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; - cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; - cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; - cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; - cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - - sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - - /* 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) { - baudrate=UBX_CFG_PRT_PAYLOAD_BAUDRATE; - baudrate_changed = true; - /* Don't wait for an ACK, we're switching baudrate and we might never get, - * after that, start fresh */ - reset(); - } + /* Send a CFG-PRT message again, this time 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; + cfg_prt_packet.portID = UBX_CFG_PRT_PAYLOAD_PORTID; + cfg_prt_packet.mode = UBX_CFG_PRT_PAYLOAD_MODE; + cfg_prt_packet.baudRate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; + cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; + + send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { + set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); + baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; + } - } else if (_config_state == UBX_CONFIG_STATE_RATE) { + /* no ack is ecpected here, keep going configuring */ - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + /* send a CFT-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_RATE; - sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASRATE; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - } else if (_config_state == UBX_CONFIG_STATE_NAV5) { - /* 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)); + send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + /* 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)); - sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_NAV5; - } else { - /* Catch the remaining config states here, they all need the same packet type */ + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - type_gps_bin_cfg_msg_packet_t cfg_msg_packet; - memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); + send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } - cfg_msg_packet.clsID = UBX_CLASS_CFG; - cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; - cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; - /* 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; + type_gps_bin_cfg_msg_packet_t cfg_msg_packet; + memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); - switch (_config_state) { - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; - break; - case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: - 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_SVINFO: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; - /* 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; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; - cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; - break; -// case UBX_CONFIG_STATE_MSG_RXM_SVSI: -// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; -// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; -// break; - default: - break; + _clsID_needed = UBX_CLASS_CFG; + _msgID_needed = UBX_MESSAGE_CFG_MSG; + + cfg_msg_packet.clsID = UBX_CLASS_CFG; + cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; + cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; + /* 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; + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; + /* For satelites info 1Hz is enough */ + cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } + + cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; + cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; + + send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + if (receive(UBX_CONFIG_TIMEOUT) < 0) { + /* try next baudrate */ + continue; + } +// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; + +// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; +// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; + + _waiting_for_ack = false; + return 0; + } + return -1; +} + +int +UBX::receive(unsigned timeout) +{ + /* poll descriptor */ + pollfd fds[1]; + fds[0].fd = _fd; + fds[0].events = POLLIN; + + uint8_t buf[32]; + + /* timeout additional to poll */ + uint64_t time_started = hrt_absolute_time(); + + int j = 0; + ssize_t count = 0; + + while (true) { + + /* pass received bytes to the packet decoder */ + while (j < count) { + if (parse_char(buf[j]) > 0) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + if (handle_message() > 0) + return 1; + } + /* in case we keep trying but only get crap from GPS */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; } + j++; + } - sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); + /* everything is read */ + j = count = 0; + + /* then poll for new data */ + int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + + if (ret < 0) { + /* something went wrong when polling */ + return -1; + + } else if (ret == 0) { + /* Timeout */ + return -1; + + } else if (ret > 0) { + /* if we have new data from GPS, go handle it */ + if (fds[0].revents & POLLIN) { + /* + * We are here because poll says there is some data, so this + * won't block even on a blocking device. If more bytes are + * available, we'll go back to poll() again... + */ + count = ::read(_fd, buf, sizeof(buf)); + } } } } -void -UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) +int +UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ @@ -227,13 +307,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _decode_state = UBX_DECODE_GOT_SYNC2; } else { /* Second start symbol was wrong, reset state machine */ - decodeInit(); + decode_init(); } break; /* Now look for class */ case UBX_DECODE_GOT_SYNC2: /* everything except sync1 and sync2 needs to be added to the checksum */ - addByteToChecksum(b); + add_byte_to_checksum(b); /* check for known class */ switch (b) { case UBX_CLASS_ACK: @@ -256,12 +336,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_class = CFG; break; default: //unknown class: reset state machine - decodeInit(); + decode_init(); break; } break; case UBX_DECODE_GOT_CLASS: - addByteToChecksum(b); + add_byte_to_checksum(b); switch (_message_class) { case NAV: switch (b) { @@ -296,7 +376,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -308,7 +388,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ // break; // // default: //unknown class: reset state machine, should not happen -// decodeInit(); +// decode_init(); // break; // } // break; @@ -321,7 +401,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; @@ -337,384 +417,282 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_ _message_id = ACK_NAK; break; default: //unknown class: reset state machine, should not happen - decodeInit(); + decode_init(); break; } break; default: //should not happen because we set the class warnx("UBX Error, we set a class that we don't know"); - decodeInit(); - config_needed = true; + decode_init(); +// config_needed = true; break; } break; case UBX_DECODE_GOT_MESSAGEID: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size = b; //this is the first length byte _decode_state = UBX_DECODE_GOT_LENGTH1; break; case UBX_DECODE_GOT_LENGTH1: - addByteToChecksum(b); + add_byte_to_checksum(b); _payload_size += b << 8; // here comes the second byte of length _decode_state = UBX_DECODE_GOT_LENGTH2; break; case UBX_DECODE_GOT_LENGTH2: /* Add to checksum if not yet at checksum byte */ if (_rx_count < _payload_size) - addByteToChecksum(b); + add_byte_to_checksum(b); _rx_buffer[_rx_count] = b; /* once the payload has arrived, we can process the information */ if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_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->lat = packet->lat; - gps_position->lon = packet->lon; - gps_position->alt = packet->height_msl; - - gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { + return 1; + } else { + decode_init(); + return -1; + warnx("ubx: Checksum wrong"); + } - /* Add timestamp to finish the report */ - gps_position->timestamp_position = hrt_absolute_time(); + return 1; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + break; + default: + break; + } + return 0; //XXX ? +} - /* set flag to trigger publishing of new position */ - pos_updated = true; - } else { - warnx("NAV_POSLLH: checksum invalid"); - } +int +UBX::handle_message() +{ + int ret = 0; - // Reset state machine to decode next packet - decodeInit(); - break; - } + switch (_message_id) { //this enum is unique for all ids --> no need to check the class + case NAV_POSLLH: { +// printf("GOT NAV_POSLLH MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; - //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 = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m - gps_position->fix_type = packet->gpsFix; - gps_position->s_variance_m_s = packet->sAcc; - gps_position->p_variance_m = packet->pAcc; + /* Add timestamp to finish the report */ + _gps_position->timestamp_position = hrt_absolute_time(); + /* only return 1 when new position is available */ + ret = 1; + } + break; + } - gps_position->timestamp_variance = hrt_absolute_time(); + case NAV_SOL: { +// printf("GOT NAV_SOL MESSAGE\n"); + if (!_waiting_for_ack) { + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - } else { - warnx("NAV_SOL: checksum invalid"); - } + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; - // Reset state machine to decode next packet - decodeInit(); - break; - } + _gps_position->timestamp_variance = hrt_absolute_time(); + } + 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; +// case NAV_DOP: { +//// printf("GOT NAV_DOP MESSAGE\n"); +// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; // -// } else { -// warnx("NAV_DOP: checksum invalid"); -// } +// _gps_position->eph_m = packet->hDOP; +// _gps_position->epv = packet->vDOP; // -// // Reset state machine to decode next packet -// decodeInit(); -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_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) { - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - - time_t epoch = mktime(&timeinfo); - - 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_time = hrt_absolute_time(); - - } else { - warnx("NAV_TIMEUTC: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); - - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3; - - //Check if checksum is valid and then store the gps information - if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) { - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage - - uint8_t satellites_used = 0; - int i; - - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; - - - /* Write satellite information in the global storage */ - gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - - if (!unhealthy) { - if ((packet_part2->flags) & 1) { //flags is a bitfield - gps_position->satellite_used[i] = 1; - satellites_used++; - - } else { - gps_position->satellite_used[i] = 0; - } - - gps_position->satellite_snr[i] = packet_part2->cno; - gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - - } else { - gps_position->satellite_used[i] = 0; - gps_position->satellite_snr[i] = 0; - gps_position->satellite_elevation[i] = 0; - gps_position->satellite_azimuth[i] = 0; - } - - } - - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - gps_position->satellite_prn[i] = 0; - gps_position->satellite_used[i] = 0; - gps_position->satellite_snr[i] = 0; - gps_position->satellite_elevation[i] = 0; - gps_position->satellite_azimuth[i] = 0; - } - gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - gps_position->satellite_info_available = true; - } else { - gps_position->satellite_info_available = false; - } - gps_position->timestamp_satellites = hrt_absolute_time(); - - } else { - warnx("NAV_SVINFO: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - - case NAV_VELNED: { -// printf("GOT NAV_VELNED MESSAGE\n"); - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_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->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->timestamp_velocity = hrt_absolute_time(); - - } else { - warnx("NAV_VELNED: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } - -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// _gps_position->timestamp_posdilution = hrt_absolute_time(); // -// //Check if checksum is valid and the store the gps information -// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { +// _new_nav_dop = true; // -// gps_position->satellites_visible = packet->numVis; -// gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// ret = 1; -// -// } else { -// warnx("RXM_SVSI: checksum invalid\n"); -// } -// -// // Reset state machine to decode next packet -// decodeInit(); -// break; -// } - - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { - - _waiting_for_ack = false; - - switch (_config_state) { - case UBX_CONFIG_STATE_PRT: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - _config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE; - break; - case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT) - _config_state = UBX_CONFIG_STATE_RATE; - break; - case UBX_CONFIG_STATE_RATE: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE) - _config_state = UBX_CONFIG_STATE_NAV5; - break; - case UBX_CONFIG_STATE_NAV5: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5) - _config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH; - break; - case UBX_CONFIG_STATE_MSG_NAV_POSLLH: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _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_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; - break; - case UBX_CONFIG_STATE_MSG_NAV_SOL: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED; - break; - case UBX_CONFIG_STATE_MSG_NAV_VELNED: - if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) - _config_state = UBX_CONFIG_STATE_CONFIGURED; - /* set the flag to tell the driver that configuration was successful */ - config_needed = false; - break; -// case UBX_CONFIG_STATE_MSG_RXM_SVSI: -// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG) -// _config_state = UBX_CONFIG_STATE_CONFIGURED; -// break; - default: - break; - } - } else { - warnx("ACK_ACK: checksum invalid"); - } - - // Reset state machine to decode next packet - decodeInit(); - break; - } +// break; +// } - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer; + case NAV_TIMEUTC: { +// printf("GOT NAV_TIMEUTC MESSAGE\n"); - //Check if checksum is valid - if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { + if (!_waiting_for_ack) { + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - config_needed = true; - } else { - warnx("ACK_NAK: checksum invalid\n"); - } + //convert to unix timestamp + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; - // Reset state machine to decode next packet - decodeInit(); - break; - } + time_t epoch = mktime(&timeinfo); - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - decodeInit(); + _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_time = hrt_absolute_time(); + } + break; + } - break; + case NAV_SVINFO: { +// printf("GOT NAV_SVINFO MESSAGE\n"); + + if (!_waiting_for_ack) { + //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + //read checksum + const int length_part3 = 2; + char _rx_buffer_part3[length_part3]; + memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); + + //definitions needed to read numCh elements from the buffer: + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; //for temporal storage + + uint8_t satellites_used = 0; + int i; + + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* Get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + + /* Write satellite information in the global storage */ + _gps_position->satellite_prn[i] = packet_part2->svid; + + //if satellite information is healthy store the data + uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + + if (!unhealthy) { + if ((packet_part2->flags) & 1) { //flags is a bitfield + _gps_position->satellite_used[i] = 1; + satellites_used++; + + } else { + _gps_position->satellite_used[i] = 0; } - } // end if _rx_count high enough - else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; + + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + } else { - warnx("buffer full, restarting"); - decodeInit(); - config_needed = true; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; } - break; - default: + + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + } else { + _gps_position->satellite_info_available = false; + } + _gps_position->timestamp_satellites = hrt_absolute_time(); + } + break; - } - return; + } + + case NAV_VELNED: { +// printf("GOT NAV_VELNED MESSAGE\n"); + + if (!_waiting_for_ack) { + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _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->timestamp_velocity = hrt_absolute_time(); + } + + break; + } + +// case RXM_SVSI: { +// printf("GOT RXM_SVSI MESSAGE\n"); +// const int length_part1 = 7; +// char _rx_buffer_part1[length_part1]; +// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); +// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; +// +// _gps_position->satellites_visible = packet->numVis; +// _gps_position->counter++; +// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); +// +// break; +// } + case ACK_ACK: { +// printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { + ret = 1; + } + } + } + break; + + case ACK_NAK: { +// printf("GOT ACK_NAK\n"); + warnx("UBX: Received: Not Acknowledged"); + /* configuration obviously not successful */ + ret = -1; + break; + } + + default: //we don't know the message + warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); + ret = -1; + break; + } + // end if _rx_count high enough + decode_init(); + return ret; //XXX? } void -UBX::decodeInit(void) +UBX::decode_init(void) { _rx_ck_a = 0; _rx_ck_b = 0; @@ -726,14 +704,14 @@ UBX::decodeInit(void) } void -UBX::addByteToChecksum(uint8_t b) +UBX::add_byte_to_checksum(uint8_t b) { _rx_ck_a = _rx_ck_a + b; _rx_ck_b = _rx_ck_b + _rx_ck_a; } void -UBX::addChecksumToMessage(uint8_t* message, const unsigned length) +UBX::add_checksum_to_message(uint8_t* message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; @@ -749,12 +727,12 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length) } void -UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) +UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; /* Calculate the checksum now */ - addChecksumToMessage(packet, length); + add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h index c420e83b9..e3dd1c7ea 100644 --- a/apps/drivers/gps/ubx.h +++ b/apps/drivers/gps/ubx.h @@ -86,6 +86,8 @@ #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 */ +#define UBX_MAX_PAYLOAD_LENGTH 500 + // ************ /** the structures of the binary packets */ #pragma pack(push, 1) @@ -337,35 +339,49 @@ typedef enum { class UBX : public GPS_Helper { public: - UBX(); + UBX(const int &fd, struct vehicle_gps_position_s *gps_position); ~UBX(); - void reset(void); - void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); - void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated); + int receive(unsigned timeout); + int configure(unsigned &baudrate); private: + + /** + * Parse the binary MTK packet + */ + int parse_char(uint8_t b); + + /** + * Handle the package once it has arrived + */ + int handle_message(void); + /** * Reset the parse state machine for a fresh start */ - void decodeInit(void); + void decode_init(void); /** * While parsing add every byte (except the sync bytes) to the checksum */ - void addByteToChecksum(uint8_t); + void add_byte_to_checksum(uint8_t); /** * Add the two checksum bytes to an outgoing message */ - void addChecksumToMessage(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t* message, const unsigned length); /** * Helper to send a config packet */ - void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length); + void send_config_packet(const int &fd, uint8_t *packet, const unsigned length); + int _fd; + struct vehicle_gps_position_s *_gps_position; ubx_config_state_t _config_state; bool _waiting_for_ack; + uint8_t _clsID_needed; + uint8_t _msgID_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; -- cgit v1.2.3