fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-21 03:23:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Another rewrite: most of the polling, reading and writing is now inside the GPS classes

Browse Source
This commit is contained in:
Julian Oes
2013-02-08 11:05:57 -08:00
parent a88b9f4eef
commit df6cf142e7
8 changed files with 583 additions and 567 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/drivers/drv_gps.h
+2 -1
View File
@@ -49,7 +49,8 @@
#define GPS_DEVICE_PATH "/dev/gps" #define GPS_DEVICE_PATH "/dev/gps"
typedef enum { typedef enum {
GPS_DRIVER_MODE_UBX = 0, GPS_DRIVER_MODE_NONE = 0,
GPS_DRIVER_MODE_UBX,
GPS_DRIVER_MODE_MTK, GPS_DRIVER_MODE_MTK,
GPS_DRIVER_MODE_NMEA, GPS_DRIVER_MODE_NMEA,
} gps_driver_mode_t; } gps_driver_mode_t;
apps/drivers/gps/gps.cpp
+35 -27
View File
@@ -36,10 +36,7 @@
* Driver for the GPS on a serial port * Driver for the GPS on a serial port
*/ */
#include <nuttx/config.h> #include <nuttx/clock.h>
#include <drivers/device/i2c.h>
#include <sys/types.h> #include <sys/types.h>
#include <stdint.h> #include <stdint.h>
#include <stdio.h> #include <stdio.h>
@@ -54,30 +51,24 @@
#include <math.h> #include <math.h>
#include <unistd.h> #include <unistd.h>
#include <fcntl.h> #include <fcntl.h>
#include <nuttx/config.h>
#include <nuttx/arch.h> #include <nuttx/arch.h>
#include <nuttx/clock.h>
#include <arch/board/board.h> #include <arch/board/board.h>
#include <drivers/drv_hrt.h> #include <drivers/drv_hrt.h>
#include <drivers/device/i2c.h>
#include <systemlib/perf_counter.h> #include <systemlib/perf_counter.h>
#include <systemlib/scheduling_priorities.h> #include <systemlib/scheduling_priorities.h>
#include <systemlib/err.h> #include <systemlib/err.h>
#include <drivers/drv_gps.h> #include <drivers/drv_gps.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_gps_position.h> #include <uORB/topics/vehicle_gps_position.h>
//#include "ubx.h" #include "ubx.h"
#include "mtk.h" #include "mtk.h"
#define SEND_BUFFER_LENGTH 100
#define TIMEOUT 1000000 //1s
#define NUMBER_OF_TRIES 5 #define TIMEOUT_5HZ 400
#define CONFIG_TIMEOUT 2000000 #define RATE_MEASUREMENT_PERIOD 5000000
/* oddly, ERROR is not defined for c++ */ /* oddly, ERROR is not defined for c++ */
#ifdef ERROR #ifdef ERROR
@@ -170,7 +161,7 @@ GPS::GPS(const char* uart_path) :
_task_should_exit(false), _task_should_exit(false),
_healthy(false), _healthy(false),
_mode_changed(false), _mode_changed(false),
_mode(GPS_DRIVER_MODE_MTK), _mode(GPS_DRIVER_MODE_UBX),
_Helper(nullptr), _Helper(nullptr),
_report_pub(-1), _report_pub(-1),
_rate(0.0f) _rate(0.0f)
@@ -279,12 +270,11 @@ GPS::task_main()
} }
switch (_mode) { switch (_mode) {
// case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
// _Helper = new UBX(); _Helper = new UBX(_serial_fd, &_report);
// break; break;
case GPS_DRIVER_MODE_MTK: case GPS_DRIVER_MODE_MTK:
printf("try new mtk\n"); _Helper = new MTK(_serial_fd, &_report);
_Helper = new MTK();
break; break;
case GPS_DRIVER_MODE_NMEA: case GPS_DRIVER_MODE_NMEA:
//_Helper = new NMEA(); //TODO: add NMEA //_Helper = new NMEA(); //TODO: add NMEA
@@ -292,11 +282,11 @@ GPS::task_main()
default: default:
break; break;
} }
// XXX unlock/lock makes sense?
unlock(); unlock();
if (_Helper->configure(_serial_fd, _baudrate) == 0) { if (_Helper->configure(_baudrate) == 0) {
unlock();
while (_Helper->receive(_serial_fd, _report) > 0 && !_task_should_exit) { while (_Helper->receive(TIMEOUT_5HZ) > 0 && !_task_should_exit) {
// lock();
/* opportunistic publishing - else invalid data would end up on the bus */ /* opportunistic publishing - else invalid data would end up on the bus */
if (_report_pub > 0) { if (_report_pub > 0) {
orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report); orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
@@ -307,7 +297,7 @@ GPS::task_main()
last_rate_count++; last_rate_count++;
/* measure update rate every 5 seconds */ /* measure update rate every 5 seconds */
if (hrt_absolute_time() - last_rate_measurement > 5000000) { if (hrt_absolute_time() - last_rate_measurement > RATE_MEASUREMENT_PERIOD) {
_rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f); _rate = last_rate_count / ((float)((hrt_absolute_time() - last_rate_measurement)) / 1000000.0f);
last_rate_measurement = hrt_absolute_time(); last_rate_measurement = hrt_absolute_time();
last_rate_count = 0; last_rate_count = 0;
@@ -323,8 +313,26 @@ GPS::task_main()
_healthy = false; _healthy = false;
_rate = 0.0f; _rate = 0.0f;
} }
lock();
} }
lock(); lock();
/* select next mode */
switch (_mode) {
case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK;
break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX;
break;
// case GPS_DRIVER_MODE_NMEA:
// _mode = GPS_DRIVER_MODE_UBX;
// break;
default:
break;
}
} }
debug("exiting"); debug("exiting");
apps/drivers/gps/gps_helper.cpp
+2
View File
@@ -57,6 +57,8 @@ GPS_Helper::set_baudrate(const int &fd, unsigned baud)
case 115200: speed = B115200; break; case 115200: speed = B115200; break;
warnx("try baudrate: %d\n", speed);
default: default:
warnx("ERROR: Unsupported baudrate: %d\n", baud); warnx("ERROR: Unsupported baudrate: %d\n", baud);
return -EINVAL; return -EINVAL;
apps/drivers/gps/gps_helper.h
+2 -2
View File
@@ -44,8 +44,8 @@
class GPS_Helper class GPS_Helper
{ {
public: public:
virtual int configure(const int &fd, unsigned &baud) = 0; virtual int configure(unsigned &baud) = 0;
virtual int receive(const int &fd, struct vehicle_gps_position_s &gps_position) = 0; virtual int receive(unsigned timeout) = 0;
int set_baudrate(const int &fd, unsigned baud); int set_baudrate(const int &fd, unsigned baud);
}; };
apps/drivers/gps/mtk.cpp
+39 -30
View File
@@ -47,7 +47,9 @@
#include "mtk.h" #include "mtk.h"
MTK::MTK() : MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd),
_gps_position(gps_position),
_mtk_revision(0) _mtk_revision(0)
{ {
decode_init(); decode_init();
@@ -58,40 +60,40 @@ MTK::~MTK()
} }
int int
MTK::configure(const int &fd, unsigned &baudrate) MTK::configure(unsigned &baudrate)
{ {
/* set baudrate first */ /* set baudrate first */
if (GPS_Helper::set_baudrate(fd, MTK_BAUDRATE) != 0) if (GPS_Helper::set_baudrate(_fd, MTK_BAUDRATE) != 0)
return -1; return -1;
baudrate = MTK_BAUDRATE; baudrate = MTK_BAUDRATE;
/* Write config messages, don't wait for an answer */ /* Write config messages, don't wait for an answer */
if (strlen(MTK_OUTPUT_5HZ) != write(fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) { if (strlen(MTK_OUTPUT_5HZ) != write(_fd, MTK_OUTPUT_5HZ, strlen(MTK_OUTPUT_5HZ))) {
warnx("mtk: config write failed"); warnx("mtk: config write failed");
return -1; return -1;
} }
usleep(10000); usleep(10000);
if (strlen(MTK_SET_BINARY) != write(fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) { if (strlen(MTK_SET_BINARY) != write(_fd, MTK_SET_BINARY, strlen(MTK_SET_BINARY))) {
warnx("mtk: config write failed"); warnx("mtk: config write failed");
return -1; return -1;
} }
usleep(10000); usleep(10000);
if (strlen(SBAS_ON) != write(fd, SBAS_ON, strlen(SBAS_ON))) { if (strlen(SBAS_ON) != write(_fd, SBAS_ON, strlen(SBAS_ON))) {
warnx("mtk: config write failed"); warnx("mtk: config write failed");
return -1; return -1;
} }
usleep(10000); usleep(10000);
if (strlen(WAAS_ON) != write(fd, WAAS_ON, strlen(WAAS_ON))) { if (strlen(WAAS_ON) != write(_fd, WAAS_ON, strlen(WAAS_ON))) {
warnx("mtk: config write failed"); warnx("mtk: config write failed");
return -1; return -1;
} }
usleep(10000); usleep(10000);
if (strlen(MTK_NAVTHRES_OFF) != write(fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) { if (strlen(MTK_NAVTHRES_OFF) != write(_fd, MTK_NAVTHRES_OFF, strlen(MTK_NAVTHRES_OFF))) {
warnx("mtk: config write failed"); warnx("mtk: config write failed");
return -1; return -1;
} }
@@ -100,16 +102,19 @@ MTK::configure(const int &fd, unsigned &baudrate)
} }
int int
MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position) MTK::receive(unsigned timeout)
{ {
/* poll descriptor */ /* poll descriptor */
pollfd fds[1]; pollfd fds[1];
fds[0].fd = fd; fds[0].fd = _fd;
fds[0].events = POLLIN; fds[0].events = POLLIN;
uint8_t buf[32]; uint8_t buf[32];
gps_mtk_packet_t packet; gps_mtk_packet_t packet;
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0; int j = 0;
ssize_t count = 0; ssize_t count = 0;
@@ -120,9 +125,13 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
/* pass received bytes to the packet decoder */ /* pass received bytes to the packet decoder */
while (j < count) { while (j < count) {
if (parse_char(buf[j], packet) > 0) { if (parse_char(buf[j], packet) > 0) {
handle_message(packet, gps_position); handle_message(packet);
return 1; return 1;
} }
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
j++; j++;
} }
/* everything is read */ /* everything is read */
@@ -130,7 +139,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
} }
/* then poll for new data */ /* then poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), MTK_TIMEOUT_5HZ); int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
if (ret < 0) { if (ret < 0) {
/* something went wrong when polling */ /* something went wrong when polling */
@@ -148,7 +157,7 @@ MTK::receive(const int &fd, struct vehicle_gps_position_s &gps_position)
* won't block even on a blocking device. If more bytes are * won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again... * available, we'll go back to poll() again...
*/ */
count = ::read(fd, buf, sizeof(buf)); count = ::read(_fd, buf, sizeof(buf));
} }
} }
} }
@@ -212,26 +221,26 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
} }
void void
MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position) MTK::handle_message(gps_mtk_packet_t &packet)
{ {
if (_mtk_revision == 16) { if (_mtk_revision == 16) {
gps_position.lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7 _gps_position->lat = packet.latitude * 10; // from degrees*1e6 to degrees*1e7
gps_position.lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7 _gps_position->lon = packet.longitude * 10; // from degrees*1e6 to degrees*1e7
} else if (_mtk_revision == 19) { } else if (_mtk_revision == 19) {
gps_position.lat = packet.latitude; // both degrees*1e7 _gps_position->lat = packet.latitude; // both degrees*1e7
gps_position.lon = packet.longitude; // both degrees*1e7 _gps_position->lon = packet.longitude; // both degrees*1e7
} else { } else {
warnx("mtk: unknown revision"); warnx("mtk: unknown revision");
gps_position.lat = 0; _gps_position->lat = 0;
gps_position.lon = 0; _gps_position->lon = 0;
} }
gps_position.alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm _gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
gps_position.fix_type = packet.fix_type; _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->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->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->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
gps_position.satellites_visible = packet.satellites; _gps_position->satellites_visible = packet.satellites;
/* convert time and date information to unix timestamp */ /* convert time and date information to unix timestamp */
struct tm timeinfo; //TODO: test this conversion struct tm timeinfo; //TODO: test this conversion
@@ -250,9 +259,9 @@ MTK::handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps
timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3; timeinfo_conversion_temp -= timeinfo.tm_sec * 1e3;
time_t epoch = mktime(&timeinfo); time_t epoch = mktime(&timeinfo);
gps_position.time_gps_usec = epoch * 1e6; //TODO: test this _gps_position->time_gps_usec = epoch * 1e6; //TODO: test this
gps_position.time_gps_usec += timeinfo_conversion_temp * 1e3; _gps_position->time_gps_usec += timeinfo_conversion_temp * 1e3;
gps_position.timestamp_position = gps_position.timestamp_time = hrt_absolute_time(); _gps_position->timestamp_position = _gps_position->timestamp_time = hrt_absolute_time();
return; return;
} }
apps/drivers/gps/mtk.h
+6 -4
View File
@@ -85,10 +85,10 @@ typedef struct {
class MTK : public GPS_Helper class MTK : public GPS_Helper
{ {
public: public:
MTK(); MTK(const int &fd, struct vehicle_gps_position_s *gps_position);
~MTK(); ~MTK();
int receive(const int &fd, struct vehicle_gps_position_s &gps_position); int receive(unsigned timeout);
int configure(const int &fd, unsigned &baudrate); int configure(unsigned &baudrate);
private: private:
/** /**
@@ -99,7 +99,7 @@ private:
/** /**
* Handle the package once it has arrived * Handle the package once it has arrived
*/ */
void handle_message(gps_mtk_packet_t &packet, struct vehicle_gps_position_s &gps_position); void handle_message(gps_mtk_packet_t &packet);
/** /**
* Reset the parse state machine for a fresh start * Reset the parse state machine for a fresh start
@@ -111,6 +111,8 @@ private:
*/ */
void add_byte_to_checksum(uint8_t); void add_byte_to_checksum(uint8_t);
int _fd;
struct vehicle_gps_position_s *_gps_position;
mtk_decode_state_t _decode_state; mtk_decode_state_t _decode_state;
uint8_t _mtk_revision; uint8_t _mtk_revision;
uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE]; uint8_t _rx_buffer[MTK_RECV_BUFFER_SIZE];
apps/drivers/gps/ubx.cpp
+257 -279
View File
@@ -37,6 +37,7 @@
#include <unistd.h> #include <unistd.h>
#include <stdio.h> #include <stdio.h>
#include <poll.h>
#include <math.h> #include <math.h>
#include <string.h> #include <string.h>
#include <assert.h> #include <assert.h>
@@ -47,43 +48,31 @@
#include "ubx.h" #include "ubx.h"
#define UBX_CONFIG_TIMEOUT 100
UBX::UBX() : UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_config_state(UBX_CONFIG_STATE_PRT), _fd(fd),
_gps_position(gps_position),
_waiting_for_ack(false) _waiting_for_ack(false)
{ {
reset(); decode_init();
} }
UBX::~UBX() UBX::~UBX()
{ {
} }
void int
UBX::reset() UBX::configure(unsigned &baudrate)
{ {
decodeInit();
_config_state = UBX_CONFIG_STATE_PRT;
_waiting_for_ack = false;
}
void
UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
{
/* Only send a new config message when we got the ACK of the last one,
* otherwise we might not configure all the messages because the ACK comes from an older/previos CFD command
* reason being that the ACK only includes CFG-MSG but not to which NAV MSG it belongs.
*/
if (!_waiting_for_ack) {
_waiting_for_ack = true; _waiting_for_ack = true;
if (_config_state == UBX_CONFIG_STATE_CONFIGURED) {
/* This should never happen, the parser should set the flag, /* try different baudrates */
* if it should be reconfigured, reset() should be called first. const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
*/
warnx("ubx: already configured"); for (int baud_i = 0; baud_i < 5; baud_i++) {
_waiting_for_ack = false; baudrate = baudrates_to_try[baud_i];
return; set_baudrate(_fd, baudrate);
} else if (_config_state == UBX_CONFIG_STATE_PRT) {
/* Send a CFG-PRT message to set the UBX protocol for in and out /* Send a CFG-PRT message to set the UBX protocol for in and out
* and leave the baudrate as it is, we just want an ACK-ACK from this * and leave the baudrate as it is, we just want an ACK-ACK from this
@@ -92,6 +81,9 @@ UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
/* Set everything else of the packet to 0, otherwise the module wont accept it */ /* Set everything else of the packet to 0, otherwise the module wont accept it */
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_PRT;
/* Define the package contents, don't change the baudrate */ /* Define the package contents, don't change the baudrate */
cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
@@ -102,13 +94,14 @@ UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
} else if (_config_state == UBX_CONFIG_STATE_PRT_NEW_BAUDRATE) { if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
/* Send a CFG-PRT message again, this time change the baudrate */ /* Send a CFG-PRT message again, this time change the baudrate */
type_gps_bin_cfg_prt_packet_t cfg_prt_packet;
memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
cfg_prt_packet.clsID = UBX_CLASS_CFG; cfg_prt_packet.clsID = UBX_CLASS_CFG;
cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT; cfg_prt_packet.msgID = UBX_MESSAGE_CFG_PRT;
@@ -119,23 +112,21 @@ UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
sendConfigPacket(fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
/* If the new baudrate will be different from the current one, we should report that back to the driver */
if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
baudrate_changed = true;
/* Don't wait for an ACK, we're switching baudrate and we might never get,
* after that, start fresh */
reset();
} }
} else if (_config_state == UBX_CONFIG_STATE_RATE) { /* no ack is ecpected here, keep going configuring */
/* send a CFT-RATE message to define update rate */ /* send a CFT-RATE message to define update rate */
type_gps_bin_cfg_rate_packet_t cfg_rate_packet; type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.clsID = UBX_CLASS_CFG; cfg_rate_packet.clsID = UBX_CLASS_CFG;
cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE;
cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; cfg_rate_packet.length = UBX_CFG_RATE_LENGTH;
@@ -143,13 +134,19 @@ UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE;
cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF;
sendConfigPacket(fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
} else if (_config_state == UBX_CONFIG_STATE_NAV5) {
/* send a NAV5 message to set the options for the internal filter */ /* send a NAV5 message to set the options for the internal filter */
type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.clsID = UBX_CLASS_CFG; cfg_nav5_packet.clsID = UBX_CLASS_CFG;
cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5;
cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH;
@@ -157,62 +154,145 @@ UBX::configure(const int &fd, bool &baudrate_changed, unsigned &baudrate)
cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
sendConfigPacket(fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
} else { /* try next baudrate */
/* Catch the remaining config states here, they all need the same packet type */ continue;
}
type_gps_bin_cfg_msg_packet_t cfg_msg_packet; type_gps_bin_cfg_msg_packet_t cfg_msg_packet;
memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet)); memset(&cfg_msg_packet, 0, sizeof(cfg_msg_packet));
_clsID_needed = UBX_CLASS_CFG;
_msgID_needed = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.clsID = UBX_CLASS_CFG; cfg_msg_packet.clsID = UBX_CLASS_CFG;
cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG; cfg_msg_packet.msgID = UBX_MESSAGE_CFG_MSG;
cfg_msg_packet.length = UBX_CFG_MSG_LENGTH; cfg_msg_packet.length = UBX_CFG_MSG_LENGTH;
/* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */ /* Choose fast 5Hz rate for all messages except SVINFO which is big and not important */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ; cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_5HZ;
switch (_config_state) {
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_POSLLH;
break;
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC: send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
break;
// case UBX_CONFIG_STATE_MSG_NAV_DOP: send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; if (receive(UBX_CONFIG_TIMEOUT) < 0) {
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP; /* try next baudrate */
// break; continue;
case UBX_CONFIG_STATE_MSG_NAV_SVINFO: }
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
/* For satelites info 1Hz is enough */ /* For satelites info 1Hz is enough */
cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ; cfg_msg_packet.rate[1] = UBX_CFG_MSG_PAYLOAD_RATE1_1HZ;
break;
case UBX_CONFIG_STATE_MSG_NAV_SOL: send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SOL;
break;
case UBX_CONFIG_STATE_MSG_NAV_VELNED: send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV; cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED; cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_VELNED;
break;
// case UBX_CONFIG_STATE_MSG_RXM_SVSI: send_config_packet(_fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet));
if (receive(UBX_CONFIG_TIMEOUT) < 0) {
/* try next baudrate */
continue;
}
// cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
// cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM; // cfg_msg_packet.msgClass_payload = UBX_CLASS_RXM;
// cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI; // cfg_msg_packet.msgID_payload = UBX_MESSAGE_RXM_SVSI;
// break;
default: _waiting_for_ack = false;
break; return 0;
}
return -1;
} }
sendConfigPacket(fd, (uint8_t*)&cfg_msg_packet, sizeof(cfg_msg_packet)); int
UBX::receive(unsigned timeout)
{
/* poll descriptor */
pollfd fds[1];
fds[0].fd = _fd;
fds[0].events = POLLIN;
uint8_t buf[32];
/* timeout additional to poll */
uint64_t time_started = hrt_absolute_time();
int j = 0;
ssize_t count = 0;
while (true) {
/* pass received bytes to the packet decoder */
while (j < count) {
if (parse_char(buf[j]) > 0) {
/* return to configure during configuration or to the gps driver during normal work
* if a packet has arrived */
if (handle_message() > 0)
return 1;
}
/* in case we keep trying but only get crap from GPS */
if (time_started + timeout*1000 < hrt_absolute_time() ) {
return -1;
}
j++;
}
/* everything is read */
j = count = 0;
/* then poll for new data */
int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
if (ret < 0) {
/* something went wrong when polling */
return -1;
} else if (ret == 0) {
/* Timeout */
return -1;
} else if (ret > 0) {
/* if we have new data from GPS, go handle it */
if (fds[0].revents & POLLIN) {
/*
* We are here because poll says there is some data, so this
* won't block even on a blocking device. If more bytes are
* available, we'll go back to poll() again...
*/
count = ::read(_fd, buf, sizeof(buf));
}
} }
} }
} }
void int
UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_needed, bool &pos_updated) UBX::parse_char(uint8_t b)
{ {
switch (_decode_state) { switch (_decode_state) {
/* First, look for sync1 */ /* First, look for sync1 */
@@ -227,13 +307,13 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
_decode_state = UBX_DECODE_GOT_SYNC2; _decode_state = UBX_DECODE_GOT_SYNC2;
} else { } else {
/* Second start symbol was wrong, reset state machine */ /* Second start symbol was wrong, reset state machine */
decodeInit(); decode_init();
} }
break; break;
/* Now look for class */ /* Now look for class */
case UBX_DECODE_GOT_SYNC2: case UBX_DECODE_GOT_SYNC2:
/* everything except sync1 and sync2 needs to be added to the checksum */ /* everything except sync1 and sync2 needs to be added to the checksum */
addByteToChecksum(b); add_byte_to_checksum(b);
/* check for known class */ /* check for known class */
switch (b) { switch (b) {
case UBX_CLASS_ACK: case UBX_CLASS_ACK:
@@ -256,12 +336,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
_message_class = CFG; _message_class = CFG;
break; break;
default: //unknown class: reset state machine default: //unknown class: reset state machine
decodeInit(); decode_init();
break; break;
} }
break; break;
case UBX_DECODE_GOT_CLASS: case UBX_DECODE_GOT_CLASS:
addByteToChecksum(b); add_byte_to_checksum(b);
switch (_message_class) { switch (_message_class) {
case NAV: case NAV:
switch (b) { switch (b) {
@@ -296,7 +376,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
break; break;
default: //unknown class: reset state machine, should not happen default: //unknown class: reset state machine, should not happen
decodeInit(); decode_init();
break; break;
} }
break; break;
@@ -308,7 +388,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
// break; // break;
// //
// default: //unknown class: reset state machine, should not happen // default: //unknown class: reset state machine, should not happen
// decodeInit(); // decode_init();
// break; // break;
// } // }
// break; // break;
@@ -321,7 +401,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
break; break;
default: //unknown class: reset state machine, should not happen default: //unknown class: reset state machine, should not happen
decodeInit(); decode_init();
break; break;
} }
break; break;
@@ -337,82 +417,97 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
_message_id = ACK_NAK; _message_id = ACK_NAK;
break; break;
default: //unknown class: reset state machine, should not happen default: //unknown class: reset state machine, should not happen
decodeInit(); decode_init();
break; break;
} }
break; break;
default: //should not happen because we set the class default: //should not happen because we set the class
warnx("UBX Error, we set a class that we don't know"); warnx("UBX Error, we set a class that we don't know");
decodeInit(); decode_init();
config_needed = true; // config_needed = true;
break; break;
} }
break; break;
case UBX_DECODE_GOT_MESSAGEID: case UBX_DECODE_GOT_MESSAGEID:
addByteToChecksum(b); add_byte_to_checksum(b);
_payload_size = b; //this is the first length byte _payload_size = b; //this is the first length byte
_decode_state = UBX_DECODE_GOT_LENGTH1; _decode_state = UBX_DECODE_GOT_LENGTH1;
break; break;
case UBX_DECODE_GOT_LENGTH1: case UBX_DECODE_GOT_LENGTH1:
addByteToChecksum(b); add_byte_to_checksum(b);
_payload_size += b << 8; // here comes the second byte of length _payload_size += b << 8; // here comes the second byte of length
_decode_state = UBX_DECODE_GOT_LENGTH2; _decode_state = UBX_DECODE_GOT_LENGTH2;
break; break;
case UBX_DECODE_GOT_LENGTH2: case UBX_DECODE_GOT_LENGTH2:
/* Add to checksum if not yet at checksum byte */ /* Add to checksum if not yet at checksum byte */
if (_rx_count < _payload_size) if (_rx_count < _payload_size)
addByteToChecksum(b); add_byte_to_checksum(b);
_rx_buffer[_rx_count] = b; _rx_buffer[_rx_count] = b;
/* once the payload has arrived, we can process the information */ /* once the payload has arrived, we can process the information */
if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes 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;
}
return 0; //XXX ?
}
int
UBX::handle_message()
{
int ret = 0;
switch (_message_id) { //this enum is unique for all ids --> no need to check the class switch (_message_id) { //this enum is unique for all ids --> no need to check the class
case NAV_POSLLH: { case NAV_POSLLH: {
// printf("GOT NAV_POSLLH MESSAGE\n"); // 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; gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
//Check if checksum is valid and the store the gps information _gps_position->lat = packet->lat;
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { _gps_position->lon = packet->lon;
gps_position->lat = packet->lat; _gps_position->alt = packet->height_msl;
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->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->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
/* Add timestamp to finish the report */ /* Add timestamp to finish the report */
gps_position->timestamp_position = hrt_absolute_time(); _gps_position->timestamp_position = hrt_absolute_time();
/* only return 1 when new position is available */
/* set flag to trigger publishing of new position */ ret = 1;
pos_updated = true;
} else {
warnx("NAV_POSLLH: checksum invalid");
} }
// Reset state machine to decode next packet
decodeInit();
break; break;
} }
case NAV_SOL: { case NAV_SOL: {
// printf("GOT NAV_SOL MESSAGE\n"); // 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_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
//Check if checksum is valid and the store the gps information _gps_position->fix_type = packet->gpsFix;
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { _gps_position->s_variance_m_s = packet->sAcc;
_gps_position->p_variance_m = packet->pAcc;
gps_position->fix_type = packet->gpsFix; _gps_position->timestamp_variance = hrt_absolute_time();
gps_position->s_variance_m_s = packet->sAcc;
gps_position->p_variance_m = packet->pAcc;
gps_position->timestamp_variance = hrt_absolute_time();
} else {
warnx("NAV_SOL: checksum invalid");
} }
// Reset state machine to decode next packet
decodeInit();
break; break;
} }
@@ -420,31 +515,22 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
//// printf("GOT NAV_DOP MESSAGE\n"); //// printf("GOT NAV_DOP MESSAGE\n");
// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; // gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
// //
// //Check if checksum is valid and the store the gps information // _gps_position->eph_m = packet->hDOP;
// if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { // _gps_position->epv = packet->vDOP;
// //
// gps_position->eph_m = packet->hDOP; // _gps_position->timestamp_posdilution = hrt_absolute_time();
// gps_position->epv = packet->vDOP;
//
// gps_position->timestamp_posdilution = hrt_absolute_time();
// //
// _new_nav_dop = true; // _new_nav_dop = true;
// //
// } else {
// warnx("NAV_DOP: checksum invalid");
// }
//
// // Reset state machine to decode next packet
// decodeInit();
// break; // break;
// } // }
case NAV_TIMEUTC: { case NAV_TIMEUTC: {
// printf("GOT NAV_TIMEUTC MESSAGE\n"); // printf("GOT NAV_TIMEUTC MESSAGE\n");
if (!_waiting_for_ack) {
gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
//Check if checksum is valid and the store the gps information
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
//convert to unix timestamp //convert to unix timestamp
struct tm timeinfo; struct tm timeinfo;
timeinfo.tm_year = packet->year - 1900; timeinfo.tm_year = packet->year - 1900;
@@ -456,23 +542,18 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
time_t epoch = mktime(&timeinfo); time_t epoch = mktime(&timeinfo);
gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this _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->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
gps_position->timestamp_time = hrt_absolute_time(); _gps_position->timestamp_time = hrt_absolute_time();
} else {
warnx("NAV_TIMEUTC: checksum invalid");
} }
// Reset state machine to decode next packet
decodeInit();
break; break;
} }
case NAV_SVINFO: { 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 //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; const int length_part1 = 8;
char _rx_buffer_part1[length_part1]; char _rx_buffer_part1[length_part1];
@@ -483,10 +564,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
const int length_part3 = 2; const int length_part3 = 2;
char _rx_buffer_part3[length_part3]; char _rx_buffer_part3[length_part3];
memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
gps_bin_nav_svinfo_part3_packet_t *packet_part3 = (gps_bin_nav_svinfo_part3_packet_t *) _rx_buffer_part3;
//Check if checksum is valid and then store the gps information
if (_rx_ck_a == packet_part3->ck_a && _rx_ck_b == packet_part3->ck_b) {
//definitions needed to read numCh elements from the buffer: //definitions needed to read numCh elements from the buffer:
const int length_part2 = 12; const int length_part2 = 12;
gps_bin_nav_svinfo_part2_packet_t *packet_part2; gps_bin_nav_svinfo_part2_packet_t *packet_part2;
@@ -503,81 +581,70 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
/* Write satellite information in the global storage */ /* Write satellite information in the global storage */
gps_position->satellite_prn[i] = packet_part2->svid; _gps_position->satellite_prn[i] = packet_part2->svid;
//if satellite information is healthy store the data //if satellite information is healthy store the data
uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
if (!unhealthy) { if (!unhealthy) {
if ((packet_part2->flags) & 1) { //flags is a bitfield if ((packet_part2->flags) & 1) { //flags is a bitfield
gps_position->satellite_used[i] = 1; _gps_position->satellite_used[i] = 1;
satellites_used++; satellites_used++;
} else { } else {
gps_position->satellite_used[i] = 0; _gps_position->satellite_used[i] = 0;
} }
gps_position->satellite_snr[i] = packet_part2->cno; _gps_position->satellite_snr[i] = packet_part2->cno;
gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); _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_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
} else { } else {
gps_position->satellite_used[i] = 0; _gps_position->satellite_used[i] = 0;
gps_position->satellite_snr[i] = 0; _gps_position->satellite_snr[i] = 0;
gps_position->satellite_elevation[i] = 0; _gps_position->satellite_elevation[i] = 0;
gps_position->satellite_azimuth[i] = 0; _gps_position->satellite_azimuth[i] = 0;
} }
} }
for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
/* Unused channels have to be set to zero for e.g. MAVLink */ /* Unused channels have to be set to zero for e.g. MAVLink */
gps_position->satellite_prn[i] = 0; _gps_position->satellite_prn[i] = 0;
gps_position->satellite_used[i] = 0; _gps_position->satellite_used[i] = 0;
gps_position->satellite_snr[i] = 0; _gps_position->satellite_snr[i] = 0;
gps_position->satellite_elevation[i] = 0; _gps_position->satellite_elevation[i] = 0;
gps_position->satellite_azimuth[i] = 0; _gps_position->satellite_azimuth[i] = 0;
} }
gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
/* set timestamp if any sat info is available */ /* set timestamp if any sat info is available */
if (packet_part1->numCh > 0) { if (packet_part1->numCh > 0) {
gps_position->satellite_info_available = true; _gps_position->satellite_info_available = true;
} else { } else {
gps_position->satellite_info_available = false; _gps_position->satellite_info_available = false;
} }
gps_position->timestamp_satellites = hrt_absolute_time(); _gps_position->timestamp_satellites = hrt_absolute_time();
} else {
warnx("NAV_SVINFO: checksum invalid");
} }
// Reset state machine to decode next packet
decodeInit();
break; break;
} }
case NAV_VELNED: { case NAV_VELNED: {
// printf("GOT NAV_VELNED MESSAGE\n"); // printf("GOT NAV_VELNED MESSAGE\n");
if (!_waiting_for_ack) {
gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
//Check if checksum is valid and the store the gps information _gps_position->vel_m_s = (float)packet->speed * 1e-2f;
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
gps_position->vel_m_s = (float)packet->speed * 1e-2f; _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; _gps_position->vel_ned_valid = true;
gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; _gps_position->timestamp_velocity = hrt_absolute_time();
gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
gps_position->vel_ned_valid = true;
gps_position->timestamp_velocity = hrt_absolute_time();
} else {
warnx("NAV_VELNED: checksum invalid");
} }
// Reset state machine to decode next packet
decodeInit();
break; break;
} }
@@ -588,133 +655,44 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
// memcpy(_rx_buffer_part1, _rx_buffer, 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_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
// //
// //Check if checksum is valid and the store the gps information // _gps_position->satellites_visible = packet->numVis;
// if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { // _gps_position->counter++;
//
// gps_position->satellites_visible = packet->numVis;
// gps_position->counter++;
// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); // _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
// ret = 1;
// //
// } else {
// warnx("RXM_SVSI: checksum invalid\n");
// }
//
// // Reset state machine to decode next packet
// decodeInit();
// break; // break;
// } // }
case ACK_ACK: { case ACK_ACK: {
// printf("GOT ACK_ACK\n"); // printf("GOT ACK_ACK\n");
gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
//Check if checksum is valid if (_waiting_for_ack) {
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) { if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
ret = 1;
_waiting_for_ack = false; }
switch (_config_state) {
case UBX_CONFIG_STATE_PRT:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
_config_state = UBX_CONFIG_STATE_PRT_NEW_BAUDRATE;
break;
case UBX_CONFIG_STATE_PRT_NEW_BAUDRATE:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_PRT)
_config_state = UBX_CONFIG_STATE_RATE;
break;
case UBX_CONFIG_STATE_RATE:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_RATE)
_config_state = UBX_CONFIG_STATE_NAV5;
break;
case UBX_CONFIG_STATE_NAV5:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_NAV5)
_config_state = UBX_CONFIG_STATE_MSG_NAV_POSLLH;
break;
case UBX_CONFIG_STATE_MSG_NAV_POSLLH:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
_config_state = UBX_CONFIG_STATE_MSG_NAV_TIMEUTC;
break;
case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
break;
// case UBX_CONFIG_STATE_MSG_NAV_DOP:
// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
// _config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
// break;
case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
_config_state = UBX_CONFIG_STATE_MSG_NAV_SOL;
break;
case UBX_CONFIG_STATE_MSG_NAV_SOL:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
_config_state = UBX_CONFIG_STATE_MSG_NAV_VELNED;
break;
case UBX_CONFIG_STATE_MSG_NAV_VELNED:
if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
_config_state = UBX_CONFIG_STATE_CONFIGURED;
/* set the flag to tell the driver that configuration was successful */
config_needed = false;
break;
// case UBX_CONFIG_STATE_MSG_RXM_SVSI:
// if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
// _config_state = UBX_CONFIG_STATE_CONFIGURED;
// break;
default:
break;
} }
} else {
warnx("ACK_ACK: checksum invalid");
} }
// Reset state machine to decode next packet
decodeInit();
break; break;
}
case ACK_NAK: { case ACK_NAK: {
// printf("GOT ACK_NAK\n"); // printf("GOT ACK_NAK\n");
gps_bin_ack_nak_packet_t *packet = (gps_bin_ack_nak_packet_t *) _rx_buffer;
//Check if checksum is valid
if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
warnx("UBX: Received: Not Acknowledged"); warnx("UBX: Received: Not Acknowledged");
/* configuration obviously not successful */ /* configuration obviously not successful */
config_needed = true; ret = -1;
} else {
warnx("ACK_NAK: checksum invalid\n");
}
// Reset state machine to decode next packet
decodeInit();
break; break;
} }
default: //we don't know the message default: //we don't know the message
warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
decodeInit(); ret = -1;
break; break;
} }
} // end if _rx_count high enough // end if _rx_count high enough
else if (_rx_count < RECV_BUFFER_SIZE) { decode_init();
_rx_count++; return ret; //XXX?
} else {
warnx("buffer full, restarting");
decodeInit();
config_needed = true;
}
break;
default:
break;
}
return;
} }
void void
UBX::decodeInit(void) UBX::decode_init(void)
{ {
_rx_ck_a = 0; _rx_ck_a = 0;
_rx_ck_b = 0; _rx_ck_b = 0;
@@ -726,14 +704,14 @@ UBX::decodeInit(void)
} }
void void
UBX::addByteToChecksum(uint8_t b) UBX::add_byte_to_checksum(uint8_t b)
{ {
_rx_ck_a = _rx_ck_a + b; _rx_ck_a = _rx_ck_a + b;
_rx_ck_b = _rx_ck_b + _rx_ck_a; _rx_ck_b = _rx_ck_b + _rx_ck_a;
} }
void void
UBX::addChecksumToMessage(uint8_t* message, const unsigned length) UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
{ {
uint8_t ck_a = 0; uint8_t ck_a = 0;
uint8_t ck_b = 0; uint8_t ck_b = 0;
@@ -749,12 +727,12 @@ UBX::addChecksumToMessage(uint8_t* message, const unsigned length)
} }
void void
UBX::sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length) UBX::send_config_packet(const int &fd, uint8_t *packet, const unsigned length)
{ {
ssize_t ret = 0; ssize_t ret = 0;
/* Calculate the checksum now */ /* Calculate the checksum now */
addChecksumToMessage(packet, length); add_checksum_to_message(packet, length);
const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2}; const uint8_t sync_bytes[] = {UBX_SYNC1, UBX_SYNC2};
apps/drivers/gps/ubx.h
+24 -8
View File
@@ -86,6 +86,8 @@
#define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_5HZ 0x01 /**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */ #define UBX_CFG_MSG_PAYLOAD_RATE1_1HZ 0x05 /**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
#define UBX_MAX_PAYLOAD_LENGTH 500
// ************ // ************
/** the structures of the binary packets */ /** the structures of the binary packets */
#pragma pack(push, 1) #pragma pack(push, 1)
@@ -337,35 +339,49 @@ typedef enum {
class UBX : public GPS_Helper class UBX : public GPS_Helper
{ {
public: public:
UBX(); UBX(const int &fd, struct vehicle_gps_position_s *gps_position);
~UBX(); ~UBX();
void reset(void); int receive(unsigned timeout);
void configure(const int &fd, bool &baudrate_changed, unsigned &baudrate); int configure(unsigned &baudrate);
void parse(uint8_t, struct vehicle_gps_position_s*, bool &config_needed, bool &pos_updated);
private: private:
/**
* Parse the binary MTK packet
*/
int parse_char(uint8_t b);
/**
* Handle the package once it has arrived
*/
int handle_message(void);
/** /**
* Reset the parse state machine for a fresh start * Reset the parse state machine for a fresh start
*/ */
void decodeInit(void); void decode_init(void);
/** /**
* While parsing add every byte (except the sync bytes) to the checksum * While parsing add every byte (except the sync bytes) to the checksum
*/ */
void addByteToChecksum(uint8_t); void add_byte_to_checksum(uint8_t);
/** /**
* Add the two checksum bytes to an outgoing message * Add the two checksum bytes to an outgoing message
*/ */
void addChecksumToMessage(uint8_t* message, const unsigned length); void add_checksum_to_message(uint8_t* message, const unsigned length);
/** /**
* Helper to send a config packet * Helper to send a config packet
*/ */
void sendConfigPacket(const int &fd, uint8_t *packet, const unsigned length); void send_config_packet(const int &fd, uint8_t *packet, const unsigned length);
int _fd;
struct vehicle_gps_position_s *_gps_position;
ubx_config_state_t _config_state; ubx_config_state_t _config_state;
bool _waiting_for_ack; bool _waiting_for_ack;
uint8_t _clsID_needed;
uint8_t _msgID_needed;
ubx_decode_state_t _decode_state; ubx_decode_state_t _decode_state;
uint8_t _rx_buffer[RECV_BUFFER_SIZE]; uint8_t _rx_buffer[RECV_BUFFER_SIZE];
unsigned _rx_count; unsigned _rx_count;