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