dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

gps: fixed code style, more informative and clean messages

This commit is contained in:
Anton Babushkin 2013-09-21 08:59:04 +02:00
parent cd8854e622
commit 8d6a47546a
6 changed files with 142 additions and 73 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

116
src/drivers/gps/gps.cpp
View File

@ -85,7 +85,7 @@ static const int ERROR = -1;
class GPS : public device::CDev class GPS : public device::CDev
{ {
public: public:
GPS(const char* uart_path); GPS(const char *uart_path);
virtual ~GPS(); virtual ~GPS();
virtual int init(); 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), CDev("gps", GPS_DEVICE_PATH),
_task_should_exit(false), _task_should_exit(false),
_healthy(false), _healthy(false),
@ -192,6 +192,7 @@ GPS::~GPS()
/* well, kill it anyway, though this will probably crash */ /* well, kill it anyway, though this will probably crash */
if (_task != -1) if (_task != -1)
task_delete(_task); task_delete(_task);
g_dev = nullptr; g_dev = nullptr;
} }
@ -270,19 +271,24 @@ GPS::task_main()
} }
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
_Helper = new UBX(_serial_fd, &_report); _Helper = new UBX(_serial_fd, &_report);
break; break;
case GPS_DRIVER_MODE_MTK:
_Helper = new MTK(_serial_fd, &_report); case GPS_DRIVER_MODE_MTK:
break; _Helper = new MTK(_serial_fd, &_report);
case GPS_DRIVER_MODE_NMEA: break;
//_Helper = new NMEA(); //TODO: add NMEA
break; case GPS_DRIVER_MODE_NMEA:
default: //_Helper = new NMEA(); //TODO: add NMEA
break; break;
default:
break;
} }
unlock(); unlock();
if (_Helper->configure(_baudrate) == 0) { if (_Helper->configure(_baudrate) == 0) {
unlock(); unlock();
@ -294,6 +300,7 @@ GPS::task_main()
/* 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);
} else { } else {
_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report); _report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
} }
@ -310,10 +317,30 @@ GPS::task_main()
} }
if (!_healthy) { 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; _healthy = true;
} }
} }
if (_healthy) { if (_healthy) {
warnx("module lost"); warnx("module lost");
_healthy = false; _healthy = false;
@ -322,24 +349,28 @@ GPS::task_main()
lock(); lock();
} }
lock(); lock();
/* select next mode */ /* select next mode */
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
_mode = GPS_DRIVER_MODE_MTK; _mode = GPS_DRIVER_MODE_MTK;
break; break;
case GPS_DRIVER_MODE_MTK:
_mode = GPS_DRIVER_MODE_UBX; case GPS_DRIVER_MODE_MTK:
break; _mode = GPS_DRIVER_MODE_UBX;
// case GPS_DRIVER_MODE_NMEA: break;
// _mode = GPS_DRIVER_MODE_UBX;
// break; // case GPS_DRIVER_MODE_NMEA:
default: // _mode = GPS_DRIVER_MODE_UBX;
break; // break;
default:
break;
} }
} }
debug("exiting"); debug("exiting");
::close(_serial_fd); ::close(_serial_fd);
@ -361,22 +392,27 @@ void
GPS::print_info() GPS::print_info()
{ {
switch (_mode) { switch (_mode) {
case GPS_DRIVER_MODE_UBX: case GPS_DRIVER_MODE_UBX:
warnx("protocol: UBX"); warnx("protocol: UBX");
break; break;
case GPS_DRIVER_MODE_MTK:
warnx("protocol: MTK"); case GPS_DRIVER_MODE_MTK:
break; warnx("protocol: MTK");
case GPS_DRIVER_MODE_NMEA: break;
warnx("protocol: NMEA");
break; case GPS_DRIVER_MODE_NMEA:
default: warnx("protocol: NMEA");
break; break;
default:
break;
} }
warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK"); warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_healthy) ? "OK" : "NOT OK");
if (_report.timestamp_position != 0) { if (_report.timestamp_position != 0) {
warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type, 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("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 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 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); errx(1, "Could not open device path: %s\n", GPS_DEVICE_PATH);
goto fail; goto fail;
} }
exit(0); exit(0);
fail: fail:
@ -503,7 +540,7 @@ gps_main(int argc, char *argv[])
{ {
/* set to default */ /* set to default */
char* device_name = GPS_DEFAULT_UART_PORT; char *device_name = GPS_DEFAULT_UART_PORT;
/* /*
* Start/load the driver. * Start/load the driver.
@ -513,15 +550,18 @@ gps_main(int argc, char *argv[])
if (argc > 3) { if (argc > 3) {
if (!strcmp(argv[2], "-d")) { if (!strcmp(argv[2], "-d")) {
device_name = argv[3]; device_name = argv[3];
} else { } else {
goto out; goto out;
} }
} }
gps::start(device_name); gps::start(device_name);
} }
if (!strcmp(argv[1], "stop")) if (!strcmp(argv[1], "stop"))
gps::stop(); gps::stop();
/* /*
* Test the driver/device. * Test the driver/device.
*/ */

7
src/drivers/gps/gps_helper.cpp
View File

@ -87,13 +87,15 @@ 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); 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;
} }
struct termios uart_config; struct termios uart_config;
int termios_state; int termios_state;
/* fill the struct for the new configuration */ /* 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); warnx("ERROR setting config: %d (cfsetispeed)\n", termios_state);
return -1; return -1;
} }
if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) { if ((termios_state = cfsetospeed(&uart_config, speed)) < 0) {
warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state); warnx("ERROR setting config: %d (cfsetospeed)\n", termios_state);
return -1; return -1;
} }
if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) { if ((termios_state = tcsetattr(fd, TCSANOW, &uart_config)) < 0) {
warnx("ERROR setting baudrate (tcsetattr)\n"); warnx("ERROR setting baudrate (tcsetattr)\n");
return -1; return -1;
} }
/* XXX if resetting the parser here, ensure it does exist (check for null pointer) */ /* XXX if resetting the parser here, ensure it does exist (check for null pointer) */
return 0; return 0;
} }

2
src/drivers/gps/gps_helper.h
View File

@ -33,7 +33,7 @@
* *
****************************************************************************/ ****************************************************************************/
/** /**
* @file gps_helper.h * @file gps_helper.h
*/ */

26
src/drivers/gps/mtk.cpp
View File

@ -48,9 +48,9 @@
MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) : MTK::MTK(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd), _fd(fd),
_gps_position(gps_position), _gps_position(gps_position),
_mtk_revision(0) _mtk_revision(0)
{ {
decode_init(); decode_init();
} }
@ -73,24 +73,28 @@ MTK::configure(unsigned &baudrate)
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))) {
@ -128,12 +132,15 @@ MTK::receive(unsigned timeout)
handle_message(packet); handle_message(packet);
return 1; return 1;
} }
/* in case we keep trying but only get crap from GPS */ /* 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; return -1;
} }
j++; j++;
} }
/* everything is read */ /* everything is read */
j = count = 0; j = count = 0;
} }
@ -181,6 +188,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
if (b == MTK_SYNC1_V16) { if (b == MTK_SYNC1_V16) {
_decode_state = MTK_DECODE_GOT_CK_A; _decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 16; _mtk_revision = 16;
} else if (b == MTK_SYNC1_V19) { } else if (b == MTK_SYNC1_V19) {
_decode_state = MTK_DECODE_GOT_CK_A; _decode_state = MTK_DECODE_GOT_CK_A;
_mtk_revision = 19; _mtk_revision = 19;
@ -201,7 +209,7 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
add_byte_to_checksum(b); add_byte_to_checksum(b);
// Fill packet buffer // Fill packet buffer
((uint8_t*)(&packet))[_rx_count] = b; ((uint8_t *)(&packet))[_rx_count] = b;
_rx_count++; _rx_count++;
/* Packet size minus checksum, XXX ? */ /* Packet size minus checksum, XXX ? */
@ -209,14 +217,17 @@ MTK::parse_char(uint8_t b, gps_mtk_packet_t &packet)
/* Compare checksum */ /* Compare checksum */
if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) { if (_rx_ck_a == packet.ck_a && _rx_ck_b == packet.ck_b) {
ret = 1; ret = 1;
} else { } else {
warnx("MTK Checksum invalid"); warnx("MTK Checksum invalid");
ret = -1; ret = -1;
} }
// Reset state machine to decode next packet // Reset state machine to decode next packet
decode_init(); decode_init();
} }
} }
return ret; return ret;
} }
@ -226,19 +237,22 @@ 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;

58
src/drivers/gps/ubx.cpp
View File

@ -60,13 +60,14 @@
#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK #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_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 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) : UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
_fd(fd), _fd(fd),
_gps_position(gps_position), _gps_position(gps_position),
_configured(false), _configured(false),
_waiting_for_ack(false), _waiting_for_ack(false),
_disable_cmd_counter(0) _disable_cmd_last(0)
{ {
decode_init(); decode_init();
} }
@ -191,35 +192,35 @@ UBX::configure(unsigned &baudrate)
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { 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; return 1;
} }
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { 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; return 1;
} }
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { 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; return 1;
} }
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { 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; return 1;
} }
configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { 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; return 1;
} }
@ -271,11 +272,18 @@ UBX::receive(unsigned timeout)
if (ret < 0) { if (ret < 0) {
/* something went wrong when polling */ /* something went wrong when polling */
warnx("ubx: poll error");
return -1; return -1;
} else if (ret == 0) { } else if (ret == 0) {
/* return success after short delay after receiving a packet or timeout after long delay */ /* 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) { } else if (ret > 0) {
/* if we have new data from GPS, go handle it */ /* 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 */ /* pass received bytes to the packet decoder */
for (int i = 0; i < count; i++) { for (int i = 0; i < count; i++) {
if (parse_char(buf[i]) > 0) { 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) if (handle_message() > 0)
handled = true; handled = true;
} }
@ -303,6 +309,7 @@ UBX::receive(unsigned timeout)
/* abort after timeout if no useful packets received */ /* abort after timeout if no useful packets received */
if (time_started + timeout * 1000 < hrt_absolute_time()) { if (time_started + timeout * 1000 < hrt_absolute_time()) {
warnx("ubx: timeout - no useful messages");
return -1; return -1;
} }
} }
@ -453,16 +460,16 @@ UBX::handle_message()
timeinfo.tm_min = packet->min; timeinfo.tm_min = packet->min;
timeinfo.tm_sec = packet->sec; timeinfo.tm_sec = packet->sec;
time_t epoch = mktime(&timeinfo); time_t epoch = mktime(&timeinfo);
#ifndef CONFIG_RTC #ifndef CONFIG_RTC
//Since we lack a hardware RTC, set the system time clock based on GPS UTC //Since we lack a hardware RTC, set the system time clock based on GPS UTC
//TODO generalize this by moving into gps.cpp? //TODO generalize this by moving into gps.cpp?
timespec ts; timespec ts;
ts.tv_sec = epoch; ts.tv_sec = epoch;
ts.tv_nsec = packet->time_nanoseconds; ts.tv_nsec = packet->time_nanoseconds;
clock_settime(CLOCK_REALTIME,&ts); clock_settime(CLOCK_REALTIME, &ts);
#endif #endif
_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();
@ -564,10 +571,13 @@ UBX::handle_message()
if (ret == 0) { if (ret == 0) {
/* message not handled */ /* 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);
if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { hrt_abstime t = hrt_absolute_time();
if (t > _disable_cmd_last + DISABLE_MSG_INTERVAL) {
/* don't attempt for every message to disable, some might not be disabled */ /* 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); warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
configure_message_rate(_message_class, _message_id, 0); 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; 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 - 2] = ck_a;
message[length - 1] = ck_b; 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; ssize_t ret = 0;
/* Calculate the checksum now */ /* calculate the checksum now */
add_checksum_to_message(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};
/* Start with the two sync bytes */ /* start with the two sync bytes */
ret += write(fd, sync_bytes, sizeof(sync_bytes)); ret += write(fd, sync_bytes, sizeof(sync_bytes));
ret += write(fd, packet, length); 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? 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 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 *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
add_checksum((uint8_t *)msg, size, 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_class_needed = msg_class;
_message_id_needed = msg_id; _message_id_needed = msg_id;

6
src/drivers/gps/ubx.h
View File

@ -347,7 +347,7 @@ private:
/** /**
* Add the two checksum bytes to an outgoing message * 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 * 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 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); int wait_for_ack(unsigned timeout);
@ -376,7 +376,7 @@ private:
uint8_t _message_class; uint8_t _message_class;
uint8_t _message_id; uint8_t _message_id;
unsigned _payload_size; unsigned _payload_size;
uint8_t _disable_cmd_counter; uint8_t _disable_cmd_last;
}; };
#endif /* UBX_H_ */ #endif /* UBX_H_ */