From 1addb9f6c56f68f600acd4bdd609ef14697bda44 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Jun 2013 20:42:43 +0400 Subject: Fixed bug in UBX::configure_message_rate() --- src/drivers/gps/ubx.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b3093b0f6..f2e7ca67d 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -739,7 +739,7 @@ UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) msg.msg_class = msg_class; msg.msg_id = msg_id; msg.rate = rate; - send_message(CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); } void -- cgit v1.2.3 From d878d4756c6defe087b6c2125e74555f02cc63f9 Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 9 Jul 2013 14:25:47 +0800 Subject: Ammended UBlox driver to record SV Info, satelites_visible == satelites used. Info is recorded for all SVs, used or not. Might be useful for GPS debugging. --- src/drivers/gps/ubx.cpp | 41 ++++++++++++++++++----------------------- 1 file changed, 18 insertions(+), 23 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..b136dfc0a 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); + 1); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -539,7 +539,7 @@ UBX::handle_message() } case NAV_SVINFO: { -// printf("GOT NAV_SVINFO MESSAGE\n"); + // 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 @@ -560,7 +560,7 @@ UBX::handle_message() uint8_t satellites_used = 0; int i; - + // printf("Number of Channels: %d\n", packet_part1->numCh); for (i = 0; i < packet_part1->numCh; i++) { //for each channel /* Get satellite information from the buffer */ @@ -572,27 +572,22 @@ UBX::handle_message() _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; + //DT uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield + //DT Above is broken due to operator precedence. should be ... & (1<<4) or ... & 0x10. + //DT If an SV is unhealthy then it won't be used. + + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + // Update count of SVs used for NAV. + satellites_used++; } + + // Record info for all used channels, whether or not the SV is used for NAV, + _gps_position->satellite_used[i] = sv_used; + _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); } -- cgit v1.2.3 From dc2ef7b3c6e1ee90ba2130ed0574987d5c99a35b Mon Sep 17 00:00:00 2001 From: Darryl Taylor Date: Tue, 9 Jul 2013 17:07:11 +0800 Subject: Some cleanup of NAV_SVINFO message handler --- src/drivers/gps/ubx.cpp | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b136dfc0a..ff8945da1 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -567,28 +567,20 @@ UBX::handle_message() 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 to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; - /* Write satellite information in the global storage */ - _gps_position->satellite_prn[i] = packet_part2->svid; - - //if satellite information is healthy store the data - //DT uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield - //DT Above is broken due to operator precedence. should be ... & (1<<4) or ... & 0x10. - //DT If an SV is unhealthy then it won't be used. - - uint8_t sv_used = packet_part2->flags & 0x01; - - if (sv_used) { - // Update count of SVs used for NAV. + if ( sv_used ) { + // Count SVs used for NAV. satellites_used++; } - // Record info for all used channels, whether or not the SV is used for NAV, + // Record info for all channels, whether or not the SV is used for NAV. _gps_position->satellite_used[i] = sv_used; _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); - + _gps_position->satellite_prn[i] = packet_part2->svid; } for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused -- cgit v1.2.3 From e3bb9e87e2af2f0f70e486405e2c6df5d3e23667 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 17:36:24 +0200 Subject: Hotfix for GPS: Disable unknown message classes --- src/drivers/gps/ubx.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index f2e7ca67d..b46089073 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,7 +176,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 1); + 0); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -327,6 +327,7 @@ UBX::parse_char(uint8_t b) } break; case UBX_DECODE_GOT_CLASS: + { add_byte_to_checksum(b); switch (_message_class) { case NAV: @@ -413,6 +414,14 @@ UBX::parse_char(uint8_t b) // config_needed = true; break; } + // Evaluate state machine - if the state changed, + // the state machine was reset via decode_init() + // and we want to tell the module to stop sending this message + + // disable unknown message + warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + configure_message_rate(_message_class, b, 0); + } break; case UBX_DECODE_GOT_MESSAGEID: add_byte_to_checksum(b); -- cgit v1.2.3 From 6cbbb9b99fbeddc2da00f67bb209d33971a30041 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:46:24 +0200 Subject: Hotfix for GPS driver --- src/drivers/gps/ubx.cpp | 51 +++++++++++++++++++++++++------------------------ 1 file changed, 26 insertions(+), 25 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b46089073..3e790499b 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -176,17 +176,17 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - 1); + UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -196,7 +196,7 @@ UBX::configure(unsigned &baudrate) // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - 0); + UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); // /* insist of receiving the ACK for this packet */ // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) // continue; @@ -224,35 +224,18 @@ UBX::receive(unsigned timeout) fds[0].fd = _fd; fds[0].events = POLLIN; - uint8_t buf[32]; + uint8_t buf[128]; /* 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++; - } + bool message_found = false; - /* everything is read */ - j = count = 0; + while (true) { - /* then poll for new data */ + /* poll for new data */ int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); if (ret < 0) { @@ -272,8 +255,26 @@ UBX::receive(unsigned timeout) * available, we'll go back to poll() again... */ count = ::read(_fd, buf, sizeof(buf)); + /* pass received bytes to the packet decoder */ + for (int i = 0; i < count; i++) { + if (parse_char(buf[i])) { + /* return to configure during configuration or to the gps driver during normal work + * if a packet has arrived */ + handle_message(); + message_found = true; + } + } } } + + /* return success after receiving a packet */ + if (message_found) + return 1; + + /* abort after timeout if no packet parsed successfully */ + if (time_started + timeout*1000 < hrt_absolute_time() ) { + return -1; + } } } -- cgit v1.2.3 From c3d07030dd1882c626ed027cfc5870f42b1cd33e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 9 Jul 2013 22:56:02 +0200 Subject: Minor additions to fix, pushing --- src/drivers/gps/ubx.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 3e790499b..ad219cd25 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -231,7 +231,7 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool message_found = false; + bool position_updated = false; while (true) { @@ -260,15 +260,15 @@ UBX::receive(unsigned timeout) if (parse_char(buf[i])) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - handle_message(); - message_found = true; + if (handle_message()) + position_updated = true; } } } } /* return success after receiving a packet */ - if (message_found) + if (position_updated) return 1; /* abort after timeout if no packet parsed successfully */ @@ -420,8 +420,8 @@ UBX::parse_char(uint8_t b) // and we want to tell the module to stop sending this message // disable unknown message - warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - configure_message_rate(_message_class, b, 0); + //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); + //configure_message_rate(_message_class, b, 0); } break; case UBX_DECODE_GOT_MESSAGEID: -- cgit v1.2.3 From 4685871c83e5175e58da08a68b49642daf79267d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 11 Jul 2013 15:28:56 +0400 Subject: Major ubx driver cleanup: few pages of code removed, send update only when full navigation solution received --- src/drivers/gps/ubx.cpp | 841 ++++++++++++++++++++++-------------------------- src/drivers/gps/ubx.h | 61 +--- 2 files changed, 392 insertions(+), 510 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 762c257aa..895d16437 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,12 +57,14 @@ #include "ubx.h" #define UBX_CONFIG_TIMEOUT 100 +#define UBX_PACKET_TIMEOUT 2 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_waiting_for_ack(false), -_disable_cmd_counter(0) + _fd(fd), + _gps_position(gps_position), + _configured(false), + _waiting_for_ack(false), + _disable_cmd_counter(0) { decode_init(); } @@ -73,12 +76,13 @@ UBX::~UBX() int UBX::configure(unsigned &baudrate) { - _waiting_for_ack = true; - + _configured = false; /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < 5; baud_i++) { + int baud_i; + + for (baud_i = 0; baud_i < 5; baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); @@ -89,8 +93,8 @@ UBX::configure(unsigned &baudrate) /* 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; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_PRT; /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; @@ -102,9 +106,9 @@ UBX::configure(unsigned &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)); + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } @@ -120,100 +124,125 @@ UBX::configure(unsigned &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)); - + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - receive(UBX_CONFIG_TIMEOUT); - + wait_for_ack(UBX_CONFIG_TIMEOUT); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } - /* 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)); + /* at this point we have correct baudrate on both ends */ + break; + } - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_RATE; + if (baud_i >= 5) { + return 1; + } - 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_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + /* send a CFG-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_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_RATE; - /* 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)); + 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_MEASINTERVAL; + 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_NAV5; + send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); - 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; + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: RATE"); + return 1; + } - send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } + /* 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)); - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP, - // 0); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - - _waiting_for_ack = false; - return 0; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_NAV5; + + 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_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: NAV5"); + return 1; } - return -1; + + /* configure message rates */ + /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SOL\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 10); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + return 1; + } + + _configured = true; + return 0; } int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - int ret = receive(timeout); - _waiting_for_ack = false; - return ret; + uint64_t time_started = hrt_absolute_time(); + + while (hrt_absolute_time() < time_started + timeout * 1000) { + if (receive(timeout) > 0) { + if (!_waiting_for_ack) { + return 1; + } + + } else { + return -1; // timeout or error receiving, or NAK + } + } + + return -1; // timeout } int @@ -231,20 +260,20 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool position_updated = false; + bool handled = false; while (true) { - /* poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ return -1; } else if (ret == 0) { - /* Timeout */ - return -1; + /* return success after short delay after receiving a packet or timeout after long delay */ + return handled ? 1 : -1; } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -254,25 +283,22 @@ UBX::receive(unsigned timeout) * 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)); - /* pass received bytes to the packet decoder */ + count = read(_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i])) { + if (parse_char(buf[i]) > 0) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - if (handle_message()) - position_updated = true; + if (handle_message() > 0) + handled = true; } } } } - /* return success after receiving a packet */ - if (position_updated) - return 1; - - /* abort after timeout if no packet parsed successfully */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + /* abort after timeout if no useful packets received */ + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } } @@ -283,406 +309,299 @@ UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - } - break; + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + + break; + /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - } else { - /* Second start symbol was wrong, reset state machine */ - decode_init(); - } - break; + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + + } else { + /* Second start symbol was wrong, reset state machine */ + decode_init(); + /* don't return error, it can be just false sync1 */ + } + + break; + /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ - add_byte_to_checksum(b); - /* check for known class */ - switch (b) { - case UBX_CLASS_ACK: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = ACK; - break; + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + add_byte_to_checksum(b); + _message_class = b; + _decode_state = UBX_DECODE_GOT_CLASS; + break; - case UBX_CLASS_NAV: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = NAV; - break; + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + _message_id = b; + _decode_state = UBX_DECODE_GOT_MESSAGEID; + break; -// case UBX_CLASS_RXM: -// _decode_state = UBX_DECODE_GOT_CLASS; -// _message_class = RXM; -// break; + case UBX_DECODE_GOT_MESSAGEID: + add_byte_to_checksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; - case UBX_CLASS_CFG: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = CFG; - break; - default: //unknown class: reset state machine - decode_init(); - break; - } - break; - case UBX_DECODE_GOT_CLASS: - { + case UBX_DECODE_GOT_LENGTH1: + 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) add_byte_to_checksum(b); - switch (_message_class) { - case NAV: - switch (b) { - case UBX_MESSAGE_NAV_POSLLH: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_POSLLH; - break; - - case UBX_MESSAGE_NAV_SOL: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SOL; - break; - - case UBX_MESSAGE_NAV_TIMEUTC: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_TIMEUTC; - break; - -// case UBX_MESSAGE_NAV_DOP: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = NAV_DOP; -// break; - - case UBX_MESSAGE_NAV_SVINFO: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SVINFO; - break; - - case UBX_MESSAGE_NAV_VELNED: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_VELNED; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; -// case RXM: -// switch (b) { -// case UBX_MESSAGE_RXM_SVSI: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = RXM_SVSI; -// break; -// -// default: //unknown class: reset state machine, should not happen -// decode_init(); -// break; -// } -// break; - - case CFG: - switch (b) { - case UBX_MESSAGE_CFG_NAV5: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = CFG_NAV5; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - case ACK: - switch (b) { - case UBX_MESSAGE_ACK_ACK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_ACK; - break; - case UBX_MESSAGE_ACK_NAK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_NAK; - break; - default: //unknown class: reset state machine, should not happen - 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"); - decode_init(); -// config_needed = true; - break; - } - // Evaluate state machine - if the state changed, - // the state machine was reset via decode_init() - // and we want to tell the module to stop sending this message + _rx_buffer[_rx_count] = b; - // disable unknown message - //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - //configure_message_rate(_message_class, b, 0); + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + decode_init(); + return 1; // message received successfully + + } else { + warnx("ubx: checksum wrong"); + decode_init(); + return -1; } - break; - case UBX_DECODE_GOT_MESSAGEID: - 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: - 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) - 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 - - /* 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"); - } - return 1; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - } else { - warnx("ubx: buffer full"); - decode_init(); - return -1; - } - break; - default: - break; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; + + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } + + break; + + default: + break; } - return 0; //XXX ? + + return 0; // message decoding in progress } + int UBX::handle_message() { int ret = 0; - 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; + if (_configured) { + /* handle only info messages when configured */ + switch (_message_class) { + case UBX_CLASS_NAV: + switch (_message_id) { + case UBX_MESSAGE_NAV_POSLLH: { + // printf("GOT NAV_POSLLH\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; + _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 + _gps_position->timestamp_position = hrt_absolute_time(); - _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 + _rate_count_lat_lon++; - _rate_count_lat_lon++; + ret = 1; + break; + } - /* Add timestamp to finish the report */ - _gps_position->timestamp_position = hrt_absolute_time(); - /* only return 1 when new position is available */ - ret = 1; - } - break; - } + case UBX_MESSAGE_NAV_SOL: { + // printf("GOT NAV_SOL\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - 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; + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + _gps_position->timestamp_variance = hrt_absolute_time(); - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; + ret = 1; + break; + } - _gps_position->timestamp_variance = hrt_absolute_time(); - } - break; - } + case UBX_MESSAGE_NAV_TIMEUTC: { + // printf("GOT NAV_TIMEUTC\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// _gps_position->eph_m = packet->hDOP; -// _gps_position->epv = packet->vDOP; -// -// _gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; -// -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\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; + time_t epoch = mktime(&timeinfo); - if (!_waiting_for_ack) { - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + _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(); + + ret = 1; + break; + } - //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; + case UBX_MESSAGE_NAV_SVINFO: { + // printf("GOT NAV_SVINFO\n"); + // TODO avoid memcpy + 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; - time_t epoch = mktime(&timeinfo); + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; // for temporal storage - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + uint8_t satellites_used = 0; + int i; - _gps_position->timestamp_time = hrt_absolute_time(); - } - break; - } + // printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { //for each channel - case NAV_SVINFO: { - // printf("GOT NAV_SVINFO MESSAGE\n"); + /* 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; - 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; - // printf("Number of Channels: %d\n", packet_part1->numCh); - 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 to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if ( sv_used ) { - // Count SVs used for NAV. - satellites_used++; + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + // Count SVs used for NAV. + satellites_used++; + } + + // Record info for all channels, whether or not the SV is used for NAV. + _gps_position->satellite_used[i] = sv_used; + _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); + _gps_position->satellite_prn[i] = packet_part2->svid; } - - // Record info for all channels, whether or not the SV is used for NAV. - _gps_position->satellite_used[i] = sv_used; - _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); - _gps_position->satellite_prn[i] = packet_part2->svid; - } - 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; + 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(); + + ret = 1; + break; } - _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; + case UBX_MESSAGE_NAV_VELNED: { + // printf("GOT NAV_VELNED\n"); + 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; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; + + ret = 1; + break; } - _gps_position->timestamp_satellites = hrt_absolute_time(); + + default: + break; } break; - } - case NAV_VELNED: { - - if (!_waiting_for_ack) { - /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ - 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; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); - - _rate_count_vel++; + case UBX_CLASS_ACK: { + /* ignore ACK when already configured */ + ret = 1; + break; } + default: 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; - } + if (ret == 0) { + /* message not handled */ + warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + + if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + /* don't attempt for every message to disable, some might not be disabled */ + warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + configure_message_rate(_message_class, _message_id, 0); } } - break; - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - ret = -1; - break; - } + } else { + /* handle only ACK while configuring */ + if (_message_class == UBX_CLASS_ACK) { + switch (_message_id) { + case UBX_MESSAGE_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 == _message_class_needed && packet->msgID == _message_id_needed) { + _waiting_for_ack = false; + ret = 1; + } + } - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - if (_disable_cmd_counter++ == 0) { - // Don't attempt for every message to disable, some might not be disabled */ - warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id); - configure_message_rate(_message_class, _message_id, 0); + break; + } + + case UBX_MESSAGE_ACK_NAK: { + // printf("GOT ACK_NAK\n"); + warnx("ubx: not acknowledged"); + /* configuration obviously not successful */ + _waiting_for_ack = false; + ret = -1; + break; + } + + default: + break; } - return ret; - ret = -1; - break; } - // end if _rx_count high enough + } + decode_init(); - return ret; //XXX? + return ret; } void @@ -692,9 +611,8 @@ UBX::decode_init(void) _rx_ck_b = 0; _rx_count = 0; _decode_state = UBX_DECODE_UNINIT; - _message_class = CLASS_UNKNOWN; - _message_id = ID_UNKNOWN; _payload_size = 0; + /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ } void @@ -705,23 +623,24 @@ UBX::add_byte_to_checksum(uint8_t b) } void -UBX::add_checksum_to_message(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; unsigned i; - for (i = 0; i < length-2; i++) { + for (i = 0; i < length - 2; i++) { 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; + message[length - 2] = ck_a; + message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) { for (unsigned i = 0; i < length; i++) { ck_a = ck_a + message[i]; @@ -732,11 +651,11 @@ UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_ void UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + struct ubx_cfg_msg_rate msg; + msg.msg_class = msg_class; + msg.msg_id = msg_id; + msg.rate = rate; + send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); } void @@ -761,19 +680,19 @@ void UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) { struct ubx_header header; - uint8_t ck_a=0, ck_b=0; + uint8_t ck_a = 0, ck_b = 0; header.sync1 = UBX_SYNC1; header.sync2 = UBX_SYNC2; header.msg_class = msg_class; header.msg_id = msg_id; header.length = size; - add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); + add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); // Configure receive check - _clsID_needed = msg_class; - _msgID_needed = msg_id; + _message_class_needed = msg_class; + _message_id_needed = msg_id; write(_fd, (const char *)&header, sizeof(header)); write(_fd, (const char *)msg, size); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5a433642c..4fc276975 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -3,6 +3,7 @@ * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,23 +52,23 @@ /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 //#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 +#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_CFG_PRT_LENGTH 20 #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_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ #define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ @@ -298,44 +299,6 @@ struct ubx_cfg_msg_rate { // END the structures of the binary packets // ************ -typedef enum { - UBX_CONFIG_STATE_PRT = 0, - UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, - UBX_CONFIG_STATE_RATE, - UBX_CONFIG_STATE_NAV5, - UBX_CONFIG_STATE_MSG_NAV_POSLLH, - UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, - UBX_CONFIG_STATE_MSG_NAV_DOP, - UBX_CONFIG_STATE_MSG_NAV_SVINFO, - UBX_CONFIG_STATE_MSG_NAV_SOL, - UBX_CONFIG_STATE_MSG_NAV_VELNED, -// UBX_CONFIG_STATE_MSG_RXM_SVSI, - UBX_CONFIG_STATE_CONFIGURED -} ubx_config_state_t; - -typedef enum { - CLASS_UNKNOWN = 0, - NAV = 1, - RXM = 2, - ACK = 3, - CFG = 4 -} ubx_message_class_t; - -typedef enum { - //these numbers do NOT correspond to the message id numbers of the ubx protocol - ID_UNKNOWN = 0, - NAV_POSLLH, - NAV_SOL, - NAV_TIMEUTC, -// NAV_DOP, - NAV_SVINFO, - NAV_VELNED, -// RXM_SVSI, - CFG_NAV5, - ACK_ACK, - ACK_NAK, -} ubx_message_id_t; - typedef enum { UBX_DECODE_UNINIT = 0, UBX_DECODE_GOT_SYNC1, @@ -401,17 +364,17 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; - ubx_config_state_t _config_state; + bool _configured; bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; + uint8_t _message_class_needed; + uint8_t _message_id_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; uint8_t _rx_ck_b; - ubx_message_class_t _message_class; - ubx_message_id_t _message_id; + uint8_t _message_class; + uint8_t _message_id; unsigned _payload_size; uint8_t _disable_cmd_counter; }; -- cgit v1.2.3 From 0ccf50bca367d39f75a7a4f76f98dec7352ef3d5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 11 Jul 2013 18:17:36 +0400 Subject: ubx: SVINFO parsing optimized and message rate increased, CPU consumption reduced in 6 times, ~0.3% now. --- src/drivers/gps/ubx.cpp | 42 +++++++++++++++++++----------------------- 1 file changed, 19 insertions(+), 23 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 895d16437..b579db715 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -56,8 +56,9 @@ #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 100 -#define UBX_PACKET_TIMEOUT 2 +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), @@ -214,7 +215,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 10); + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); @@ -280,9 +281,11 @@ UBX::receive(unsigned timeout) 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... + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If more bytes are available, we'll go back to poll() again. */ + usleep(UBX_WAIT_BEFORE_READ * 1000); count = read(_fd, buf, sizeof(buf)); /* pass received bytes to the packet decoder */ @@ -459,45 +462,39 @@ UBX::handle_message() } case UBX_MESSAGE_NAV_SVINFO: { - // printf("GOT NAV_SVINFO\n"); - // TODO avoid memcpy + //printf("GOT NAV_SVINFO\n"); 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; - + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_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; - // printf("Number of Channels: %d\n", packet_part1->numCh); - 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; + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); /* write satellite information to global storage */ uint8_t sv_used = packet_part2->flags & 0x01; if (sv_used) { - // Count SVs used for NAV. + /* count SVs used for NAV */ satellites_used++; } - // Record info for all channels, whether or not the SV is used for NAV. + /* record info for all channels, whether or not the SV is used for NAV */ _gps_position->satellite_used[i] = sv_used; _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); _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ + for (i = packet_part1->numCh; i < 20; i++) { + /* 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; @@ -507,7 +504,6 @@ UBX::handle_message() _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; -- cgit v1.2.3 From f2f66432d76dad2587d3e8089d0c728eb8fdf2ae Mon Sep 17 00:00:00 2001 From: tstellanova Date: Wed, 28 Aug 2013 08:49:21 -0700 Subject: Grab UTC time from GPS --- src/drivers/gps/ubx.cpp | 22 ++++++++++++++++------ 1 file changed, 16 insertions(+), 6 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index b579db715..ba5d14cc4 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -42,13 +42,14 @@ * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ - -#include -#include -#include +#include #include +#include +#include #include -#include +#include +#include + #include #include #include @@ -452,7 +453,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + +#ifndef CONFIG_RTC + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME,&ts); +#endif + _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(); -- cgit v1.2.3 From 8d6a47546a32e6911a0f769070511baa6549ed03 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 08:59:04 +0200 Subject: gps: fixed code style, more informative and clean messages --- src/drivers/gps/gps.cpp | 116 +++++++++++++++++++++++++++-------------- src/drivers/gps/gps_helper.cpp | 7 ++- src/drivers/gps/gps_helper.h | 2 +- src/drivers/gps/mtk.cpp | 26 ++++++--- src/drivers/gps/ubx.cpp | 58 ++++++++++++--------- src/drivers/gps/ubx.h | 6 +-- 6 files changed, 142 insertions(+), 73 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 38835418b..dd21fe61d 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char* uart_path); + GPS(const char *uart_path); virtual ~GPS(); virtual int init(); @@ -156,7 +156,7 @@ GPS *g_dev; } -GPS::GPS(const char* uart_path) : +GPS::GPS(const char *uart_path) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -192,6 +192,7 @@ GPS::~GPS() /* well, kill it anyway, though this will probably crash */ if (_task != -1) task_delete(_task); + g_dev = nullptr; } @@ -270,19 +271,24 @@ GPS::task_main() } switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; + + case GPS_DRIVER_MODE_NMEA: + //_Helper = new NMEA(); //TODO: add NMEA + break; + + default: + break; } + unlock(); + if (_Helper->configure(_baudrate) == 0) { unlock(); @@ -294,6 +300,7 @@ GPS::task_main() /* 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); + } else { _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); } @@ -310,10 +317,30 @@ GPS::task_main() } if (!_healthy) { - warnx("module found"); + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "ubx"; + break; + + case GPS_DRIVER_MODE_MTK: + mode_str = "mtk"; + break; + + case GPS_DRIVER_MODE_NMEA: + mode_str = "nmea"; + break; + + default: + break; + } + + warnx("module found: %s", mode_str); _healthy = true; } } + if (_healthy) { warnx("module lost"); _healthy = false; @@ -322,24 +349,28 @@ GPS::task_main() 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; + 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"); ::close(_serial_fd); @@ -361,22 +392,27 @@ void GPS::print_info() { switch (_mode) { - case GPS_DRIVER_MODE_UBX: - warnx("protocol: UBX"); - break; - case GPS_DRIVER_MODE_MTK: - warnx("protocol: MTK"); - break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: - break; + case GPS_DRIVER_MODE_UBX: + warnx("protocol: UBX"); + break; + + case GPS_DRIVER_MODE_MTK: + warnx("protocol: MTK"); + break; + + case GPS_DRIVER_MODE_NMEA: + warnx("protocol: NMEA"); + break; + + default: + break; } + warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); + if (_report.timestamp_position != 0) { warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); warnx("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()); @@ -428,6 +464,7 @@ start(const char *uart_path) errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH); goto fail; } + exit(0); fail: @@ -503,7 +540,7 @@ gps_main(int argc, char *argv[]) { /* set to default */ - char* device_name = GPS_DEFAULT_UART_PORT; + char *device_name = GPS_DEFAULT_UART_PORT; /* * Start/load the driver. @@ -513,15 +550,18 @@ gps_main(int argc, char *argv[]) if (argc > 3) { if (!strcmp(argv[2], "-d")) { device_name = argv[3]; + } else { goto out; } } + gps::start(device_name); } if (!strcmp(argv[1], "stop")) gps::stop(); + /* * Test the driver/device. */ diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index ba86d370a..2e2cbc8dd 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -87,13 +87,15 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) case 115200: speed = B115200; break; - warnx("try baudrate: %d\n", speed); + warnx("try baudrate: %d\n", speed); default: warnx("ERROR: Unsupported baudrate: %d\n", baud); return -EINVAL; } + struct termios uart_config; + int termios_state; /* fill the struct for the new configuration */ @@ -109,14 +111,17 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud) 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(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; } diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index defc1a074..73d4b889c 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -33,7 +33,7 @@ * ****************************************************************************/ -/** +/** * @file gps_helper.h */ diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 62941d74b..56b702ea6 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -48,9 +48,9 @@ MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_mtk_revision(0) + _fd(fd), + _gps_position(gps_position), + _mtk_revision(0) { decode_init(); } @@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate) warnx("mtk: config write failed"); return -1; } + usleep(10000); 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))) { warnx("mtk: config write failed"); return -1; } + usleep(10000); 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))) { @@ -128,12 +132,15 @@ MTK::receive(unsigned timeout) handle_message(packet); return 1; } + /* in case we keep trying but only get crap from GPS */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } + j++; } + /* everything is read */ j = count = 0; } @@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) if (b == MTK_SYNC1_V16) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 16; + } else if (b == MTK_SYNC1_V19) { _decode_state = MTK_DECODE_GOT_CK_A; _mtk_revision = 19; @@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) add_byte_to_checksum(b); // Fill packet buffer - ((uint8_t*)(&packet))[_rx_count] = b; + ((uint8_t *)(&packet))[_rx_count] = b; _rx_count++; /* Packet size minus checksum, XXX ? */ @@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet) /* Compare checksum */ if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { ret = 1; + } else { warnx("MTK Checksum invalid"); ret = -1; } + // Reset state machine to decode next packet decode_init(); } } + return ret; } @@ -226,19 +237,22 @@ 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 + } else if (_mtk_revision == 19) { _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->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->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; diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index ba5d14cc4..0b25b379f 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -60,13 +60,14 @@ #define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK #define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received #define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls +#define DISABLE_MSG_INTERVAL 1000000 // us, try to disable message with this interval UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), _gps_position(gps_position), _configured(false), _waiting_for_ack(false), - _disable_cmd_counter(0) + _disable_cmd_last(0) { decode_init(); } @@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate) configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + warnx("ubx: msg rate configuration failed: NAV POSLLH"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + warnx("ubx: msg rate configuration failed: NAV TIMEUTC"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SOL\n"); + warnx("ubx: msg rate configuration failed: NAV SOL"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + warnx("ubx: msg rate configuration failed: NAV VELNED"); return 1; } configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + warnx("ubx: msg rate configuration failed: NAV SVINFO"); return 1; } @@ -271,11 +272,18 @@ UBX::receive(unsigned timeout) if (ret < 0) { /* something went wrong when polling */ + warnx("ubx: poll error"); return -1; } else if (ret == 0) { /* return success after short delay after receiving a packet or timeout after long delay */ - return handled ? 1 : -1; + if (handled) { + return 1; + + } else { + warnx("ubx: timeout - no messages"); + return -1; + } } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -292,8 +300,6 @@ UBX::receive(unsigned timeout) /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { if (parse_char(buf[i]) > 0) { - /* return to configure during configuration or to the gps driver during normal work - * if a packet has arrived */ if (handle_message() > 0) handled = true; } @@ -303,6 +309,7 @@ UBX::receive(unsigned timeout) /* abort after timeout if no useful packets received */ if (time_started + timeout * 1000 < hrt_absolute_time()) { + warnx("ubx: timeout - no useful messages"); return -1; } } @@ -453,16 +460,16 @@ UBX::handle_message() timeinfo.tm_min = packet->min; timeinfo.tm_sec = packet->sec; time_t epoch = mktime(&timeinfo); - + #ifndef CONFIG_RTC - //Since we lack a hardware RTC, set the system time clock based on GPS UTC - //TODO generalize this by moving into gps.cpp? - timespec ts; - ts.tv_sec = epoch; - ts.tv_nsec = packet->time_nanoseconds; - clock_settime(CLOCK_REALTIME,&ts); + //Since we lack a hardware RTC, set the system time clock based on GPS UTC + //TODO generalize this by moving into gps.cpp? + timespec ts; + ts.tv_sec = epoch; + ts.tv_nsec = packet->time_nanoseconds; + clock_settime(CLOCK_REALTIME, &ts); #endif - + _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(); @@ -564,10 +571,13 @@ UBX::handle_message() if (ret == 0) { /* message not handled */ - warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + warnx("ubx: unknown message received: 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); + + hrt_abstime t = hrt_absolute_time(); - if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) { /* don't attempt for every message to disable, some might not be disabled */ + _disable_cmd_last = t; warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); configure_message_rate(_message_class, _message_id, 0); } @@ -640,7 +650,7 @@ UBX::add_checksum_to_message(uint8_t *message, const unsigned length) ck_b = ck_b + ck_a; } - /* The checksum is written to the last to bytes of a message */ + /* the checksum is written to the last to bytes of a message */ message[length - 2] = ck_a; message[length - 1] = ck_b; } @@ -669,17 +679,17 @@ UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length) { ssize_t ret = 0; - /* Calculate the checksum now */ + /* calculate the checksum now */ add_checksum_to_message(packet, length); const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; - /* Start with the two sync bytes */ + /* start with the two sync bytes */ ret += write(fd, sync_bytes, sizeof(sync_bytes)); ret += write(fd, packet, length); if (ret != (int)length + (int)sizeof(sync_bytes)) // XXX is there a neater way to get rid of the unsigned signed warning? - warnx("ubx: config write fail"); + warnx("ubx: configuration write fail"); } void @@ -696,7 +706,7 @@ UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); - // Configure receive check + /* configure ACK check */ _message_class_needed = msg_class; _message_id_needed = msg_id; diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 4fc276975..76ef873a3 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -347,7 +347,7 @@ private: /** * Add the two checksum bytes to an outgoing message */ - void add_checksum_to_message(uint8_t* message, const unsigned length); + void add_checksum_to_message(uint8_t *message, const unsigned length); /** * Helper to send a config packet @@ -358,7 +358,7 @@ private: void send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size); - void add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); + void add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b); int wait_for_ack(unsigned timeout); @@ -376,7 +376,7 @@ private: uint8_t _message_class; uint8_t _message_id; unsigned _payload_size; - uint8_t _disable_cmd_counter; + uint8_t _disable_cmd_last; }; #endif /* UBX_H_ */ -- cgit v1.2.3 From 42f930908fc3f60e6305257f25b625830a020a80 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 21 Sep 2013 10:13:50 +0200 Subject: gps: more cleanup, some more info in 'gps status' --- src/drivers/gps/gps.cpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index dd21fe61d..a84cb8e59 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -321,15 +321,15 @@ GPS::task_main() switch (_mode) { case GPS_DRIVER_MODE_UBX: - mode_str = "ubx"; + mode_str = "UBX"; break; case GPS_DRIVER_MODE_MTK: - mode_str = "mtk"; + mode_str = "MTK"; break; case GPS_DRIVER_MODE_NMEA: - mode_str = "nmea"; + mode_str = "NMEA"; break; default: @@ -371,7 +371,7 @@ GPS::task_main() } - debug("exiting"); + warnx("exiting"); ::close(_serial_fd); @@ -411,9 +411,10 @@ GPS::print_info() warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); if (_report.timestamp_position != 0) { - warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, - (double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f)); + warnx("position lock: %dD, satellites: %d, last update: %fms ago", (int)_report.fix_type, + _report.satellites_visible, (hrt_absolute_time() - _report.timestamp_position) / 1000.0f); warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt); + warnx("eph: %.2fm, epv: %.2fm", _report.eph_m, _report.epv_m); 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()); warnx("rate publication:\t%6.2f Hz", (double)_rate); -- cgit v1.2.3 From 9eb4e05c9dd1c9116fbe58da76fa5f750cc8911a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 22 Sep 2013 12:04:30 +0200 Subject: Hotfix: Silence GPS driver if no GPS is connected --- src/drivers/drv_gps.h | 3 +-- src/drivers/gps/gps.cpp | 15 --------------- src/drivers/gps/ubx.cpp | 1 - 3 files changed, 1 insertion(+), 18 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h index 1dda8ef0b..398cf4870 100644 --- a/src/drivers/drv_gps.h +++ b/src/drivers/drv_gps.h @@ -51,8 +51,7 @@ typedef enum { GPS_DRIVER_MODE_NONE = 0, GPS_DRIVER_MODE_UBX, - GPS_DRIVER_MODE_MTK, - GPS_DRIVER_MODE_NMEA, + GPS_DRIVER_MODE_MTK } gps_driver_mode_t; diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a84cb8e59..fc500a9ec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -279,10 +279,6 @@ GPS::task_main() _Helper = new MTK(_serial_fd, &_report); break; - case GPS_DRIVER_MODE_NMEA: - //_Helper = new NMEA(); //TODO: add NMEA - break; - default: break; } @@ -328,10 +324,6 @@ GPS::task_main() mode_str = "MTK"; break; - case GPS_DRIVER_MODE_NMEA: - mode_str = "NMEA"; - break; - default: break; } @@ -362,9 +354,6 @@ GPS::task_main() _mode = GPS_DRIVER_MODE_UBX; break; - // case GPS_DRIVER_MODE_NMEA: - // _mode = GPS_DRIVER_MODE_UBX; - // break; default: break; } @@ -400,10 +389,6 @@ GPS::print_info() warnx("protocol: MTK"); break; - case GPS_DRIVER_MODE_NMEA: - warnx("protocol: NMEA"); - break; - default: break; } diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 0b25b379f..86291901c 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -281,7 +281,6 @@ UBX::receive(unsigned timeout) return 1; } else { - warnx("ubx: timeout - no messages"); return -1; } -- cgit v1.2.3 From f205c07c084fd1008f201518d84c64718e7df9cc Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 10 Jan 2014 22:38:12 +0100 Subject: very simple gps fix fake in gps driver only for development --- src/drivers/gps/gps.cpp | 23 +++++++++++++++++++++++ 1 file changed, 23 insertions(+) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index fc500a9ec..1afb279fe 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -294,6 +294,29 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ +//#define FAKEGPS +#ifdef FAKEGPS + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 20.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated +#endif if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); -- cgit v1.2.3 From a522c64fee57c8e5e0cd188e589c8bcf87b5396d Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:08:02 +0100 Subject: fake gps: gps device is not needed for fake position generation --- src/drivers/gps/gps.cpp | 57 +++++++++++++++++++++++++++++-------------------- 1 file changed, 34 insertions(+), 23 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 1afb279fe..3649a41d6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -264,6 +264,39 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { +//#define FAKEGPS +#ifdef FAKEGPS + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 20.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } + + usleep(2e5); +#else + if (_Helper != nullptr) { delete(_Helper); /* set to zero to ensure parser is not used while not instantiated */ @@ -294,29 +327,6 @@ GPS::task_main() while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) { // lock(); /* opportunistic publishing - else invalid data would end up on the bus */ -//#define FAKEGPS -#ifdef FAKEGPS - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 20.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; - - //no time and satellite information simulated -#endif if (_report_pub > 0) { orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); @@ -380,6 +390,7 @@ GPS::task_main() default: break; } +#endif } -- cgit v1.2.3 From 2ed315480e4582c9f223b88e1fee39303fbc9248 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:19:43 +0100 Subject: fakegps: default to 0 m/s speed --- src/drivers/gps/gps.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 3649a41d6..7df9cdb6d 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -264,7 +264,7 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { -//#define FAKEGPS +#define FAKEGPS #ifdef FAKEGPS _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; @@ -278,7 +278,7 @@ GPS::task_main() _report.eph_m = 10.0f; _report.epv_m = 10.0f; _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 20.0f; + _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; _report.vel_d_m_s = 0.0f; _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); -- cgit v1.2.3 From bccd0f8947f8d95f048b421b06fde25d162aac50 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 11 Jan 2014 00:46:45 +0100 Subject: fakegps: add command-line option --- src/drivers/gps/gps.cpp | 222 +++++++++++++++++++++++++----------------------- 1 file changed, 116 insertions(+), 106 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 7df9cdb6d..6b72d560f 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -85,7 +85,7 @@ static const int ERROR = -1; class GPS : public device::CDev { public: - GPS(const char *uart_path); + GPS(const char *uart_path, bool fake_gps); virtual ~GPS(); virtual int init(); @@ -112,6 +112,7 @@ private: 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 _fake_gps; ///< fake gps output /** @@ -156,7 +157,7 @@ GPS *g_dev; } -GPS::GPS(const char *uart_path) : +GPS::GPS(const char *uart_path, bool fake_gps) : CDev("gps", GPS_DEVICE_PATH), _task_should_exit(false), _healthy(false), @@ -164,7 +165,8 @@ GPS::GPS(const char *uart_path) : _mode(GPS_DRIVER_MODE_UBX), _Helper(nullptr), _report_pub(-1), - _rate(0.0f) + _rate(0.0f), + _fake_gps(fake_gps) { /* store port name */ strncpy(_port, uart_path, sizeof(_port)); @@ -264,133 +266,134 @@ GPS::task_main() /* loop handling received serial bytes and also configuring in between */ while (!_task_should_exit) { -#define FAKEGPS -#ifdef FAKEGPS - _report.timestamp_position = hrt_absolute_time(); - _report.lat = (int32_t)47.378301e7f; - _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; - _report.timestamp_variance = hrt_absolute_time(); - _report.s_variance_m_s = 10.0f; - _report.p_variance_m = 10.0f; - _report.c_variance_rad = 0.1f; - _report.fix_type = 3; - _report.eph_m = 10.0f; - _report.epv_m = 10.0f; - _report.timestamp_velocity = hrt_absolute_time(); - _report.vel_n_m_s = 0.0f; - _report.vel_e_m_s = 0.0f; - _report.vel_d_m_s = 0.0f; - _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); - _report.cog_rad = 0.0f; - _report.vel_ned_valid = true; - - //no time and satellite information simulated - - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (_fake_gps) { + + _report.timestamp_position = hrt_absolute_time(); + _report.lat = (int32_t)47.378301e7f; + _report.lon = (int32_t)8.538777e7f; + _report.alt = (int32_t)400e3f; + _report.timestamp_variance = hrt_absolute_time(); + _report.s_variance_m_s = 10.0f; + _report.p_variance_m = 10.0f; + _report.c_variance_rad = 0.1f; + _report.fix_type = 3; + _report.eph_m = 10.0f; + _report.epv_m = 10.0f; + _report.timestamp_velocity = hrt_absolute_time(); + _report.vel_n_m_s = 0.0f; + _report.vel_e_m_s = 0.0f; + _report.vel_d_m_s = 0.0f; + _report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s); + _report.cog_rad = 0.0f; + _report.vel_ned_valid = true; + + //no time and satellite information simulated + + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - usleep(2e5); -#else + usleep(2e5); - if (_Helper != nullptr) { - delete(_Helper); - /* set to zero to ensure parser is not used while not instantiated */ - _Helper = nullptr; - } + } else { - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - _Helper = new UBX(_serial_fd, &_report); - break; + if (_Helper != nullptr) { + delete(_Helper); + /* set to zero to ensure parser is not used while not instantiated */ + _Helper = nullptr; + } - case GPS_DRIVER_MODE_MTK: - _Helper = new MTK(_serial_fd, &_report); - break; + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _Helper = new UBX(_serial_fd, &_report); + break; - default: - break; - } + case GPS_DRIVER_MODE_MTK: + _Helper = new MTK(_serial_fd, &_report); + break; - unlock(); + default: + break; + } - if (_Helper->configure(_baudrate) == 0) { unlock(); - // GPS is obviously detected successfully, reset statistics - _Helper->reset_update_rates(); + 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); + // GPS is obviously detected successfully, reset statistics + _Helper->reset_update_rates(); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); - } + 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); - last_rate_count++; + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } - /* measure update rate every 5 seconds */ - 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; - _Helper->store_update_rates(); - _Helper->reset_update_rates(); - } + last_rate_count++; - if (!_healthy) { - char *mode_str = "unknown"; + /* measure update rate every 5 seconds */ + 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; + _Helper->store_update_rates(); + _Helper->reset_update_rates(); + } + + if (!_healthy) { + char *mode_str = "unknown"; + + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + mode_str = "UBX"; + break; - switch (_mode) { - case GPS_DRIVER_MODE_UBX: - mode_str = "UBX"; - break; + case GPS_DRIVER_MODE_MTK: + mode_str = "MTK"; + break; - case GPS_DRIVER_MODE_MTK: - mode_str = "MTK"; - break; + default: + break; + } - default: - break; + warnx("module found: %s", mode_str); + _healthy = true; } + } - warnx("module found: %s", mode_str); - _healthy = true; + if (_healthy) { + warnx("module lost"); + _healthy = false; + _rate = 0.0f; } - } - if (_healthy) { - warnx("module lost"); - _healthy = false; - _rate = 0.0f; + lock(); } lock(); - } - lock(); + /* select next mode */ + switch (_mode) { + case GPS_DRIVER_MODE_UBX: + _mode = GPS_DRIVER_MODE_MTK; + break; - /* 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_MTK: - _mode = GPS_DRIVER_MODE_UBX; - break; - - default: - break; + default: + break; + } } -#endif } @@ -451,7 +454,7 @@ namespace gps GPS *g_dev; -void start(const char *uart_path); +void start(const char *uart_path, bool fake_gps); void stop(); void test(); void reset(); @@ -461,7 +464,7 @@ void info(); * Start the driver. */ void -start(const char *uart_path) +start(const char *uart_path, bool fake_gps) { int fd; @@ -469,7 +472,7 @@ start(const char *uart_path) errx(1, "already started"); /* create the driver */ - g_dev = new GPS(uart_path); + g_dev = new GPS(uart_path, fake_gps); if (g_dev == nullptr) goto fail; @@ -561,6 +564,7 @@ gps_main(int argc, char *argv[]) /* set to default */ char *device_name = GPS_DEFAULT_UART_PORT; + bool fake_gps = false; /* * Start/load the driver. @@ -576,7 +580,13 @@ gps_main(int argc, char *argv[]) } } - gps::start(device_name); + /* Detect fake gps option */ + for (int i = 2; i < argc; i++) { + if (!strcmp(argv[i], "-f")) + fake_gps = true; + } + + gps::start(device_name, fake_gps); } if (!strcmp(argv[1], "stop")) @@ -601,5 +611,5 @@ gps_main(int argc, char *argv[]) gps::info(); out: - errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n]"); + errx(1, "unrecognized command, try 'start', 'stop', 'test', 'reset' or 'status' [-d /dev/ttyS0-n][-f]"); } -- cgit v1.2.3 From c6e196edca7acae0b548a85c94d0c8f37df3c7aa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 12 Jan 2014 11:52:19 +0100 Subject: Support disabling GPS output via IOCTL, general cleanup of author and copyright code style --- src/drivers/gps/gps.cpp | 21 +++++++++++++-------- src/drivers/gps/gps_helper.cpp | 7 ++++--- src/drivers/gps/gps_helper.h | 6 +++--- src/drivers/gps/module.mk | 2 +- src/drivers/gps/mtk.cpp | 11 +++++++---- src/drivers/gps/mtk.h | 11 +++++++---- src/drivers/gps/ubx.cpp | 10 ++++++---- src/drivers/gps/ubx.h | 17 ++++++++++++----- 8 files changed, 53 insertions(+), 32 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index 6b72d560f..a736cbdf6 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -289,11 +289,13 @@ GPS::task_main() //no time and satellite information simulated - if (_report_pub > 0) { - orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } usleep(2e5); @@ -330,11 +332,14 @@ GPS::task_main() 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); - } else { - _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + if (!(_pub_blocked)) { + if (_report_pub > 0) { + orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); + + } else { + _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); + } } last_rate_count++; diff --git a/src/drivers/gps/gps_helper.cpp b/src/drivers/gps/gps_helper.cpp index 2e2cbc8dd..2360ff39b 100644 --- a/src/drivers/gps/gps_helper.cpp +++ b/src/drivers/gps/gps_helper.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012,2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -41,6 +39,9 @@ /** * @file gps_helper.cpp + * + * @author Thomas Gubler + * @author Julian Oes */ float diff --git a/src/drivers/gps/gps_helper.h b/src/drivers/gps/gps_helper.h index 73d4b889c..cfb9e0d43 100644 --- a/src/drivers/gps/gps_helper.h +++ b/src/drivers/gps/gps_helper.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,6 +33,8 @@ /** * @file gps_helper.h + * @author Thomas Gubler + * @author Julian Oes */ #ifndef GPS_HELPER_H diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk index 097db2abf..82c67d40a 100644 --- a/src/drivers/gps/module.mk +++ b/src/drivers/gps/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 56b702ea6..456a9645b 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mkt.cpp */ +/** + * @file mkt.cpp + * + * @author Thomas Gubler + * @author Julian Oes + */ #include #include diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index b5cfbf0a6..9032e45b0 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +31,12 @@ * ****************************************************************************/ -/* @file mtk.h */ +/** + * @file mkt.cpp + * + * @author Thomas Gubler + * @author Julian Oes + */ #ifndef MTK_H_ #define MTK_H_ diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 86291901c..8a2afecb7 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -40,8 +37,13 @@ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description * including Prototol Specification. * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf */ + #include #include #include diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 76ef873a3..79a904f4a 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -1,9 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. - * Author: Thomas Gubler - * Julian Oes - * Anton Babushkin + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -34,7 +31,17 @@ * ****************************************************************************/ -/* @file U-Blox protocol definitions */ +/** + * @file ubx.h + * + * U-Blox protocol definition. Following u-blox 6/7 Receiver Description + * including Prototol Specification. + * + * @author Thomas Gubler + * @author Julian Oes + * @author Anton Babushkin + * + */ #ifndef UBX_H_ #define UBX_H_ -- cgit v1.2.3 From 47e0c926a6932b7a60ce85a5c748ce5bfcc102e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 14 Jan 2014 14:02:16 +0100 Subject: Fixed two typos identified by kroimon --- src/drivers/gps/mtk.cpp | 2 +- src/drivers/gps/mtk.h | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers/gps') diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp index 456a9645b..c90ecbe28 100644 --- a/src/drivers/gps/mtk.cpp +++ b/src/drivers/gps/mtk.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mkt.cpp + * @file mtk.cpp * * @author Thomas Gubler * @author Julian Oes diff --git a/src/drivers/gps/mtk.h b/src/drivers/gps/mtk.h index 9032e45b0..a2d5e27bb 100644 --- a/src/drivers/gps/mtk.h +++ b/src/drivers/gps/mtk.h @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mkt.cpp + * @file mtk.cpp * * @author Thomas Gubler * @author Julian Oes -- cgit v1.2.3