Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

uncrustify libraries/AP_GPS/AP_GPS_UBLOX.cpp

This commit is contained in:
uncrustify 2012-08-21 19:19:52 -07:00 committed by Pat Hickey
parent 7de08acf09
commit 5ed90851e3
1 changed files with 198 additions and 198 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

394
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -14,15 +14,15 @@
#include <FastSerial.h> #include <FastSerial.h>
#if UBLOX_DEBUGGING #if UBLOX_DEBUGGING
# define Debug(fmt, args...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__ , ##args); delay(0); } while(0) # define Debug(fmt, args ...) do {Serial.printf("%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); delay(0); } while(0)
#else #else
# define Debug(fmt, args...) # define Debug(fmt, args ...)
#endif #endif
#include "AP_GPS_UBLOX.h" #include "AP_GPS_UBLOX.h"
#include <stdint.h> #include <stdint.h>
const prog_char AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY; const prog_char AP_GPS_UBLOX::_ublox_set_binary[] PROGMEM = UBLOX_SET_BINARY;
const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary); const uint8_t AP_GPS_UBLOX::_ublox_set_binary_size = sizeof(AP_GPS_UBLOX::_ublox_set_binary);
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
@ -44,10 +44,10 @@ AP_GPS_UBLOX::init(enum GPS_Engine_Setting nav_setting)
_epoch = TIME_OF_WEEK; _epoch = TIME_OF_WEEK;
idleTimeout = 1200; idleTimeout = 1200;
// configure the GPS for the messages we want // configure the GPS for the messages we want
_configure_gps(); _configure_gps();
_nav_setting = nav_setting; _nav_setting = nav_setting;
} }
// Process bytes available from the stream // Process bytes available from the stream
@ -62,107 +62,107 @@ AP_GPS_UBLOX::init(enum GPS_Engine_Setting nav_setting)
bool bool
AP_GPS_UBLOX::read(void) AP_GPS_UBLOX::read(void)
{ {
uint8_t data; uint8_t data;
int16_t numc; int16_t numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
for (int16_t i = 0; i < numc; i++) { // Process bytes received for (int16_t i = 0; i < numc; i++) { // Process bytes received
// read the next byte // read the next byte
data = _port->read(); data = _port->read();
switch(_step) { switch(_step) {
// Message preamble detection // Message preamble detection
// //
// If we fail to match any of the expected bytes, we reset // If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as // the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our // the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less // chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing // likely that we will be fooled by the preamble appearing
// as data in some other message. // as data in some other message.
// //
case 1: case 1:
if (PREAMBLE2 == data) { if (PREAMBLE2 == data) {
_step++; _step++;
break; break;
} }
_step = 0; _step = 0;
Debug("reset %u", __LINE__); Debug("reset %u", __LINE__);
// FALLTHROUGH // FALLTHROUGH
case 0: case 0:
if(PREAMBLE1 == data) if(PREAMBLE1 == data)
_step++; _step++;
break; break;
// Message header processing // Message header processing
// //
// We sniff the class and message ID to decide whether we // We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard // are going to gather the message bytes or just discard
// them. // them.
// //
// We always collect the length so that we can avoid being // We always collect the length so that we can avoid being
// fooled by preamble bytes in messages. // fooled by preamble bytes in messages.
// //
case 2: case 2:
_step++; _step++;
_class = data; _class = data;
_ck_b = _ck_a = data; // reset the checksum accumulators _ck_b = _ck_a = data; // reset the checksum accumulators
break; break;
case 3: case 3:
_step++; _step++;
_ck_b += (_ck_a += data); // checksum byte _ck_b += (_ck_a += data); // checksum byte
_msg_id = data; _msg_id = data;
break; break;
case 4: case 4:
_step++; _step++;
_ck_b += (_ck_a += data); // checksum byte _ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte _payload_length = data; // payload length low byte
break; break;
case 5: case 5:
_step++; _step++;
_ck_b += (_ck_a += data); // checksum byte _ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)(data<<8); _payload_length += (uint16_t)(data<<8);
if (_payload_length > 512) { if (_payload_length > 512) {
Debug("large payload %u", (unsigned)_payload_length); Debug("large payload %u", (unsigned)_payload_length);
// assume very large payloads are line noise // assume very large payloads are line noise
_payload_length = 0; _payload_length = 0;
_step = 0; _step = 0;
} }
_payload_counter = 0; // prepare to receive payload _payload_counter = 0; // prepare to receive payload
break; break;
// Receive message data // Receive message data
// //
case 6: case 6:
_ck_b += (_ck_a += data); // checksum byte _ck_b += (_ck_a += data); // checksum byte
if (_payload_counter < sizeof(_buffer)) { if (_payload_counter < sizeof(_buffer)) {
_buffer.bytes[_payload_counter] = data; _buffer.bytes[_payload_counter] = data;
} }
if (++_payload_counter == _payload_length) if (++_payload_counter == _payload_length)
_step++; _step++;
break; break;
// Checksum and message processing // Checksum and message processing
// //
case 7: case 7:
_step++; _step++;
if (_ck_a != data) { if (_ck_a != data) {
Debug("bad cka %x should be %x", data, _ck_a); Debug("bad cka %x should be %x", data, _ck_a);
_step = 0; // bad checksum _step = 0; // bad checksum
} }
break; break;
case 8: case 8:
_step = 0; _step = 0;
if (_ck_b != data) { if (_ck_b != data) {
Debug("bad ckb %x should be %x", data, _ck_b); Debug("bad ckb %x should be %x", data, _ck_b);
break; // bad checksum break; // bad checksum
} }
if (_parse_gps()) { if (_parse_gps()) {
parsed = true; parsed = true;
} }
} }
} }
@ -174,184 +174,184 @@ AP_GPS_UBLOX::read(void)
bool bool
AP_GPS_UBLOX::_parse_gps(void) AP_GPS_UBLOX::_parse_gps(void)
{ {
if (_class == CLASS_ACK) { if (_class == CLASS_ACK) {
Debug("ACK %u", (unsigned)_msg_id); Debug("ACK %u", (unsigned)_msg_id);
return false;
}
if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
if (_nav_setting != GPS_ENGINE_NONE &&
_buffer.nav_settings.dynModel != _nav_setting) {
// we've received the current nav settings, change the engine
// settings and send them back
Debug("Changing engine setting from %u to %u\n",
(unsigned)_buffer.nav_settings.dynModel, (unsigned)_nav_setting);
_buffer.nav_settings.dynModel = _nav_setting;
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
}
return false;
}
if (_class != CLASS_NAV) {
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
if (++_disable_counter == 0) {
// disable future sends of this message id, but
// only do this every 256 messages, as some
// message types can't be disabled and we don't
// want to get into an ack war
Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
_configure_message_rate(_class, _msg_id, 0);
}
return false;
}
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
fix = next_fix;
_new_position = true;
break;
case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
if (!next_fix) {
fix = false;
}
break;
case MSG_SOL:
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix) {
fix = false;
}
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
Debug("MSG_VELNED");
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
_have_raw_velocity = true;
_vel_north = _buffer.velned.ned_north;
_vel_east = _buffer.velned.ned_east;
_vel_down = _buffer.velned.ned_down;
_new_speed = true;
break;
default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) {
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
_configure_message_rate(CLASS_NAV, _msg_id, 0);
}
return false; return false;
} }
// we only return true when we get new position and speed data if (_class == CLASS_CFG && _msg_id == MSG_CFG_NAV_SETTINGS) {
// this ensures we don't use stale data if (_nav_setting != GPS_ENGINE_NONE &&
if (_new_position && _new_speed) { _buffer.nav_settings.dynModel != _nav_setting) {
_new_speed = _new_position = false; // we've received the current nav settings, change the engine
return true; // settings and send them back
} Debug("Changing engine setting from %u to %u\n",
return false; (unsigned)_buffer.nav_settings.dynModel, (unsigned)_nav_setting);
_buffer.nav_settings.dynModel = _nav_setting;
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS,
&_buffer.nav_settings,
sizeof(_buffer.nav_settings));
}
return false;
}
if (_class != CLASS_NAV) {
Debug("Unexpected message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
if (++_disable_counter == 0) {
// disable future sends of this message id, but
// only do this every 256 messages, as some
// message types can't be disabled and we don't
// want to get into an ack war
Debug("Disabling message 0x%02x 0x%02x", (unsigned)_class, (unsigned)_msg_id);
_configure_message_rate(_class, _msg_id, 0);
}
return false;
}
switch (_msg_id) {
case MSG_POSLLH:
Debug("MSG_POSLLH next_fix=%u", next_fix);
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
fix = next_fix;
_new_position = true;
break;
case MSG_STATUS:
Debug("MSG_STATUS fix_status=%u fix_type=%u",
_buffer.status.fix_status,
_buffer.status.fix_type);
next_fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
if (!next_fix) {
fix = false;
}
break;
case MSG_SOL:
Debug("MSG_SOL fix_status=%u fix_type=%u",
_buffer.solution.fix_status,
_buffer.solution.fix_type);
next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
if (!next_fix) {
fix = false;
}
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
Debug("MSG_VELNED");
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
_have_raw_velocity = true;
_vel_north = _buffer.velned.ned_north;
_vel_east = _buffer.velned.ned_east;
_vel_down = _buffer.velned.ned_down;
_new_speed = true;
break;
default:
Debug("Unexpected NAV message 0x%02x", (unsigned)_msg_id);
if (++_disable_counter == 0) {
Debug("Disabling NAV message 0x%02x", (unsigned)_msg_id);
_configure_message_rate(CLASS_NAV, _msg_id, 0);
}
return false;
}
// we only return true when we get new position and speed data
// this ensures we don't use stale data
if (_new_position && _new_speed) {
_new_speed = _new_position = false;
return true;
}
return false;
} }
// UBlox auto configuration // UBlox auto configuration
/* /*
update checksum for a set of bytes * update checksum for a set of bytes
*/ */
void void
AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b) AP_GPS_UBLOX::_update_checksum(uint8_t *data, uint8_t len, uint8_t &ck_a, uint8_t &ck_b)
{ {
while (len--) { while (len--) {
ck_a += *data; ck_a += *data;
ck_b += ck_a; ck_b += ck_a;
data++; data++;
} }
} }
/* /*
send a ublox message * send a ublox message
*/ */
void void
AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) AP_GPS_UBLOX::_send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
{ {
struct ubx_header header; struct ubx_header header;
uint8_t ck_a=0, ck_b=0; uint8_t ck_a=0, ck_b=0;
header.preamble1 = PREAMBLE1; header.preamble1 = PREAMBLE1;
header.preamble2 = PREAMBLE2; header.preamble2 = PREAMBLE2;
header.msg_class = msg_class; header.msg_class = msg_class;
header.msg_id = msg_id; header.msg_id = msg_id;
header.length = size; header.length = size;
_update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); _update_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
_update_checksum((uint8_t *)msg, size, ck_a, ck_b); _update_checksum((uint8_t *)msg, size, ck_a, ck_b);
_port->write((const uint8_t *)&header, sizeof(header)); _port->write((const uint8_t *)&header, sizeof(header));
_port->write((const uint8_t *)msg, size); _port->write((const uint8_t *)msg, size);
_port->write((const uint8_t *)&ck_a, 1); _port->write((const uint8_t *)&ck_a, 1);
_port->write((const uint8_t *)&ck_b, 1); _port->write((const uint8_t *)&ck_b, 1);
} }
/* /*
configure a UBlox GPS for the given message rate for a specific * configure a UBlox GPS for the given message rate for a specific
message class and msg_id * message class and msg_id
*/ */
void void
AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) AP_GPS_UBLOX::_configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
{ {
struct ubx_cfg_msg_rate msg; struct ubx_cfg_msg_rate msg;
msg.msg_class = msg_class; msg.msg_class = msg_class;
msg.msg_id = msg_id; msg.msg_id = msg_id;
msg.rate = rate; msg.rate = rate;
_send_message(CLASS_CFG, MSG_CFG_SET_RATE, &msg, sizeof(msg)); _send_message(CLASS_CFG, MSG_CFG_SET_RATE, &msg, sizeof(msg));
} }
/* /*
configure a UBlox GPS for the given message rate * configure a UBlox GPS for the given message rate
*/ */
void void
AP_GPS_UBLOX::_configure_gps(void) AP_GPS_UBLOX::_configure_gps(void)
{ {
struct ubx_cfg_nav_rate msg; struct ubx_cfg_nav_rate msg;
const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U}; const unsigned baudrates[4] = {9600U, 19200U, 38400U, 57600U};
FastSerial *_fs = (FastSerial *)_port; FastSerial *_fs = (FastSerial *)_port;
// the GPS may be setup for a different baud rate. This ensures // the GPS may be setup for a different baud rate. This ensures
// it gets configured correctly // it gets configured correctly
for (uint8_t i=0; i<4; i++) { for (uint8_t i=0; i<4; i++) {
_fs->begin(baudrates[i]); _fs->begin(baudrates[i]);
_write_progstr_block(_fs, _ublox_set_binary, _ublox_set_binary_size); _write_progstr_block(_fs, _ublox_set_binary, _ublox_set_binary_size);
while (_fs->tx_pending()) delay(1); while (_fs->tx_pending()) delay(1);
} }
_fs->begin(38400U); _fs->begin(38400U);
// ask for navigation solutions every 200ms // ask for navigation solutions every 200ms
msg.measure_rate_ms = 200; msg.measure_rate_ms = 200;
msg.nav_rate = 1; msg.nav_rate = 1;
msg.timeref = 0; // UTC time msg.timeref = 0; // UTC time
_send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg)); _send_message(CLASS_CFG, MSG_CFG_RATE, &msg, sizeof(msg));
// ask for the messages we parse to be sent on every navigation solution // ask for the messages we parse to be sent on every navigation solution
_configure_message_rate(CLASS_NAV, MSG_POSLLH, 1); _configure_message_rate(CLASS_NAV, MSG_POSLLH, 1);
_configure_message_rate(CLASS_NAV, MSG_STATUS, 1); _configure_message_rate(CLASS_NAV, MSG_STATUS, 1);
_configure_message_rate(CLASS_NAV, MSG_SOL, 1); _configure_message_rate(CLASS_NAV, MSG_SOL, 1);
_configure_message_rate(CLASS_NAV, MSG_VELNED, 1); _configure_message_rate(CLASS_NAV, MSG_VELNED, 1);
// ask for the current navigation settings // ask for the current navigation settings
_send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0); _send_message(CLASS_CFG, MSG_CFG_NAV_SETTINGS, NULL, 0);
} }