Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-11 02:13:57 -04:00
Code Issues Packages Projects Releases Wiki Activity

Cleaned up AP_GPS formatting.

Browse Source
This commit is contained in:
James Goppert 2011-10-28 14:52:50 -04:00
parent d97692de9d
commit 14d27b1e4c
28 changed files with 1764 additions and 1756 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/scripts/format.sh
View File

@ -10,3 +10,4 @@ format ArduRover
format ArduBoat format ArduBoat
format libraries/APO format libraries/APO
format libraries/AP_Common format libraries/AP_Common
format libraries/AP_GPS

72
libraries/AP_GPS/AP_GPS_406.cpp
View File

@ -23,35 +23,35 @@ AP_GPS_406::AP_GPS_406(Stream *s) : AP_GPS_SIRF(s)
// Public Methods //////////////////////////////////////////////////////////////////// // Public Methods ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void) void AP_GPS_406::init(void)
{ {
_change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate _change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate
_configure_gps(); // Function to configure GPS, to output only the desired msg's _configure_gps(); // Function to configure GPS, to output only the desired msg's
AP_GPS_SIRF::init(); // let the superclass do anything it might need here AP_GPS_SIRF::init(); // let the superclass do anything it might need here
idleTimeout = 1200; idleTimeout = 1200;
} }
// Private Methods ////////////////////////////////////////////////////////////// // Private Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_406::_configure_gps(void) AP_GPS_406::_configure_gps(void)
{ {
const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00}; const uint8_t gps_header[] = {0xA0, 0xA2, 0x00, 0x08, 0xA6, 0x00};
const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B}; const uint8_t gps_payload[] = {0x02, 0x04, 0x07, 0x09, 0x1B};
const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1}; const uint8_t gps_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3}; const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++){ for(int z = 0; z < 2; z++) {
for(int x = 0; x < 5; x++){ for(int x = 0; x < 5; x++) {
_port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg.. _port->write(gps_header, sizeof(gps_header)); // Prints the msg header, is the same header for all msg..
_port->write(gps_payload[x]); // Prints the payload, is not the same for every msg _port->write(gps_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++) // Prints 6 zeros for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0); _port->write((uint8_t)0);
_port->write(gps_checksum[x]); // Print the Checksum _port->write(gps_checksum[x]); // Print the Checksum
_port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's. _port->write(gps_ender[0]); // Print the Ender of the string, is same on all msg's.
_port->write(gps_ender[1]); // ender _port->write(gps_ender[1]); // ender
} }
} }
} }
// The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary // The EM406 defalts to NMEA at 4800bps. We want to switch it to SiRF binary
@ -60,24 +60,24 @@ AP_GPS_406::_configure_gps(void)
// The change is sticky, but only for as long as the internal supercap holds // The change is sticky, but only for as long as the internal supercap holds
// settings (usually less than a week). // settings (usually less than a week).
// //
void void
AP_GPS_406::_change_to_sirf_protocol(void) AP_GPS_406::_change_to_sirf_protocol(void)
{ {
FastSerial *fs = (FastSerial *)_port; // this is a bit grody... FastSerial *fs = (FastSerial *)_port; // this is a bit grody...
fs->begin(4800); fs->begin(4800);
delay(300); delay(300);
_port->print(init_str); _port->print(init_str);
delay(300); delay(300);
fs->begin(9600); fs->begin(9600);
delay(300); delay(300);
_port->print(init_str); _port->print(init_str);
delay(300); delay(300);
fs->begin(GPS_406_BITRATE); fs->begin(GPS_406_BITRATE);
delay(300); delay(300);
_port->print(init_str); _port->print(init_str);
delay(300); delay(300);
} }

8
libraries/AP_GPS/AP_GPS_406.h
View File

@ -20,12 +20,12 @@ class AP_GPS_406 : public AP_GPS_SIRF
{ {
public: public:
// Methods // Methods
AP_GPS_406(Stream *port); AP_GPS_406(Stream *port);
virtual void init(void); virtual void init(void);
private: private:
void _change_to_sirf_protocol(void); void _change_to_sirf_protocol(void);
void _configure_gps(void); void _configure_gps(void);
}; };
#endif #endif

288
libraries/AP_GPS/AP_GPS_Auto.cpp
View File

@ -25,9 +25,9 @@ const prog_char AP_GPS_Auto::_sirf_set_binary[] PROGMEM = SIRF_SET_BINARY;
AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) : AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) :
GPS(s), GPS(s),
_fs(s), _fs(s),
_gps(gps) _gps(gps)
{ {
} }
@ -36,8 +36,8 @@ AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) :
void void
AP_GPS_Auto::init(void) AP_GPS_Auto::init(void)
{ {
idleTimeout = 1200; idleTimeout = 1200;
if (callback == NULL) callback = delay; if (callback == NULL) callback = delay;
} }
// Called the first time that a client tries to kick the GPS to update. // Called the first time that a client tries to kick the GPS to update.
@ -53,41 +53,41 @@ AP_GPS_Auto::init(void)
bool bool
AP_GPS_Auto::read(void) AP_GPS_Auto::read(void)
{ {
GPS *gps; GPS *gps;
uint8_t i; uint8_t i;
unsigned long then; unsigned long then;
// Loop through possible baudrates trying to detect a GPS at one of them. // Loop through possible baudrates trying to detect a GPS at one of them.
// //
// Note that we need to have a FastSerial rather than a Stream here because // Note that we need to have a FastSerial rather than a Stream here because
// Stream has no idea of line speeds. FastSerial is quite OK with us calling // Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times. // ::begin any number of times.
// //
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) { for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
_fs->begin(baudrates[i]); _fs->begin(baudrates[i]);
if (NULL != (gps = _detect())) { if (NULL != (gps = _detect())) {
// configure the detected GPS and give it a chance to listen to its device // configure the detected GPS and give it a chance to listen to its device
gps->init(); gps->init();
then = millis(); then = millis();
while ((millis() - then) < 1200) { while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done // if we get a successful update from the GPS, we are done
gps->new_data = false; gps->new_data = false;
gps->update(); gps->update();
if (gps->new_data) { if (gps->new_data) {
Serial.println_P(PSTR("OK")); Serial.println_P(PSTR("OK"));
*_gps = gps; *_gps = gps;
return true; return true;
} }
} }
// GPS driver failed to parse any data from GPS, // GPS driver failed to parse any data from GPS,
// delete the driver and continue the process. // delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying")); Serial.println_P(PSTR("failed, retrying"));
delete gps; delete gps;
} }
} }
return false; return false;
} }
// //
@ -96,126 +96,126 @@ AP_GPS_Auto::read(void)
GPS * GPS *
AP_GPS_Auto::_detect(void) AP_GPS_Auto::_detect(void)
{ {
unsigned long then; unsigned long then;
int fingerprint[4]; int fingerprint[4];
int tries; int tries;
GPS *gps; GPS *gps;
// //
// Loop attempting to detect a recognized GPS // Loop attempting to detect a recognized GPS
// //
Serial.print('G'); Serial.print('G');
gps = NULL; gps = NULL;
for (tries = 0; tries < 2; tries++) { for (tries = 0; tries < 2; tries++) {
// //
// Empty the serial buffer and wait for 50ms of quiet. // Empty the serial buffer and wait for 50ms of quiet.
// //
// XXX We can detect babble by counting incoming characters, but // XXX We can detect babble by counting incoming characters, but
// what would we do about it? // what would we do about it?
// //
_port->flush(); _port->flush();
then = millis(); then = millis();
do { do {
callback(1); callback(1);
if (_port->available()) { if (_port->available()) {
then = millis(); then = millis();
_port->read(); _port->read();
} }
} while ((millis() - then) < 50); } while ((millis() - then) < 50);
// //
// Collect four characters to fingerprint a device // Collect four characters to fingerprint a device
// //
// If we take more than 1200ms to receive four characters, abort. // If we take more than 1200ms to receive four characters, abort.
// This will normally only be the case where there is no GPS attached. // This will normally only be the case where there is no GPS attached.
// //
while (_port->available() < 4) { while (_port->available() < 4) {
callback(1); callback(1);
if ((millis() - then) > 1200) { if ((millis() - then) > 1200) {
Serial.print('!'); Serial.print('!');
return NULL; return NULL;
} }
} }
fingerprint[0] = _port->read(); fingerprint[0] = _port->read();
fingerprint[1] = _port->read(); fingerprint[1] = _port->read();
fingerprint[2] = _port->read(); fingerprint[2] = _port->read();
fingerprint[3] = _port->read(); fingerprint[3] = _port->read();
// //
// ublox or MTK in DIYD binary mode (whose smart idea was // ublox or MTK in DIYD binary mode (whose smart idea was
// it to make the MTK look sort-of like it was talking UBX?) // it to make the MTK look sort-of like it was talking UBX?)
// //
if ((0xb5 == fingerprint[0]) && if ((0xb5 == fingerprint[0]) &&
(0x62 == fingerprint[1]) && (0x62 == fingerprint[1]) &&
(0x01 == fingerprint[2])) { (0x01 == fingerprint[2])) {
// message 5 is MTK pretending to talk UBX // message 5 is MTK pretending to talk UBX
if (0x05 == fingerprint[3]) { if (0x05 == fingerprint[3]) {
gps = new AP_GPS_MTK(_port); gps = new AP_GPS_MTK(_port);
Serial.print_P(PSTR(" MTK1.4 ")); Serial.print_P(PSTR(" MTK1.4 "));
break; break;
} }
// any other message is ublox // any other message is ublox
gps = new AP_GPS_UBLOX(_port); gps = new AP_GPS_UBLOX(_port);
Serial.print_P(PSTR(" ublox ")); Serial.print_P(PSTR(" ublox "));
break; break;
} }
// //
// MTK v1.6 // MTK v1.6
// //
if ((0xd0 == fingerprint[0]) && if ((0xd0 == fingerprint[0]) &&
(0xdd == fingerprint[1]) && (0xdd == fingerprint[1]) &&
(0x20 == fingerprint[2])) { (0x20 == fingerprint[2])) {
gps = new AP_GPS_MTK16(_port); gps = new AP_GPS_MTK16(_port);
Serial.print_P(PSTR(" MTK1.6 ")); Serial.print_P(PSTR(" MTK1.6 "));
break; break;
} }
// //
// SIRF in binary mode // SIRF in binary mode
// //
if ((0xa0 == fingerprint[0]) && if ((0xa0 == fingerprint[0]) &&
(0xa2 == fingerprint[1])) { (0xa2 == fingerprint[1])) {
gps = new AP_GPS_SIRF(_port); gps = new AP_GPS_SIRF(_port);
Serial.print_P(PSTR(" SiRF ")); Serial.print_P(PSTR(" SiRF "));
break; break;
} }
// //
// If we haven't spammed the various init strings, send them now // If we haven't spammed the various init strings, send them now
// and retry to avoid a false-positive on the NMEA detector. // and retry to avoid a false-positive on the NMEA detector.
// //
if (0 == tries) { if (0 == tries) {
Serial.print('*'); Serial.print('*');
// use the FastSerial port handle so that we can use PROGMEM strings // use the FastSerial port handle so that we can use PROGMEM strings
_fs->println_P((const prog_char_t *)_mtk_set_binary); _fs->println_P((const prog_char_t *)_mtk_set_binary);
_fs->println_P((const prog_char_t *)_ublox_set_binary); _fs->println_P((const prog_char_t *)_ublox_set_binary);
_fs->println_P((const prog_char_t *)_sirf_set_binary); _fs->println_P((const prog_char_t *)_sirf_set_binary);
// give the GPS time to react to the settings // give the GPS time to react to the settings
callback(100); callback(100);
continue; continue;
} else { } else {
Serial.print('?'); Serial.print('?');
} }
#if WITH_NMEA_MODE #if WITH_NMEA_MODE
// //
// Something talking NMEA // Something talking NMEA
// //
if (('$' == fingerprint[0]) && if (('$' == fingerprint[0]) &&
(('G' == fingerprint[1]) || ('P' == fingerprint[1]))) { (('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
// XXX this may be a bit presumptive, might want to give the GPS a couple of // XXX this may be a bit presumptive, might want to give the GPS a couple of
// iterations around the loop to react to init strings? // iterations around the loop to react to init strings?
gps = new AP_GPS_NMEA(_port); gps = new AP_GPS_NMEA(_port);
break; break;
} }
#endif #endif
} }
return(gps); return(gps);
} }

54
libraries/AP_GPS/AP_GPS_Auto.h
View File

@ -12,40 +12,40 @@
class AP_GPS_Auto : public GPS class AP_GPS_Auto : public GPS
{ {
public: public:
/// Constructor /// Constructor
/// ///
/// @note The stream is expected to be set up and configured for the /// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called. /// correct bitrate before ::init is called.
/// ///
/// @param port Stream connected to the GPS module. /// @param port Stream connected to the GPS module.
/// @param ptr Pointer to a GPS * that will be fixed up by ::init /// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected. /// when the GPS type has been detected.
/// ///
AP_GPS_Auto(FastSerial *s, GPS **gps); AP_GPS_Auto(FastSerial *s, GPS **gps);
/// Dummy init routine, does nothing /// Dummy init routine, does nothing
virtual void init(void); virtual void init(void);
/// Detect and initialise the attached GPS unit. Updates the /// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected. /// pointer passed into the constructor when a GPS is detected.
/// ///
virtual bool read(void); virtual bool read(void);
private: private:
/// Copy of the port, known at construction time to be a real FastSerial port. /// Copy of the port, known at construction time to be a real FastSerial port.
FastSerial *_fs; FastSerial *_fs;
/// global GPS driver pointer, updated by auto-detection /// global GPS driver pointer, updated by auto-detection
/// ///
GPS **_gps; GPS **_gps;
/// low-level auto-detect routine /// low-level auto-detect routine
/// ///
GPS *_detect(void); GPS *_detect(void);
static const prog_char _mtk_set_binary[]; static const prog_char _mtk_set_binary[];
static const prog_char _ublox_set_binary[]; static const prog_char _ublox_set_binary[];
static const prog_char _sirf_set_binary[]; static const prog_char _sirf_set_binary[];
}; };
#endif #endif

18
libraries/AP_GPS/AP_GPS_HIL.cpp
View File

@ -21,21 +21,21 @@ AP_GPS_HIL::AP_GPS_HIL(Stream *s) : GPS(s)
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_HIL::init(void) void AP_GPS_HIL::init(void)
{ {
idleTimeout = 1200; idleTimeout = 1200;
} }
bool AP_GPS_HIL::read(void) bool AP_GPS_HIL::read(void)
{ {
bool result = _updated; bool result = _updated;
// return true once for each update pushed in // return true once for each update pushed in
_updated = false; _updated = false;
return result; return result;
} }
void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude, void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats)
{ {
time = _time; time = _time;
latitude = _latitude*1.0e7; latitude = _latitude*1.0e7;
@ -46,7 +46,7 @@ void AP_GPS_HIL::setHIL(long _time, float _latitude, float _longitude, float _al
speed_3d = _speed_3d*1.0e2; speed_3d = _speed_3d*1.0e2;
num_sats = _num_sats; num_sats = _num_sats;
fix = true; fix = true;
new_data = true; new_data = true;
_updated = true; _updated = true;
} }

10
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -16,9 +16,9 @@
class AP_GPS_HIL : public GPS { class AP_GPS_HIL : public GPS {
public: public:
AP_GPS_HIL(Stream *s); AP_GPS_HIL(Stream *s);
virtual void init(void); virtual void init(void);
virtual bool read(void); virtual bool read(void);
/** /**
* Hardware in the loop set function * Hardware in the loop set function
@ -31,10 +31,10 @@ public:
* @param altitude - altitude in meters * @param altitude - altitude in meters
*/ */
virtual void setHIL(long time, float latitude, float longitude, float altitude, virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
private: private:
bool _updated; bool _updated;
}; };
#endif // AP_GPS_HIL_H #endif // AP_GPS_HIL_H

282
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -15,7 +15,7 @@
Methods: Methods:
init() : GPS initialization init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties: Properties:
lattitude : lattitude * 10000000 (long value) lattitude : lattitude * 10000000 (long value)
longitude : longitude * 10000000 (long value) longitude : longitude * 10000000 (long value)
@ -25,7 +25,7 @@
new_data : 1 when a new data is received. new_data : 1 when a new data is received.
You need to write a 0 to new_data when you read the data You need to write a 0 to new_data when you read the data
fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix. fix : 1: GPS NO fix, 2: 2D fix, 3: 3D fix.
*/ */
#include "AP_GPS_IMU.h" #include "AP_GPS_IMU.h"
#include "WProgram.h" #include "WProgram.h"
@ -38,192 +38,192 @@ AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_IMU::init(void) AP_GPS_IMU::init(void)
{ {
// we expect the stream to already be open at the corret bitrate // we expect the stream to already be open at the corret bitrate
idleTimeout = 1200; idleTimeout = 1200;
} }
// optimization : This code doesn't wait for data. It only proccess the data available. // optimization : This code doesn't wait for data. It only proccess the data available.
// We can call this function on the main loop (50Hz loop) // We can call this function on the main loop (50Hz loop)
// If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info. // If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
bool bool
AP_GPS_IMU::read(void) AP_GPS_IMU::read(void)
{ {
byte data; byte data;
int numc = 0; int numc = 0;
numc = _port->available(); numc = _port->available();
if (numc > 0){ if (numc > 0) {
for (int i=0;i<numc;i++){ // Process bytes received for (int i=0; i<numc; i++) { // Process bytes received
data = _port->read(); data = _port->read();
switch(step){ //Normally we start from zero. This is a state machine switch(step) { //Normally we start from zero. This is a state machine
case 0: case 0:
if(data == 0x44) // IMU sync char 1 if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step step++; //OH first data packet is correct, so jump to the next step
break; break;
case 1: case 1:
if(data == 0x49) // IMU sync char 2 if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 2: case 2:
if(data == 0x59) // IMU sync char 3 if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 3: case 3:
if(data == 0x64) // IMU sync char 4 if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2 step++; //ooh! The second data packet is correct, jump to the step 2
else else
step=0; //Nop, is not correct so restart to step zero and try again. step=0; //Nop, is not correct so restart to step zero and try again.
break; break;
case 4: case 4:
payload_length = data; payload_length = data;
checksum(payload_length); checksum(payload_length);
step++; step++;
if (payload_length > 28){ if (payload_length > 28) {
step = 0; //Bad data, so restart to step zero and try again. step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0; payload_counter = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
//payload_error_count++; //payload_error_count++;
} }
break; break;
case 5: case 5:
message_num = data; message_num = data;
checksum(data); checksum(data);
step++; step++;
break; break;
case 6: // Payload data read... case 6: // Payload data read...
// We stay in this state until we reach the payload_length // We stay in this state until we reach the payload_length
buffer[payload_counter] = data; buffer[payload_counter] = data;
checksum(data); checksum(data);
payload_counter++; payload_counter++;
if (payload_counter >= payload_length) { if (payload_counter >= payload_length) {
step++; step++;
} }
break; break;
case 7: case 7:
GPS_ck_a = data; // First checksum byte GPS_ck_a = data; // First checksum byte
step++; step++;
break; break;
case 8: case 8:
GPS_ck_b = data; // Second checksum byte GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read... // We end the IMU/GPS read...
// Verify the received checksum with the generated checksum.. // Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) { if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) { if (message_num == 0x02) {
join_data(); join_data();
} else if (message_num == 0x03) { } else if (message_num == 0x03) {
GPS_join_data(); GPS_join_data();
} else if (message_num == 0x04) { } else if (message_num == 0x04) {
join_data_xplane(); join_data_xplane();
} else if (message_num == 0x0a) { } else if (message_num == 0x0a) {
//PERF_join_data(); //PERF_join_data();
} else { } else {
// _error("Invalid message number = %d\n", (int)message_num); // _error("Invalid message number = %d\n", (int)message_num);
} }
} else { } else {
// _error("XXX Checksum error\n"); //bad checksum // _error("XXX Checksum error\n"); //bad checksum
//imu_checksum_error_count++; //imu_checksum_error_count++;
} }
// Variable initialization // Variable initialization
step = 0; step = 0;
payload_counter = 0; payload_counter = 0;
ck_a = 0; ck_a = 0;
ck_b = 0; ck_b = 0;
break; break;
} }
}// End for... }// End for...
} }
return true; return true;
} }
/**************************************************************** /****************************************************************
* *
****************************************************************/ ****************************************************************/
void AP_GPS_IMU::join_data(void) void AP_GPS_IMU::join_data(void)
{ {
//Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
//Storing IMU roll //Storing IMU roll
roll_sensor = *(int *)&buffer[0]; roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch //Storing IMU pitch
pitch_sensor = *(int *)&buffer[2]; pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw) //Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4]; ground_course = *(int *)&buffer[4];
imu_ok = true; imu_ok = true;
} }
void AP_GPS_IMU::join_data_xplane() void AP_GPS_IMU::join_data_xplane()
{ {
//Storing IMU roll //Storing IMU roll
roll_sensor = *(int *)&buffer[0]; roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw) //Storing IMU pitch
ground_course = *(unsigned int *)&buffer[4]; pitch_sensor = *(int *)&buffer[2];
//Storing airspeed
airspeed = *(int *)&buffer[6];
imu_ok = true; //Storing IMU heading (yaw)
ground_course = *(unsigned int *)&buffer[4];
//Storing airspeed
airspeed = *(int *)&buffer[6];
imu_ok = true;
} }
void AP_GPS_IMU::GPS_join_data(void) void AP_GPS_IMU::GPS_join_data(void)
{ {
longitude = *(long *)&buffer[0]; // degrees * 10e7 longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4]; latitude = *(long *)&buffer[4];
//Storing GPS Height above the sea level //Storing GPS Height above the sea level
altitude = (long)*(int *)&buffer[8] * 10; altitude = (long)*(int *)&buffer[8] * 10;
//Storing Speed //Storing Speed
speed_3d = ground_speed = (float)*(int *)&buffer[10]; speed_3d = ground_speed = (float)*(int *)&buffer[10];
//We skip the gps ground course because we use yaw value from the IMU for ground course
time = *(long *)&buffer[14];
imu_health = buffer[15]; //We skip the gps ground course because we use yaw value from the IMU for ground course
time = *(long *)&buffer[14];
new_data = true;
fix = true; imu_health = buffer[15];
new_data = true;
fix = true;
} }
/**************************************************************** /****************************************************************
* *
****************************************************************/ ****************************************************************/
// checksum algorithm // checksum algorithm
void AP_GPS_IMU::checksum(byte data) void AP_GPS_IMU::checksum(byte data)
{ {
ck_a += data; ck_a += data;
ck_b += ck_a; ck_b += ck_a;
} }
@ -231,4 +231,4 @@ void AP_GPS_IMU::checksum(byte data)
* Unused * Unused
****************************************************************/ ****************************************************************/
void AP_GPS_IMU::setHIL(long _time, float _latitude, float _longitude, float _altitude, void AP_GPS_IMU::setHIL(long _time, float _latitude, float _longitude, float _altitude,
float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {}; float _ground_speed, float _ground_course, float _speed_3d, uint8_t _num_sats) {};

66
libraries/AP_GPS/AP_GPS_IMU.h
View File

@ -7,42 +7,42 @@
#define MAXPAYLOAD 32 #define MAXPAYLOAD 32
class AP_GPS_IMU : public GPS { class AP_GPS_IMU : public GPS {
public: public:
// Methods // Methods
AP_GPS_IMU(Stream *s); AP_GPS_IMU(Stream *s);
virtual void init(void); virtual void init(void);
virtual bool read(void); virtual bool read(void);
// Properties
long roll_sensor; // how much we're turning in deg * 100
long pitch_sensor; // our angle of attack in deg * 100
int airspeed;
float imu_health;
uint8_t imu_ok;
// Unused
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
private: // Properties
// Packet checksums long roll_sensor; // how much we're turning in deg * 100
uint8_t ck_a; long pitch_sensor; // our angle of attack in deg * 100
uint8_t ck_b; int airspeed;
uint8_t GPS_ck_a; float imu_health;
uint8_t GPS_ck_b; uint8_t imu_ok;
uint8_t step; // Unused
uint8_t msg_class; virtual void setHIL(long time, float latitude, float longitude, float altitude,
uint8_t message_num; float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
uint8_t payload_length;
uint8_t payload_counter; private:
uint8_t buffer[MAXPAYLOAD]; // Packet checksums
uint8_t ck_a;
void join_data(); uint8_t ck_b;
void join_data_xplane(); uint8_t GPS_ck_a;
void GPS_join_data(); uint8_t GPS_ck_b;
void checksum(unsigned char data);
uint8_t step;
uint8_t msg_class;
uint8_t message_num;
uint8_t payload_length;
uint8_t payload_counter;
uint8_t buffer[MAXPAYLOAD];
void join_data();
void join_data_xplane();
void GPS_join_data();
void checksum(unsigned char data);
}; };
#endif #endif

208
libraries/AP_GPS/AP_GPS_MTK.cpp
View File

@ -20,21 +20,21 @@ AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s)
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_MTK::init(void) AP_GPS_MTK::init(void)
{ {
_port->flush(); _port->flush();
// initialize serial port for binary protocol use // initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config? // XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY); _port->print(MTK_SET_BINARY);
// set 4Hz update rate // set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ); _port->print(MTK_OUTPUT_4HZ);
// set initial epoch code // set initial epoch code
_epoch = TIME_OF_DAY; _epoch = TIME_OF_DAY;
idleTimeout = 1200; idleTimeout = 1200;
} }
// Process bytes available from the stream // Process bytes available from the stream
@ -51,103 +51,103 @@ AP_GPS_MTK::init(void)
bool bool
AP_GPS_MTK::read(void) AP_GPS_MTK::read(void)
{ {
uint8_t data; uint8_t data;
int numc; int numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte // read the next byte
data = _port->read(); data = _port->read();
restart: restart:
switch(_step){ switch(_step) {
// Message preamble, class, ID detection // Message preamble, class, ID detection
// //
// If we fail to match any of the expected bytes, we // If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed // reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This // byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch // improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by // and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message. // the preamble appearing as data in some other message.
// //
case 0: case 0:
if(PREAMBLE1 == data) if(PREAMBLE1 == data)
_step++; _step++;
break; break;
case 1: case 1:
if (PREAMBLE2 == data) { if (PREAMBLE2 == data) {
_step++; _step++;
break; break;
} }
_step = 0; _step = 0;
goto restart; goto restart;
case 2: case 2:
if (MESSAGE_CLASS == data) { if (MESSAGE_CLASS == data) {
_step++; _step++;
_ck_b = _ck_a = data; // reset the checksum accumulators _ck_b = _ck_a = data; // reset the checksum accumulators
} else { } else {
_step = 0; // reset and wait for a message of the right class _step = 0; // reset and wait for a message of the right class
goto restart; goto restart;
} }
break; break;
case 3: case 3:
if (MESSAGE_ID == data) { if (MESSAGE_ID == data) {
_step++; _step++;
_ck_b += (_ck_a += data); _ck_b += (_ck_a += data);
_payload_counter = 0; _payload_counter = 0;
} else { } else {
_step = 0; _step = 0;
goto restart; goto restart;
} }
break; break;
// Receive message data // Receive message data
// //
case 4: case 4:
_buffer.bytes[_payload_counter++] = data; _buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data); _ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer)) if (_payload_counter == sizeof(_buffer))
_step++; _step++;
break; break;
// Checksum and message processing // Checksum and message processing
// //
case 5: case 5:
_step++; _step++;
if (_ck_a != data) { if (_ck_a != data) {
_error("GPS_MTK: checksum error\n"); _error("GPS_MTK: checksum error\n");
_step = 0; _step = 0;
} }
break; break;
case 6: case 6:
_step = 0; _step = 0;
if (_ck_b != data) { if (_ck_b != data) {
_error("GPS_MTK: checksum error\n"); _error("GPS_MTK: checksum error\n");
break; break;
} }
fix = (_buffer.msg.fix_type == FIX_3D); fix = (_buffer.msg.fix_type == FIX_3D);
latitude = _swapl(&_buffer.msg.latitude) * 10; latitude = _swapl(&_buffer.msg.latitude) * 10;
longitude = _swapl(&_buffer.msg.longitude) * 10; longitude = _swapl(&_buffer.msg.longitude) * 10;
altitude = _swapl(&_buffer.msg.altitude); altitude = _swapl(&_buffer.msg.altitude);
ground_speed = _swapl(&_buffer.msg.ground_speed); ground_speed = _swapl(&_buffer.msg.ground_speed);
ground_course = _swapl(&_buffer.msg.ground_course) / 10000; ground_course = _swapl(&_buffer.msg.ground_course) / 10000;
num_sats = _buffer.msg.satellites; num_sats = _buffer.msg.satellites;
// time from gps is UTC, but convert here to msToD
long time_utc = _swapl(&_buffer.msg.utc_time);
long temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
parsed = true; // time from gps is UTC, but convert here to msToD
} long time_utc = _swapl(&_buffer.msg.utc_time);
} long temp = (time_utc/10000000);
return parsed; time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
parsed = true;
}
}
return parsed;
} }

74
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -20,52 +20,52 @@
class AP_GPS_MTK : public GPS { class AP_GPS_MTK : public GPS {
public: public:
AP_GPS_MTK(Stream *s); AP_GPS_MTK(Stream *s);
virtual void init(void); virtual void init(void);
virtual bool read(void); virtual bool read(void);
private: private:
// XXX this is being ignored by the compiler #pragma pack(1) // XXX this is being ignored by the compiler #pragma pack(1)
struct diyd_mtk_msg { struct diyd_mtk_msg {
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t altitude; int32_t altitude;
int32_t ground_speed; int32_t ground_speed;
int32_t ground_course; int32_t ground_course;
uint8_t satellites; uint8_t satellites;
uint8_t fix_type; uint8_t fix_type;
uint32_t utc_time; uint32_t utc_time;
}; };
// #pragma pack(pop) // #pragma pack(pop)
enum diyd_mtk_fix_type { enum diyd_mtk_fix_type {
FIX_NONE = 1, FIX_NONE = 1,
FIX_2D = 2, FIX_2D = 2,
FIX_3D = 3 FIX_3D = 3
}; };
enum diyd_mtk_protocol_bytes { enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xb5, PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62, PREAMBLE2 = 0x62,
MESSAGE_CLASS = 1, MESSAGE_CLASS = 1,
MESSAGE_ID = 5 MESSAGE_ID = 5
}; };
// Packet checksum accumulators // Packet checksum accumulators
uint8_t _ck_a; uint8_t _ck_a;
uint8_t _ck_b; uint8_t _ck_b;
// State machine state // State machine state
uint8_t _step; uint8_t _step;
uint8_t _payload_counter; uint8_t _payload_counter;
// Receive buffer // Receive buffer
union { union {
diyd_mtk_msg msg; diyd_mtk_msg msg;
uint8_t bytes[]; uint8_t bytes[];
} _buffer; } _buffer;
// Buffer parse & GPS state update // Buffer parse & GPS state update
void _parse_gps(); void _parse_gps();
}; };
#endif // AP_GPS_MTK_H #endif // AP_GPS_MTK_H

220
libraries/AP_GPS/AP_GPS_MTK16.cpp
View File

@ -20,23 +20,23 @@ AP_GPS_MTK16::AP_GPS_MTK16(Stream *s) : GPS(s)
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_MTK16::init(void) AP_GPS_MTK16::init(void)
{ {
_port->flush(); _port->flush();
// initialize serial port for binary protocol use // initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config? // XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY); _port->print(MTK_SET_BINARY);
// set 4Hz update rate // set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ); _port->print(MTK_OUTPUT_4HZ);
// set initial epoch code // set initial epoch code
_epoch = TIME_OF_DAY; _epoch = TIME_OF_DAY;
_time_offset = 0; _time_offset = 0;
_offset_calculated = false; _offset_calculated = false;
idleTimeout = 1200; idleTimeout = 1200;
} }
// Process bytes available from the stream // Process bytes available from the stream
@ -53,109 +53,109 @@ AP_GPS_MTK16::init(void)
bool bool
AP_GPS_MTK16::read(void) AP_GPS_MTK16::read(void)
{ {
uint8_t data; uint8_t data;
int numc; int numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte // read the next byte
data = _port->read(); data = _port->read();
restart: restart:
switch(_step){ switch(_step) {
// Message preamble, class, ID detection // Message preamble, class, ID detection
// //
// If we fail to match any of the expected bytes, we // If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed // reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This // byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch // improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by // and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message. // the preamble appearing as data in some other message.
// //
case 0: case 0:
if(PREAMBLE1 == data) if(PREAMBLE1 == data)
_step++; _step++;
break; break;
case 1: case 1:
if (PREAMBLE2 == data) { if (PREAMBLE2 == data) {
_step++; _step++;
break; break;
} }
_step = 0; _step = 0;
goto restart; goto restart;
case 2: case 2:
if (sizeof(_buffer) == data) { if (sizeof(_buffer) == data) {
_step++; _step++;
_ck_b = _ck_a = data; // reset the checksum accumulators _ck_b = _ck_a = data; // reset the checksum accumulators
_payload_counter = 0; _payload_counter = 0;
} else { } else {
_step = 0; // reset and wait for a message of the right class _step = 0; // reset and wait for a message of the right class
goto restart; goto restart;
} }
break; break;
// Receive message data // Receive message data
// //
case 3: case 3:
_buffer.bytes[_payload_counter++] = data; _buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data); _ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer)) if (_payload_counter == sizeof(_buffer))
_step++; _step++;
break; break;
// Checksum and message processing // Checksum and message processing
// //
case 4: case 4:
_step++; _step++;
if (_ck_a != data) { if (_ck_a != data) {
_step = 0; _step = 0;
} }
break; break;
case 5: case 5:
_step = 0; _step = 0;
if (_ck_b != data) { if (_ck_b != data) {
break; break;
} }
fix = _buffer.msg.fix_type == FIX_3D; fix = _buffer.msg.fix_type == FIX_3D;
latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise latitude = _buffer.msg.latitude * 10; // XXX doc says *10e7 but device says otherwise
longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise longitude = _buffer.msg.longitude * 10; // XXX doc says *10e7 but device says otherwise
altitude = _buffer.msg.altitude; altitude = _buffer.msg.altitude;
ground_speed = _buffer.msg.ground_speed; ground_speed = _buffer.msg.ground_speed;
ground_course = _buffer.msg.ground_course; ground_course = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites; num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop; hdop = _buffer.msg.hdop;
date = _buffer.msg.utc_date; date = _buffer.msg.utc_date;
// time from gps is UTC, but convert here to msToD
long time_utc = _buffer.msg.utc_time;
long temp = (time_utc/10000000);
time_utc -= temp*10000000;
time = temp * 3600000;
temp = (time_utc/100000);
time_utc -= temp*100000;
time += temp * 60000 + time_utc;
parsed = true; // time from gps is UTC, but convert here to msToD
long time_utc = _buffer.msg.utc_time;
/* Waiting on clarification of MAVLink protocol! long temp = (time_utc/10000000);
if(!_offset_calculated && parsed) { time_utc -= temp*10000000;
long tempd1 = date; time = temp * 3600000;
long day = tempd1/10000; temp = (time_utc/100000);
tempd1 -= day * 10000; time_utc -= temp*100000;
long month = tempd1/100; time += temp * 60000 + time_utc;
long year = tempd1 - month * 100;
_time_offset = _calc_epoch_offset(day, month, year); parsed = true;
_epoch = UNIX_EPOCH;
_offset_calculated = TRUE; /* Waiting on clarification of MAVLink protocol!
} if(!_offset_calculated && parsed) {
*/ long tempd1 = date;
long day = tempd1/10000;
} tempd1 -= day * 10000;
} long month = tempd1/100;
return parsed; long year = tempd1 - month * 100;
_time_offset = _calc_epoch_offset(day, month, year);
_epoch = UNIX_EPOCH;
_offset_calculated = TRUE;
}
*/
}
}
return parsed;
} }

78
libraries/AP_GPS/AP_GPS_MTK16.h
View File

@ -18,53 +18,53 @@
class AP_GPS_MTK16 : public GPS { class AP_GPS_MTK16 : public GPS {
public: public:
AP_GPS_MTK16(Stream *s); AP_GPS_MTK16(Stream *s);
virtual void init(void); virtual void init(void);
virtual bool read(void); virtual bool read(void);
private: private:
// XXX this is being ignored by the compiler #pragma pack(1) // XXX this is being ignored by the compiler #pragma pack(1)
struct diyd_mtk_msg { struct diyd_mtk_msg {
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t altitude; int32_t altitude;
int32_t ground_speed; int32_t ground_speed;
int32_t ground_course; int32_t ground_course;
uint8_t satellites; uint8_t satellites;
uint8_t fix_type; uint8_t fix_type;
uint32_t utc_date; uint32_t utc_date;
uint32_t utc_time; uint32_t utc_time;
uint16_t hdop; uint16_t hdop;
}; };
// #pragma pack(pop) // #pragma pack(pop)
enum diyd_mtk_fix_type { enum diyd_mtk_fix_type {
FIX_NONE = 1, FIX_NONE = 1,
FIX_2D = 2, FIX_2D = 2,
FIX_3D = 3 FIX_3D = 3
}; };
enum diyd_mtk_protocol_bytes { enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xd0, PREAMBLE1 = 0xd0,
PREAMBLE2 = 0xdd, PREAMBLE2 = 0xdd,
}; };
// Packet checksum accumulators // Packet checksum accumulators
uint8_t _ck_a; uint8_t _ck_a;
uint8_t _ck_b; uint8_t _ck_b;
// State machine state // State machine state
uint8_t _step; uint8_t _step;
uint8_t _payload_counter; uint8_t _payload_counter;
// Time from UNIX Epoch offset
long _time_offset;
bool _offset_calculated;
// Receive buffer // Time from UNIX Epoch offset
union { long _time_offset;
diyd_mtk_msg msg; bool _offset_calculated;
uint8_t bytes[];
} _buffer; // Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
}; };
#endif // AP_GPS_MTK16_H #endif // AP_GPS_MTK16_H

498
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -41,17 +41,17 @@
// the autodetection process. // the autodetection process.
// //
const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM = const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz "$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
"$PSRF103,1,0,0,1*25\r\n" // GLL off "$PSRF103,1,0,0,1*25\r\n" // GLL off
"$PSRF103,2,0,0,1*26\r\n" // GSA off "$PSRF103,2,0,0,1*26\r\n" // GSA off
"$PSRF103,3,0,0,1*27\r\n" // GSV off "$PSRF103,3,0,0,1*27\r\n" // GSV off
"$PSRF103,4,0,1,1*20\r\n" // RMC off "$PSRF103,4,0,1,1*20\r\n" // RMC off
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz "$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
"$PSRF103,6,0,0,1*22\r\n" // MSS off "$PSRF103,6,0,0,1*22\r\n" // MSS off
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off "$PSRF103,8,0,0,1*2C\r\n" // ZDA off
"$PSRF151,1*3F\r\n" // WAAS on (not always supported) "$PSRF151,1*3F\r\n" // WAAS on (not always supported)
"$PSRF106,21*0F\r\n" // datum = WGS84 "$PSRF106,21*0F\r\n" // datum = WGS84
""; "";
// MediaTek init messages ////////////////////////////////////////////////////// // MediaTek init messages //////////////////////////////////////////////////////
// //
@ -59,11 +59,11 @@ const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
// MediaTek-based GPS. // MediaTek-based GPS.
// //
const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM = const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
"$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix "$PMTK314,0,0,1,1,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n" // GGA & VTG once every fix
"$PMTK330,0*2E\r\n" // datum = WGS84 "$PMTK330,0*2E\r\n" // datum = WGS84
"$PMTK313,1*2E\r\n" // SBAS on "$PMTK313,1*2E\r\n" // SBAS on
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS "$PMTK301,2*2E\r\n" // use SBAS data for DGPS
""; "";
// ublox init messages ///////////////////////////////////////////////////////// // ublox init messages /////////////////////////////////////////////////////////
// //
@ -75,10 +75,10 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
// and we don't know the baudrate. // and we don't know the baudrate.
// //
const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM = const prog_char AP_GPS_NMEA::_ublox_init_string[] PROGMEM =
"$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix "$PUBX,40,gga,0,1,0,0,0,0*7B\r\n" // GGA on at one per fix
"$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix "$PUBX,40,vtg,0,1,0,0,0,0*7F\r\n" // VTG on at one per fix
"$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?) "$PUBX,40,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
""; "";
// NMEA message identifiers //////////////////////////////////////////////////// // NMEA message identifiers ////////////////////////////////////////////////////
// //
@ -92,83 +92,83 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) :
GPS(s) GPS(s)
{ {
FastSerial *fs = (FastSerial *)_port; FastSerial *fs = (FastSerial *)_port;
// Re-open the port with enough receive buffering for the messages we expect // Re-open the port with enough receive buffering for the messages we expect
// and very little tx buffering, since we don't care about sending. // and very little tx buffering, since we don't care about sending.
// Leave the port speed alone as we don't actually know at what rate we're running... // Leave the port speed alone as we don't actually know at what rate we're running...
// //
fs->begin(0, 200, 16); fs->begin(0, 200, 16);
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_NMEA::init(void) void AP_GPS_NMEA::init(void)
{ {
BetterStream *bs = (BetterStream *)_port; BetterStream *bs = (BetterStream *)_port;
// send the SiRF init strings // send the SiRF init strings
bs->print_P((const prog_char_t *)_SiRF_init_string); bs->print_P((const prog_char_t *)_SiRF_init_string);
// send the MediaTek init strings // send the MediaTek init strings
bs->print_P((const prog_char_t *)_MTK_init_string); bs->print_P((const prog_char_t *)_MTK_init_string);
// send the ublox init strings // send the ublox init strings
bs->print_P((const prog_char_t *)_ublox_init_string); bs->print_P((const prog_char_t *)_ublox_init_string);
idleTimeout = 1200; idleTimeout = 1200;
} }
bool AP_GPS_NMEA::read(void) bool AP_GPS_NMEA::read(void)
{ {
int numc; int numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
while (numc--) { while (numc--) {
if (_decode(_port->read())) { if (_decode(_port->read())) {
parsed = true; parsed = true;
} }
} }
return parsed; return parsed;
} }
bool AP_GPS_NMEA::_decode(char c) bool AP_GPS_NMEA::_decode(char c)
{ {
bool valid_sentence = false; bool valid_sentence = false;
switch (c) { switch (c) {
case ',': // term terminators case ',': // term terminators
_parity ^= c; _parity ^= c;
case '\r': case '\r':
case '\n': case '\n':
case '*': case '*':
if (_term_offset < sizeof(_term)) { if (_term_offset < sizeof(_term)) {
_term[_term_offset] = 0; _term[_term_offset] = 0;
valid_sentence = _term_complete(); valid_sentence = _term_complete();
} }
++_term_number; ++_term_number;
_term_offset = 0; _term_offset = 0;
_is_checksum_term = c == '*'; _is_checksum_term = c == '*';
return valid_sentence; return valid_sentence;
case '$': // sentence begin case '$': // sentence begin
_term_number = _term_offset = 0; _term_number = _term_offset = 0;
_parity = 0; _parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER; _sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false; _is_checksum_term = false;
_gps_data_good = false; _gps_data_good = false;
return valid_sentence; return valid_sentence;
} }
// ordinary characters // ordinary characters
if (_term_offset < sizeof(_term) - 1) if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c; _term[_term_offset++] = c;
if (!_is_checksum_term) if (!_is_checksum_term)
_parity ^= c; _parity ^= c;
return valid_sentence; return valid_sentence;
} }
// //
@ -176,203 +176,203 @@ bool AP_GPS_NMEA::_decode(char c)
// //
int AP_GPS_NMEA::_from_hex(char a) int AP_GPS_NMEA::_from_hex(char a)
{ {
if (a >= 'A' && a <= 'F') if (a >= 'A' && a <= 'F')
return a - 'A' + 10; return a - 'A' + 10;
else if (a >= 'a' && a <= 'f') else if (a >= 'a' && a <= 'f')
return a - 'a' + 10; return a - 'a' + 10;
else else
return a - '0'; return a - '0';
} }
unsigned long AP_GPS_NMEA::_parse_decimal() unsigned long AP_GPS_NMEA::_parse_decimal()
{ {
char *p = _term; char *p = _term;
unsigned long ret = 100UL * atol(p); unsigned long ret = 100UL * atol(p);
while (isdigit(*p)) while (isdigit(*p))
++p; ++p;
if (*p == '.') { if (*p == '.') {
if (isdigit(p[1])) { if (isdigit(p[1])) {
ret += 10 * (p[1] - '0'); ret += 10 * (p[1] - '0');
if (isdigit(p[2])) if (isdigit(p[2]))
ret += p[2] - '0'; ret += p[2] - '0';
} }
} }
return ret; return ret;
} }
unsigned long AP_GPS_NMEA::_parse_degrees() unsigned long AP_GPS_NMEA::_parse_degrees()
{ {
char *p, *q; char *p, *q;
uint8_t deg = 0, min = 0; uint8_t deg = 0, min = 0;
unsigned int frac_min = 0; unsigned int frac_min = 0;
// scan for decimal point or end of field // scan for decimal point or end of field
for (p = _term; isdigit(*p); p++) for (p = _term; isdigit(*p); p++)
; ;
q = _term; q = _term;
// convert degrees // convert degrees
while ((p - q) > 2) { while ((p - q) > 2) {
if (deg) if (deg)
deg *= 10; deg *= 10;
deg += DIGIT_TO_VAL(*q++); deg += DIGIT_TO_VAL(*q++);
} }
// convert minutes // convert minutes
while (p > q) { while (p > q) {
if (min) if (min)
min *= 10; min *= 10;
min += DIGIT_TO_VAL(*q++); min += DIGIT_TO_VAL(*q++);
} }
// convert fractional minutes // convert fractional minutes
// expect up to four digits, result is in // expect up to four digits, result is in
// ten-thousandths of a minute // ten-thousandths of a minute
if (*p == '.') { if (*p == '.') {
q = p + 1; q = p + 1;
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
frac_min *= 10; frac_min *= 10;
if (isdigit(*q)) if (isdigit(*q))
frac_min += *q++ - '0'; frac_min += *q++ - '0';
} }
} }
return deg * 100000UL + (min * 10000UL + frac_min) / 6; return deg * 100000UL + (min * 10000UL + frac_min) / 6;
} }
// Processes a just-completed term // Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated // Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete() bool AP_GPS_NMEA::_term_complete()
{ {
// handle the last term in a message // handle the last term in a message
if (_is_checksum_term) { if (_is_checksum_term) {
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]); uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) { if (checksum == _parity) {
if (_gps_data_good) { if (_gps_data_good) {
switch (_sentence_type) { switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC: case _GPS_SENTENCE_GPRMC:
time = _new_time; time = _new_time;
date = _new_date; date = _new_date;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7 latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7 longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
ground_speed = _new_speed; ground_speed = _new_speed;
ground_course = _new_course; ground_course = _new_course;
fix = true; fix = true;
break; break;
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GPGGA:
altitude = _new_altitude; altitude = _new_altitude;
time = _new_time; time = _new_time;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7 latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7 longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
num_sats = _new_satellite_count; num_sats = _new_satellite_count;
hdop = _new_hdop; hdop = _new_hdop;
fix = true; fix = true;
break; break;
case _GPS_SENTENCE_GPVTG: case _GPS_SENTENCE_GPVTG:
ground_speed = _new_speed; ground_speed = _new_speed;
ground_course = _new_course; ground_course = _new_course;
// VTG has no fix indicator, can't change fix status // VTG has no fix indicator, can't change fix status
break; break;
} }
} else { } else {
switch (_sentence_type) { switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC: case _GPS_SENTENCE_GPRMC:
case _GPS_SENTENCE_GPGGA: case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about // Only these sentences give us information about
// fix status. // fix status.
fix = false; fix = false;
} }
} }
// we got a good message // we got a good message
return true; return true;
} }
// we got a bad message, ignore it // we got a bad message, ignore it
return false; return false;
} }
// the first term determines the sentence type // the first term determines the sentence type
if (_term_number == 0) { if (_term_number == 0) {
if (!strcmp_P(_term, _gprmc_string)) { if (!strcmp_P(_term, _gprmc_string)) {
_sentence_type = _GPS_SENTENCE_GPRMC; _sentence_type = _GPS_SENTENCE_GPRMC;
} else if (!strcmp_P(_term, _gpgga_string)) { } else if (!strcmp_P(_term, _gpgga_string)) {
_sentence_type = _GPS_SENTENCE_GPGGA; _sentence_type = _GPS_SENTENCE_GPGGA;
} else if (!strcmp_P(_term, _gpvtg_string)) { } else if (!strcmp_P(_term, _gpvtg_string)) {
_sentence_type = _GPS_SENTENCE_GPVTG; _sentence_type = _GPS_SENTENCE_GPVTG;
// VTG may not contain a data qualifier, presume the solution is good // VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise. // unless it tells us otherwise.
_gps_data_good = true; _gps_data_good = true;
} else { } else {
_sentence_type = _GPS_SENTENCE_OTHER; _sentence_type = _GPS_SENTENCE_OTHER;
} }
return false; return false;
} }
// 32 = RMC, 64 = GGA, 96 = VTG // 32 = RMC, 64 = GGA, 96 = VTG
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) { if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) { switch (_sentence_type + _term_number) {
// operational status // operational status
// //
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC) case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A'; _gps_data_good = _term[0] == 'A';
break; break;
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA) case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0'; _gps_data_good = _term[0] > '0';
break; break;
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field) case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N'; _gps_data_good = _term[0] != 'N';
break; break;
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA) case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term); _new_satellite_count = atol(_term);
break; break;
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA) case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
_new_hdop = _parse_decimal(); _new_hdop = _parse_decimal();
break; break;
// time and date // time and date
// //
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC) case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA) case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
_new_time = _parse_decimal(); _new_time = _parse_decimal();
break; break;
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC) case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
_new_date = atol(_term); _new_date = atol(_term);
break; break;
// location // location
// //
case _GPS_SENTENCE_GPRMC + 3: // Latitude case _GPS_SENTENCE_GPRMC + 3: // Latitude
case _GPS_SENTENCE_GPGGA + 2: case _GPS_SENTENCE_GPGGA + 2:
_new_latitude = _parse_degrees(); _new_latitude = _parse_degrees();
break; break;
case _GPS_SENTENCE_GPRMC + 4: // N/S case _GPS_SENTENCE_GPRMC + 4: // N/S
case _GPS_SENTENCE_GPGGA + 3: case _GPS_SENTENCE_GPGGA + 3:
if (_term[0] == 'S') if (_term[0] == 'S')
_new_latitude = -_new_latitude; _new_latitude = -_new_latitude;
break; break;
case _GPS_SENTENCE_GPRMC + 5: // Longitude case _GPS_SENTENCE_GPRMC + 5: // Longitude
case _GPS_SENTENCE_GPGGA + 4: case _GPS_SENTENCE_GPGGA + 4:
_new_longitude = _parse_degrees(); _new_longitude = _parse_degrees();
break; break;
case _GPS_SENTENCE_GPRMC + 6: // E/W case _GPS_SENTENCE_GPRMC + 6: // E/W
case _GPS_SENTENCE_GPGGA + 5: case _GPS_SENTENCE_GPGGA + 5:
if (_term[0] == 'W') if (_term[0] == 'W')
_new_longitude = -_new_longitude; _new_longitude = -_new_longitude;
break; break;
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA) case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal(); _new_altitude = _parse_decimal();
break; break;
// course and speed // course and speed
// //
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC) case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG) case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514 _new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break; break;
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC) case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG) case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
_new_course = _parse_decimal(); _new_course = _parse_decimal();
break; break;
} }
} }
return false; return false;
} }

110
libraries/AP_GPS/AP_GPS_NMEA.h
View File

@ -51,72 +51,72 @@
class AP_GPS_NMEA : public GPS class AP_GPS_NMEA : public GPS
{ {
public: public:
/// Constructor /// Constructor
/// ///
AP_GPS_NMEA(Stream *s); AP_GPS_NMEA(Stream *s);
/// Perform a (re)initialisation of the GPS; sends the /// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages. /// protocol configuration messages.
/// ///
virtual void init(); virtual void init();
/// Checks the serial receive buffer for characters, /// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state /// attempts to parse NMEA data and updates internal state
/// accordingly. /// accordingly.
/// ///
virtual bool read(); virtual bool read();
private: private:
/// Coding for the GPS sentences that the parser handles /// Coding for the GPS sentences that the parser handles
enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value. enum _sentence_types { //there are some more than 10 fields in some sentences , thus we have to increase these value.
_GPS_SENTENCE_GPRMC = 32, _GPS_SENTENCE_GPRMC = 32,
_GPS_SENTENCE_GPGGA = 64, _GPS_SENTENCE_GPGGA = 64,
_GPS_SENTENCE_GPVTG = 96, _GPS_SENTENCE_GPVTG = 96,
_GPS_SENTENCE_OTHER = 0 _GPS_SENTENCE_OTHER = 0
}; };
/// Update the decode state machine with a new character /// Update the decode state machine with a new character
/// ///
/// @param c The next character in the NMEA input stream /// @param c The next character in the NMEA input stream
/// @returns True if processing the character has resulted in /// @returns True if processing the character has resulted in
/// an update to the GPS state /// an update to the GPS state
/// ///
bool _decode(char c); bool _decode(char c);
/// Return the numeric value of an ascii hex character /// Return the numeric value of an ascii hex character
/// ///
/// @param a The character to be converted /// @param a The character to be converted
/// @returns The value of the character as a hex digit /// @returns The value of the character as a hex digit
/// ///
int _from_hex(char a); int _from_hex(char a);
/// Parses the current term as a NMEA-style decimal number with /// Parses the current term as a NMEA-style decimal number with
/// up to two decimal digits. /// up to two decimal digits.
/// ///
/// @returns The value expressed by the string in _term, /// @returns The value expressed by the string in _term,
/// multiplied by 100. /// multiplied by 100.
/// ///
unsigned long _parse_decimal(); unsigned long _parse_decimal();
/// Parses the current term as a NMEA-style degrees + minutes /// Parses the current term as a NMEA-style degrees + minutes
/// value with up to four decimal digits. /// value with up to four decimal digits.
/// ///
/// This gives a theoretical resolution limit of around 18cm. /// This gives a theoretical resolution limit of around 18cm.
/// ///
/// @returns The value expressed by the string in _term, /// @returns The value expressed by the string in _term,
/// multiplied by 10000. /// multiplied by 10000.
/// ///
unsigned long _parse_degrees(); unsigned long _parse_degrees();
/// Processes the current term when it has been deemed to be /// Processes the current term when it has been deemed to be
/// complete. /// complete.
/// ///
/// Each GPS message is broken up into terms separated by commas. /// Each GPS message is broken up into terms separated by commas.
/// Each term is then processed by this function as it is received. /// Each term is then processed by this function as it is received.
/// ///
/// @returns True if completing the term has resulted in /// @returns True if completing the term has resulted in
/// an update to the GPS state. /// an update to the GPS state.
bool _term_complete(); bool _term_complete();
uint8_t _parity; ///< NMEA message checksum accumulator uint8_t _parity; ///< NMEA message checksum accumulator
bool _is_checksum_term; ///< current term is the checksum bool _is_checksum_term; ///< current term is the checksum

8
libraries/AP_GPS/AP_GPS_None.h
View File

@ -8,8 +8,10 @@
class AP_GPS_None : public GPS class AP_GPS_None : public GPS
{ {
public: public:
AP_GPS_None(Stream *s) : GPS(s) {} AP_GPS_None(Stream *s) : GPS(s) {}
virtual void init(void) {}; virtual void init(void) {};
virtual bool read(void) { return false; }; virtual bool read(void) {
return false;
};
}; };
#endif #endif

262
libraries/AP_GPS/AP_GPS_SIRF.cpp
View File

@ -19,8 +19,8 @@
// XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them. // XXX the bytes show up on the wire, but at least my test unit (EM-411) seems to ignore them.
// //
static uint8_t init_messages[] = { static uint8_t init_messages[] = {
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3, 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xa8, 0xb0, 0xb3,
0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3 0xa0, 0xa2, 0x00, 0x08, 0xa6, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 0xb0, 0xb3
}; };
// Constructors //////////////////////////////////////////////////////////////// // Constructors ////////////////////////////////////////////////////////////////
@ -29,18 +29,18 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
} }
// Public Methods ////////////////////////////////////////////////////////////// // Public Methods //////////////////////////////////////////////////////////////
void void
AP_GPS_SIRF::init(void) AP_GPS_SIRF::init(void)
{ {
_port->flush(); _port->flush();
// For modules that default to something other than SiRF binary, // For modules that default to something other than SiRF binary,
// the module-specific subclass should take care of switching to binary mode // the module-specific subclass should take care of switching to binary mode
// before calling us. // before calling us.
// send SiRF binary setup messages // send SiRF binary setup messages
_port->write(init_messages, sizeof(init_messages)); _port->write(init_messages, sizeof(init_messages));
idleTimeout = 1200; idleTimeout = 1200;
} }
// Process bytes available from the stream // Process bytes available from the stream
@ -55,143 +55,143 @@ AP_GPS_SIRF::init(void)
bool bool
AP_GPS_SIRF::read(void) AP_GPS_SIRF::read(void)
{ {
uint8_t data; uint8_t data;
int numc; int numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
while(numc--) { while(numc--) {
// 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;
// FALLTHROUGH // FALLTHROUGH
case 0: case 0:
if(PREAMBLE1 == data) if(PREAMBLE1 == data)
_step++; _step++;
break; break;
// Message length // Message length
// //
// 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++;
_payload_length = (uint16_t)data << 8; _payload_length = (uint16_t)data << 8;
break; break;
case 3: case 3:
_step++; _step++;
_payload_length |= data; _payload_length |= data;
_payload_counter = 0; _payload_counter = 0;
_checksum = 0; _checksum = 0;
break; break;
// Message header processing // Message header processing
// //
// We sniff the message ID to determine whether we are going // We sniff the message ID to determine whether we are going
// to gather the message bytes or just discard them. // to gather the message bytes or just discard them.
// //
case 4: case 4:
_step++; _step++;
_accumulate(data); _accumulate(data);
_payload_length--; _payload_length--;
_gather = false; _gather = false;
switch(data) { switch(data) {
case MSG_GEONAV: case MSG_GEONAV:
if (_payload_length == sizeof(sirf_geonav)) { if (_payload_length == sizeof(sirf_geonav)) {
_gather = true; _gather = true;
_msg_id = data; _msg_id = data;
} }
break; break;
} }
break; break;
// Receive message data // Receive message data
// //
// Note that we are effectively guaranteed by the protocol // Note that we are effectively guaranteed by the protocol
// that the checksum and postamble cannot be mistaken for // that the checksum and postamble cannot be mistaken for
// the preamble, so if we are discarding bytes in this // the preamble, so if we are discarding bytes in this
// message when the payload is done we return directly // message when the payload is done we return directly
// to the preamble detector rather than bothering with // to the preamble detector rather than bothering with
// the checksum logic. // the checksum logic.
// //
case 5: case 5:
if (_gather) { // gather data if requested if (_gather) { // gather data if requested
_accumulate(data); _accumulate(data);
_buffer.bytes[_payload_counter] = data; _buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length) if (++_payload_counter == _payload_length)
_step++; _step++;
} else { } else {
if (++_payload_counter == _payload_length) if (++_payload_counter == _payload_length)
_step = 0; _step = 0;
} }
break; break;
// Checksum and message processing // Checksum and message processing
// //
case 6: case 6:
_step++; _step++;
if ((_checksum >> 8) != data) { if ((_checksum >> 8) != data) {
_error("GPS_SIRF: checksum error\n"); _error("GPS_SIRF: checksum error\n");
_step = 0; _step = 0;
} }
break; break;
case 7: case 7:
_step = 0; _step = 0;
if ((_checksum & 0xff) != data) { if ((_checksum & 0xff) != data) {
_error("GPS_SIRF: checksum error\n"); _error("GPS_SIRF: checksum error\n");
break; break;
} }
if (_gather) { if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet parsed = _parse_gps(); // Parse the new GPS packet
} }
} }
} }
return(parsed); return(parsed);
} }
bool bool
AP_GPS_SIRF::_parse_gps(void) AP_GPS_SIRF::_parse_gps(void)
{ {
switch(_msg_id) { switch(_msg_id) {
case MSG_GEONAV: case MSG_GEONAV:
time = _swapl(&_buffer.nav.time); time = _swapl(&_buffer.nav.time);
//fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK)); //fix = (0 == _buffer.nav.fix_invalid) && (FIX_3D == (_buffer.nav.fix_type & FIX_MASK));
fix = (0 == _buffer.nav.fix_invalid); fix = (0 == _buffer.nav.fix_invalid);
latitude = _swapl(&_buffer.nav.latitude); latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude); longitude = _swapl(&_buffer.nav.longitude);
altitude = _swapl(&_buffer.nav.altitude_msl); altitude = _swapl(&_buffer.nav.altitude_msl);
ground_speed = _swapi(&_buffer.nav.ground_speed); ground_speed = _swapi(&_buffer.nav.ground_speed);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving // at low speeds, ground course wanders wildly; suppress changes if we are not moving
if (ground_speed > 50) if (ground_speed > 50)
ground_course = _swapi(&_buffer.nav.ground_course); ground_course = _swapi(&_buffer.nav.ground_course);
num_sats = _buffer.nav.satellites; num_sats = _buffer.nav.satellites;
return true; return true;
} }
return false; return false;
} }
void void
AP_GPS_SIRF::_accumulate(uint8_t val) AP_GPS_SIRF::_accumulate(uint8_t val)
{ {
_checksum = (_checksum + val) & 0x7fff; _checksum = (_checksum + val) & 0x7fff;
} }

130
libraries/AP_GPS/AP_GPS_SIRF.h
View File

@ -17,80 +17,80 @@
class AP_GPS_SIRF : public GPS { class AP_GPS_SIRF : public GPS {
public: public:
AP_GPS_SIRF(Stream *s); AP_GPS_SIRF(Stream *s);
virtual void init(); virtual void init();
virtual bool read(); virtual bool read();
private: private:
// XXX this is being ignored by the compiler #pragma pack(1) // XXX this is being ignored by the compiler #pragma pack(1)
struct sirf_geonav { struct sirf_geonav {
uint16_t fix_invalid; uint16_t fix_invalid;
uint16_t fix_type; uint16_t fix_type;
uint16_t week; uint16_t week;
uint32_t time; uint32_t time;
uint16_t year; uint16_t year;
uint8_t month; uint8_t month;
uint8_t day; uint8_t day;
uint8_t hour; uint8_t hour;
uint8_t minute; uint8_t minute;
uint16_t second; uint16_t second;
uint32_t satellites_used; uint32_t satellites_used;
int32_t latitude; int32_t latitude;
int32_t longitude; int32_t longitude;
int32_t altitude_ellipsoid; int32_t altitude_ellipsoid;
int32_t altitude_msl; int32_t altitude_msl;
int8_t map_datum; int8_t map_datum;
int16_t ground_speed; int16_t ground_speed;
int16_t ground_course; int16_t ground_course;
int16_t res1; int16_t res1;
int16_t climb_rate; int16_t climb_rate;
uint16_t heading_rate; uint16_t heading_rate;
uint32_t horizontal_position_error; uint32_t horizontal_position_error;
uint32_t vertical_position_error; uint32_t vertical_position_error;
uint32_t time_error; uint32_t time_error;
int16_t horizontal_velocity_error; int16_t horizontal_velocity_error;
int32_t clock_bias; int32_t clock_bias;
uint32_t clock_bias_error; uint32_t clock_bias_error;
int32_t clock_drift; int32_t clock_drift;
uint32_t clock_drift_error; uint32_t clock_drift_error;
uint32_t distance; uint32_t distance;
uint16_t distance_error; uint16_t distance_error;
uint16_t heading_error; uint16_t heading_error;
uint8_t satellites; uint8_t satellites;
uint8_t hdop; uint8_t hdop;
uint8_t mode_info; uint8_t mode_info;
}; };
// #pragma pack(pop) // #pragma pack(pop)
enum sirf_protocol_bytes { enum sirf_protocol_bytes {
PREAMBLE1 = 0xa0, PREAMBLE1 = 0xa0,
PREAMBLE2 = 0xa2, PREAMBLE2 = 0xa2,
POSTAMBLE1 = 0xb0, POSTAMBLE1 = 0xb0,
POSTAMBLE2 = 0xb3, POSTAMBLE2 = 0xb3,
MSG_GEONAV = 0x29 MSG_GEONAV = 0x29
}; };
enum sirf_fix_type { enum sirf_fix_type {
FIX_3D = 0x6, FIX_3D = 0x6,
FIX_MASK = 0x7 FIX_MASK = 0x7
}; };
// State machine state // State machine state
uint8_t _step; uint8_t _step;
uint16_t _checksum; uint16_t _checksum;
bool _gather; bool _gather;
uint16_t _payload_length; uint16_t _payload_length;
uint16_t _payload_counter; uint16_t _payload_counter;
uint8_t _msg_id; uint8_t _msg_id;
// Message buffer // Message buffer
union { union {
sirf_geonav nav; sirf_geonav nav;
uint8_t bytes[]; uint8_t bytes[];
} _buffer; } _buffer;
bool _parse_gps(void); bool _parse_gps(void);
void _accumulate(uint8_t val); void _accumulate(uint8_t val);
}; };
#endif // AP_GPS_SIRF_h #endif // AP_GPS_SIRF_h

38
libraries/AP_GPS/AP_GPS_Shim.h
View File

@ -17,31 +17,31 @@
class AP_GPS_Shim : public GPS class AP_GPS_Shim : public GPS
{ {
public: public:
AP_GPS_Shim() : GPS(NULL) {} AP_GPS_Shim() : GPS(NULL) {}
virtual void init(void) {}; virtual void init(void) {};
virtual bool read(void) { virtual bool read(void) {
bool updated = _updated; bool updated = _updated;
_updated = false; _updated = false;
return updated; return updated;
} }
/// Set-and-mark-updated macro for the public member variables; each instance /// Set-and-mark-updated macro for the public member variables; each instance
/// defines a member function set_<variable>(<type>) /// defines a member function set_<variable>(<type>)
/// ///
#define __GPS_SHIM_SET(__type, __name) void set_##__name(__type v) { __name = v; _updated = true; } #define __GPS_SHIM_SET(__type, __name) void set_##__name(__type v) { __name = v; _updated = true; }
__GPS_SHIM_SET(long, time); __GPS_SHIM_SET(long, time);
__GPS_SHIM_SET(long, latitude); __GPS_SHIM_SET(long, latitude);
__GPS_SHIM_SET(long, longitude); __GPS_SHIM_SET(long, longitude);
__GPS_SHIM_SET(long, altitude); __GPS_SHIM_SET(long, altitude);
__GPS_SHIM_SET(long, ground_speed); __GPS_SHIM_SET(long, ground_speed);
__GPS_SHIM_SET(long, ground_course); __GPS_SHIM_SET(long, ground_course);
__GPS_SHIM_SET(long, speed_3d); __GPS_SHIM_SET(long, speed_3d);
__GPS_SHIM_SET(int, hdop); __GPS_SHIM_SET(int, hdop);
#undef __GPS_SHIM_SET #undef __GPS_SHIM_SET
private: private:
bool _updated; ///< set anytime a member is updated, cleared when read() returns true bool _updated; ///< set anytime a member is updated, cleared when read() returns true
}; };
#endif // AP_GPS_HIL_H #endif // AP_GPS_HIL_H

276
libraries/AP_GPS/AP_GPS_UBLOX.cpp
View File

@ -23,13 +23,13 @@ AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s)
void void
AP_GPS_UBLOX::init(void) AP_GPS_UBLOX::init(void)
{ {
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the // XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration. // right reporting configuration.
_port->flush(); _port->flush();
_epoch = TIME_OF_WEEK; _epoch = TIME_OF_WEEK;
idleTimeout = 1200; idleTimeout = 1200;
} }
// Process bytes available from the stream // Process bytes available from the stream
@ -44,122 +44,122 @@ AP_GPS_UBLOX::init(void)
bool bool
AP_GPS_UBLOX::read(void) AP_GPS_UBLOX::read(void)
{ {
uint8_t data; uint8_t data;
int numc; int numc;
bool parsed = false; bool parsed = false;
numc = _port->available(); numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received for (int 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;
// 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++;
if (CLASS_NAV == data) { if (CLASS_NAV == data) {
_gather = true; // class is interesting, maybe gather _gather = true; // class is interesting, maybe gather
_ck_b = _ck_a = data; // reset the checksum accumulators _ck_b = _ck_a = data; // reset the checksum accumulators
} else { } else {
_gather = false; // class is not interesting, discard _gather = false; // class is not interesting, discard
} }
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;
if (_gather) { // if class was interesting if (_gather) { // if class was interesting
switch(data) { switch(data) {
case MSG_POSLLH: // message is interesting case MSG_POSLLH: // message is interesting
_expect = sizeof(ubx_nav_posllh); _expect = sizeof(ubx_nav_posllh);
break; break;
case MSG_STATUS: case MSG_STATUS:
_expect = sizeof(ubx_nav_status); _expect = sizeof(ubx_nav_status);
break; break;
case MSG_SOL: case MSG_SOL:
_expect = sizeof(ubx_nav_solution); _expect = sizeof(ubx_nav_solution);
break; break;
case MSG_VELNED: case MSG_VELNED:
_expect = sizeof(ubx_nav_velned); _expect = sizeof(ubx_nav_velned);
break; break;
default: default:
_gather = false; // message is not interesting _gather = false; // message is not interesting
} }
} }
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; // payload length high byte _payload_length += (uint16_t)data; // payload length high byte
_payload_counter = 0; // prepare to receive payload _payload_counter = 0; // prepare to receive payload
if (_payload_length != _expect) if (_payload_length != _expect)
_gather = false; _gather = false;
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 (_gather) // gather data if requested if (_gather) // gather data if requested
_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)
_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)
break; // bad checksum break; // bad checksum
if (_gather) { if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet parsed = _parse_gps(); // Parse the new GPS packet
} }
} }
} }
return parsed; return parsed;
} }
// Private Methods ///////////////////////////////////////////////////////////// // Private Methods /////////////////////////////////////////////////////////////
@ -167,28 +167,28 @@ AP_GPS_UBLOX::read(void)
bool bool
AP_GPS_UBLOX::_parse_gps(void) AP_GPS_UBLOX::_parse_gps(void)
{ {
switch (_msg_id) { switch (_msg_id) {
case MSG_POSLLH: case MSG_POSLLH:
time = _buffer.posllh.time; time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude; longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude; latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10; altitude = _buffer.posllh.altitude_msl / 10;
break; break;
case MSG_STATUS: case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D); fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break; break;
case MSG_SOL: case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites; num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP; hdop = _buffer.solution.position_DOP;
break; break;
case MSG_VELNED: case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // 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 ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break; break;
default: default:
return false; return false;
} }
return true; return true;
} }

184
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

@ -19,106 +19,106 @@ class AP_GPS_UBLOX : public GPS
{ {
public: public:
// Methods // Methods
AP_GPS_UBLOX(Stream *s); AP_GPS_UBLOX(Stream *s);
virtual void init(); virtual void init();
virtual bool read(); virtual bool read();
private: private:
// u-blox UBX protocol essentials // u-blox UBX protocol essentials
// XXX this is being ignored by the compiler #pragma pack(1) // XXX this is being ignored by the compiler #pragma pack(1)
struct ubx_nav_posllh { struct ubx_nav_posllh {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
int32_t longitude; int32_t longitude;
int32_t latitude; int32_t latitude;
int32_t altitude_ellipsoid; int32_t altitude_ellipsoid;
int32_t altitude_msl; int32_t altitude_msl;
uint32_t horizontal_accuracy; uint32_t horizontal_accuracy;
uint32_t vertical_accuracy; uint32_t vertical_accuracy;
}; };
struct ubx_nav_status { struct ubx_nav_status {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
uint8_t fix_type; uint8_t fix_type;
uint8_t fix_status; uint8_t fix_status;
uint8_t differential_status; uint8_t differential_status;
uint8_t res; uint8_t res;
uint32_t time_to_first_fix; uint32_t time_to_first_fix;
uint32_t uptime; // milliseconds uint32_t uptime; // milliseconds
}; };
struct ubx_nav_solution { struct ubx_nav_solution {
uint32_t time; uint32_t time;
int32_t time_nsec; int32_t time_nsec;
int16_t week; int16_t week;
uint8_t fix_type; uint8_t fix_type;
uint8_t fix_status; uint8_t fix_status;
int32_t ecef_x; int32_t ecef_x;
int32_t ecef_y; int32_t ecef_y;
int32_t ecef_z; int32_t ecef_z;
uint32_t position_accuracy_3d; uint32_t position_accuracy_3d;
int32_t ecef_x_velocity; int32_t ecef_x_velocity;
int32_t ecef_y_velocity; int32_t ecef_y_velocity;
int32_t ecef_z_velocity; int32_t ecef_z_velocity;
uint32_t speed_accuracy; uint32_t speed_accuracy;
uint16_t position_DOP; uint16_t position_DOP;
uint8_t res; uint8_t res;
uint8_t satellites; uint8_t satellites;
uint32_t res2; uint32_t res2;
}; };
struct ubx_nav_velned { struct ubx_nav_velned {
uint32_t time; // GPS msToW uint32_t time; // GPS msToW
int32_t ned_north; int32_t ned_north;
int32_t ned_east; int32_t ned_east;
int32_t ned_down; int32_t ned_down;
uint32_t speed_3d; uint32_t speed_3d;
uint32_t speed_2d; uint32_t speed_2d;
int32_t heading_2d; int32_t heading_2d;
uint32_t speed_accuracy; uint32_t speed_accuracy;
uint32_t heading_accuracy; uint32_t heading_accuracy;
}; };
// // #pragma pack(pop) // // #pragma pack(pop)
enum ubs_protocol_bytes { enum ubs_protocol_bytes {
PREAMBLE1 = 0xb5, PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62, PREAMBLE2 = 0x62,
CLASS_NAV = 0x1, CLASS_NAV = 0x1,
MSG_POSLLH = 0x2, MSG_POSLLH = 0x2,
MSG_STATUS = 0x3, MSG_STATUS = 0x3,
MSG_SOL = 0x6, MSG_SOL = 0x6,
MSG_VELNED = 0x12 MSG_VELNED = 0x12
}; };
enum ubs_nav_fix_type { enum ubs_nav_fix_type {
FIX_NONE = 0, FIX_NONE = 0,
FIX_DEAD_RECKONING = 1, FIX_DEAD_RECKONING = 1,
FIX_2D = 2, FIX_2D = 2,
FIX_3D = 3, FIX_3D = 3,
FIX_GPS_DEAD_RECKONING = 4, FIX_GPS_DEAD_RECKONING = 4,
FIX_TIME = 5 FIX_TIME = 5
}; };
enum ubx_nav_status_bits { enum ubx_nav_status_bits {
NAV_STATUS_FIX_VALID = 1 NAV_STATUS_FIX_VALID = 1
}; };
// Packet checksum accumulators // Packet checksum accumulators
uint8_t _ck_a; uint8_t _ck_a;
uint8_t _ck_b; uint8_t _ck_b;
// State machine state // State machine state
uint8_t _step; uint8_t _step;
uint8_t _msg_id; uint8_t _msg_id;
bool _gather; bool _gather;
uint16_t _expect; uint16_t _expect;
uint16_t _payload_length; uint16_t _payload_length;
uint16_t _payload_counter; uint16_t _payload_counter;
// Receive buffer // Receive buffer
union { union {
ubx_nav_posllh posllh; ubx_nav_posllh posllh;
ubx_nav_status status; ubx_nav_status status;
ubx_nav_solution solution; ubx_nav_solution solution;
ubx_nav_velned velned; ubx_nav_velned velned;
uint8_t bytes[]; uint8_t bytes[];
} _buffer; } _buffer;
// Buffer parse & GPS state update // Buffer parse & GPS state update
bool _parse_gps(); bool _parse_gps();
}; };
#endif #endif

42
libraries/AP_GPS/GPS.cpp
View File

@ -6,30 +6,30 @@
void void
GPS::update(void) GPS::update(void)
{ {
bool result; bool result;
// call the GPS driver to process incoming data // call the GPS driver to process incoming data
result = read(); result = read();
// if we did not get a message, and the idle timer has expired, re-init // if we did not get a message, and the idle timer has expired, re-init
if (!result) { if (!result) {
if ((millis() - _idleTimer) > idleTimeout) { if ((millis() - _idleTimer) > idleTimeout) {
_status = NO_GPS; _status = NO_GPS;
init();
// reset the idle timer
_idleTimer = millis();
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
valid_read = true; init();
new_data = true; // reset the idle timer
_idleTimer = millis();
}
} else {
// we got a message, update our status correspondingly
_status = fix ? GPS_OK : NO_FIX;
// reset the idle timer valid_read = true;
_idleTimer = millis(); new_data = true;
}
// reset the idle timer
_idleTimer = millis();
}
} }
void void
@ -42,5 +42,5 @@ GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude,
void void
GPS::_error(const char *msg) GPS::_error(const char *msg)
{ {
Serial.println(msg); Serial.println(msg);
} }

288
libraries/AP_GPS/GPS.h
View File

@ -15,189 +15,193 @@ class GPS
{ {
public: public:
/// Update GPS state based on possible bytes received from the module. /// Update GPS state based on possible bytes received from the module.
/// ///
/// This routine must be called periodically to process incoming data. /// This routine must be called periodically to process incoming data.
/// ///
/// GPS drivers should not override this function; they should implement /// GPS drivers should not override this function; they should implement
/// ::read instead. /// ::read instead.
/// ///
void update(void); void update(void);
void (*callback)(unsigned long t); void (*callback)(unsigned long t);
/// GPS status codes /// GPS status codes
/// ///
/// \note Non-intuitive ordering for legacy reasons /// \note Non-intuitive ordering for legacy reasons
/// ///
enum GPS_Status { enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK = 2 ///< Receiving valid messages and locked GPS_OK = 2 ///< Receiving valid messages and locked
}; };
/// Query GPS status /// Query GPS status
/// ///
/// The 'valid message' status indicates that a recognised message was /// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms. /// received from the GPS within the last 500ms.
/// ///
/// @returns Current GPS status /// @returns Current GPS status
/// ///
GPS_Status status(void) { return _status; } GPS_Status status(void) {
return _status;
}
/// GPS time epoch codes /// GPS time epoch codes
/// ///
enum GPS_Time_Epoch { enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///< TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available UNIX_EPOCH = 3 ///< If available
}; ///< SIFR? }; ///< SIFR?
/// Query GPS time epoch /// Query GPS time epoch
/// ///
/// @returns Current GPS time epoch code /// @returns Current GPS time epoch code
/// ///
GPS_Time_Epoch epoch(void) { return _epoch; } GPS_Time_Epoch epoch(void) {
return _epoch;
}
/// Startup initialisation. /// Startup initialisation.
/// ///
/// This routine performs any one-off initialisation required to set the /// This routine performs any one-off initialisation required to set the
/// GPS up for use. /// GPS up for use.
/// ///
/// Must be implemented by the GPS driver. /// Must be implemented by the GPS driver.
/// ///
virtual void init(void) = 0; virtual void init(void) = 0;
// Properties // Properties
long time; ///< GPS time (milliseconds from epoch) long time; ///< GPS time (milliseconds from epoch)
long date; ///< GPS date (FORMAT TBD) long date; ///< GPS date (FORMAT TBD)
long latitude; ///< latitude in degrees * 10,000,000 long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000 long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm long altitude; ///< altitude in cm
long ground_speed; ///< ground speed in cm/sec long ground_speed; ///< ground speed in cm/sec
long ground_course; ///< ground course in 100ths of a degree long ground_course; ///< ground course in 100ths of a degree
long speed_3d; ///< 3D speed in cm/sec (not always available) long speed_3d; ///< 3D speed in cm/sec (not always available)
int hdop; ///< horizontal dilution of precision in cm int hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites uint8_t num_sats; ///< Number of visible satelites
/// Set to true when new data arrives. A client may set this /// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have /// to false in order to avoid processing data they have
/// already seen. /// already seen.
bool new_data; bool new_data;
// Deprecated properties // Deprecated properties
bool fix; ///< true if we have a position fix (use ::status instead) bool fix; ///< true if we have a position fix (use ::status instead)
bool valid_read; ///< true if we have seen data from the GPS (use ::status instead) bool valid_read; ///< true if we have seen data from the GPS (use ::status instead)
// Debug support // Debug support
bool print_errors; ///< deprecated bool print_errors; ///< deprecated
// HIL support // HIL support
virtual void setHIL(long time, float latitude, float longitude, float altitude, virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats); float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
/// Time in milliseconds after which we will assume the GPS is no longer /// Time in milliseconds after which we will assume the GPS is no longer
/// sending us updates and attempt a re-init. /// sending us updates and attempt a re-init.
/// ///
/// 1200ms allows a small amount of slack over the worst-case 1Hz update /// 1200ms allows a small amount of slack over the worst-case 1Hz update
/// rate. /// rate.
/// ///
unsigned long idleTimeout; unsigned long idleTimeout;
protected: protected:
Stream *_port; ///< port the GPS is attached to Stream *_port; ///< port the GPS is attached to
/// Constructor /// Constructor
/// ///
/// @note The stream is expected to be set up and configured for the /// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called. /// correct bitrate before ::init is called.
/// ///
/// @param s Stream connected to the GPS module. /// @param s Stream connected to the GPS module.
/// ///
GPS(Stream *s) : _port(s) {}; GPS(Stream *s) : _port(s) {};
/// read from the GPS stream and update properties /// read from the GPS stream and update properties
/// ///
/// Must be implemented by the GPS driver. /// Must be implemented by the GPS driver.
/// ///
/// @returns true if a valid message was received from the GPS /// @returns true if a valid message was received from the GPS
/// ///
virtual bool read(void) = 0; virtual bool read(void) = 0;
/// perform an endian swap on a long /// perform an endian swap on a long
/// ///
/// @param bytes pointer to a buffer containing bytes representing a /// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order /// long in the wrong byte order
/// @returns endian-swapped value /// @returns endian-swapped value
/// ///
long _swapl(const void *bytes); long _swapl(const void *bytes);
/// perform an endian swap on an int /// perform an endian swap on an int
/// ///
/// @param bytes pointer to a buffer containing bytes representing an /// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order /// int in the wrong byte order
/// @returns endian-swapped value /// @returns endian-swapped value
int16_t _swapi(const void *bytes); int16_t _swapi(const void *bytes);
/// emit an error message /// emit an error message
/// ///
/// based on the value of print_errors, emits the printf-formatted message /// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr /// in msg,... to stderr
/// ///
/// @param fmt printf-like format string /// @param fmt printf-like format string
/// ///
/// @note deprecated as-is due to the difficulty of hooking up to a working /// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits /// printf vs. the potential benefits
/// ///
void _error(const char *msg); void _error(const char *msg);
/// Time epoch code for the gps in use /// Time epoch code for the gps in use
GPS_Time_Epoch _epoch; GPS_Time_Epoch _epoch;
private: private:
/// Last time that the GPS driver got a good packet from the GPS /// Last time that the GPS driver got a good packet from the GPS
/// ///
unsigned long _idleTimer; unsigned long _idleTimer;
/// Our current status /// Our current status
GPS_Status _status; GPS_Status _status;
}; };
inline long inline long
GPS::_swapl(const void *bytes) GPS::_swapl(const void *bytes)
{ {
const uint8_t *b = (const uint8_t *)bytes; const uint8_t *b = (const uint8_t *)bytes;
union { union {
long v; long v;
uint8_t b[4]; uint8_t b[4];
} u; } u;
u.b[0] = b[3]; u.b[0] = b[3];
u.b[1] = b[2]; u.b[1] = b[2];
u.b[2] = b[1]; u.b[2] = b[1];
u.b[3] = b[0]; u.b[3] = b[0];
return(u.v); return(u.v);
} }
inline int16_t inline int16_t
GPS::_swapi(const void *bytes) GPS::_swapi(const void *bytes)
{ {
const uint8_t *b = (const uint8_t *)bytes; const uint8_t *b = (const uint8_t *)bytes;
union { union {
int16_t v; int16_t v;
uint8_t b[2]; uint8_t b[2];
} u; } u;
u.b[0] = b[1]; u.b[0] = b[1];
u.b[1] = b[0]; u.b[1] = b[0];
return(u.v); return(u.v);
} }
#endif #endif

62
libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde
View File

@ -19,41 +19,41 @@ AP_GPS_406 gps(&Serial1);
void setup() void setup()
{ {
tone(A6, 1000, 200); tone(A6, 1000, 200);
Serial.begin(38400, 16, 128); Serial.begin(38400, 16, 128);
Serial1.begin(57600, 128, 16); Serial1.begin(57600, 128, 16);
stderr = stdout; stderr = stdout;
gps.print_errors = true; gps.print_errors = true;
Serial.println("GPS 406 library test"); Serial.println("GPS 406 library test");
gps.init(); // GPS Initialization gps.init(); // GPS Initialization
delay(1000); delay(1000);
} }
void loop() void loop()
{ {
delay(20); delay(20);
gps.update(); gps.update();
if (gps.new_data){ if (gps.new_data) {
Serial.print("gps:"); Serial.print("gps:");
Serial.print(" Lat:"); Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC); Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:"); Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC); Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:"); Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC); Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:"); Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0); Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:"); Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC); Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:"); Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC); Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:"); Serial.print(" FIX:");
Serial.print(gps.fix, DEC); Serial.print(gps.fix, DEC);
Serial.print(" TIM:"); Serial.print(" TIM:");
Serial.print(gps.time, DEC); Serial.print(gps.time, DEC);
Serial.println(); Serial.println();
gps.new_data = 0; // We have readed the data gps.new_data = 0; // We have readed the data
} }
} }

42
libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde
View File

@ -18,31 +18,31 @@ AP_GPS_Auto GPS(&Serial1, &gps);
void setup() void setup()
{ {
Serial.begin(115200); Serial.begin(115200);
Serial1.begin(38400); Serial1.begin(38400);
Serial.println("GPS AUTO library test"); Serial.println("GPS AUTO library test");
gps = &GPS; gps = &GPS;
gps->init(); gps->init();
} }
void loop() void loop()
{ {
gps->update(); gps->update();
if (gps->new_data){ if (gps->new_data) {
if (gps->fix) { if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu", Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7, (float)gps->latitude / T7,
(float)gps->longitude / T7, (float)gps->longitude / T7,
(float)gps->altitude / 100.0, (float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0, (float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100, (int)gps->ground_course / 100,
gps->num_sats, gps->num_sats,
gps->time); gps->time);
} else { } else {
Serial.println("No fix"); Serial.println("No fix");
} }
gps->new_data = false; gps->new_data = false;
} }
} }

60
libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde
View File

@ -19,39 +19,39 @@ AP_GPS_MTK gps(&Serial1);
void setup() void setup()
{ {
Serial.begin(38400); Serial.begin(38400);
Serial1.begin(38400); Serial1.begin(38400);
stderr = stdout; stderr = stdout;
gps.print_errors = true; gps.print_errors = true;
Serial.println("GPS MTK library test"); Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization gps.init(); // GPS Initialization
delay(1000); delay(1000);
} }
void loop() void loop()
{ {
delay(20); delay(20);
gps.update(); gps.update();
if (gps.new_data){ if (gps.new_data) {
Serial.print("gps:"); Serial.print("gps:");
Serial.print(" Lat:"); Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC); Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:"); Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC); Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:"); Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC); Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:"); Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0); Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:"); Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC); Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:"); Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC); Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:"); Serial.print(" FIX:");
Serial.print(gps.fix, DEC); Serial.print(gps.fix, DEC);
Serial.print(" TIM:"); Serial.print(" TIM:");
Serial.print(gps.time, DEC); Serial.print(gps.time, DEC);
Serial.println(); Serial.println();
gps.new_data = 0; // We have readed the data gps.new_data = 0; // We have readed the data
} }
} }

81
libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde
View File

@ -18,54 +18,55 @@ GPS *gps = &NMEA_gps;
#define T7 10000000 #define T7 10000000
const uint8_t sirf_to_nmea[] = { 0xa0, 0xa2, // preamble const uint8_t sirf_to_nmea[] = { 0xa0, 0xa2, // preamble
0x00, 0x18, // message length 0x00, 0x18, // message length
0x81, 0x02, // switch to NMEA 0x81, 0x02, // switch to NMEA
0x01, 0x01, // GGA on with checksum 0x01, 0x01, // GGA on with checksum
0x00, 0x01, // GLL off 0x00, 0x01, // GLL off
0x00, 0x01, // GSA off 0x00, 0x01, // GSA off
0x00, 0x01, // GSV off 0x00, 0x01, // GSV off
0x01, 0x01, // RMC on with checksum 0x01, 0x01, // RMC on with checksum
0x01, 0x01, // VTG on with checksum 0x01, 0x01, // VTG on with checksum
0x00, 0x01, // MSS off 0x00, 0x01, // MSS off
0x00, 0x01, // EPE off 0x00, 0x01, // EPE off
0x00, 0x01, // ZPA off 0x00, 0x01, // ZPA off
0x00, 0x00, // pad 0x00, 0x00, // pad
0x96, 0x00, // 38400 0x96, 0x00, // 38400
0x01, 0x25, // checksum TBD 0x01, 0x25, // checksum TBD
0xb0, 0xb3 }; // postamble 0xb0, 0xb3
}; // postamble
void setup() void setup()
{ {
Serial.begin(38400); Serial.begin(38400);
Serial1.begin(38400); Serial1.begin(38400);
// try to coerce a SiRF unit that's been traumatized by // try to coerce a SiRF unit that's been traumatized by
// AP_GPS_AUTO back into NMEA mode so that we can test // AP_GPS_AUTO back into NMEA mode so that we can test
// it. // it.
for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++) for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++)
Serial1.write(sirf_to_nmea[i]); Serial1.write(sirf_to_nmea[i]);
Serial.println("GPS NMEA library test"); Serial.println("GPS NMEA library test");
gps->init(); gps->init();
} }
void loop() void loop()
{ {
gps->update(); gps->update();
if (gps->new_data){ if (gps->new_data) {
if (gps->fix) { if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu", Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7, (float)gps->latitude / T7,
(float)gps->longitude / T7, (float)gps->longitude / T7,
(float)gps->altitude / 100.0, (float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0, (float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100, (int)gps->ground_course / 100,
gps->num_sats, gps->num_sats,
gps->time); gps->time);
} else { } else {
Serial.println("No fix"); Serial.println("No fix");
} }
gps->new_data = false; gps->new_data = false;
} }
} }

60
libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde
View File

@ -19,39 +19,39 @@ AP_GPS_UBLOX gps(&Serial1);
void setup() void setup()
{ {
Serial.begin(38400); Serial.begin(38400);
Serial1.begin(38400); Serial1.begin(38400);
stderr = stdout; stderr = stdout;
gps.print_errors = true; gps.print_errors = true;
Serial.println("GPS UBLOX library test"); Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization gps.init(); // GPS Initialization
delay(1000); delay(1000);
} }
void loop() void loop()
{ {
delay(20); delay(20);
gps.update(); gps.update();
if (gps.new_data){ if (gps.new_data) {
Serial.print("gps:"); Serial.print("gps:");
Serial.print(" Lat:"); Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC); Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:"); Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC); Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:"); Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC); Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:"); Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0); Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:"); Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC); Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:"); Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC); Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:"); Serial.print(" FIX:");
Serial.print(gps.fix, DEC); Serial.print(gps.fix, DEC);
Serial.print(" TIM:"); Serial.print(" TIM:");
Serial.print(gps.time, DEC); Serial.print(gps.time, DEC);
Serial.println(); Serial.println();
gps.new_data = 0; // We have readed the data gps.new_data = 0; // We have readed the data
} }
} }