aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions 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 dd0ac600b3
commit 57301ce647
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 libraries/APO
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 ////////////////////////////////////////////////////////////////////
void AP_GPS_406::init(void)
{
_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
_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
AP_GPS_SIRF::init(); // let the superclass do anything it might need here
idleTimeout = 1200;
AP_GPS_SIRF::init(); // let the superclass do anything it might need here
idleTimeout = 1200;
}
// Private Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_406::_configure_gps(void)
{
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_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++){
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_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0);
_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[1]); // ender
}
}
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_checksum[] = {0xA8, 0xAA, 0xAD, 0xAF, 0xC1};
const uint8_t gps_ender[] = {0xB0, 0xB3};
for(int z = 0; z < 2; z++) {
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_payload[x]); // Prints the payload, is not the same for every msg
for(int y = 0; y < 6; y++) // Prints 6 zeros
_port->write((uint8_t)0);
_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[1]); // ender
}
}
}
// 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
// settings (usually less than a week).
//
void
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);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(4800);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(9600);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(9600);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(GPS_406_BITRATE);
delay(300);
_port->print(init_str);
delay(300);
fs->begin(GPS_406_BITRATE);
delay(300);
_port->print(init_str);
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:
// Methods
AP_GPS_406(Stream *port);
virtual void init(void);
AP_GPS_406(Stream *port);
virtual void init(void);
private:
void _change_to_sirf_protocol(void);
void _configure_gps(void);
void _change_to_sirf_protocol(void);
void _configure_gps(void);
};
#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) :
GPS(s),
_fs(s),
_gps(gps)
GPS(s),
_fs(s),
_gps(gps)
{
}
@ -36,8 +36,8 @@ AP_GPS_Auto::AP_GPS_Auto(FastSerial *s, GPS **gps) :
void
AP_GPS_Auto::init(void)
{
idleTimeout = 1200;
if (callback == NULL) callback = delay;
idleTimeout = 1200;
if (callback == NULL) callback = delay;
}
// Called the first time that a client tries to kick the GPS to update.
@ -53,41 +53,41 @@ AP_GPS_Auto::init(void)
bool
AP_GPS_Auto::read(void)
{
GPS *gps;
uint8_t i;
unsigned long then;
GPS *gps;
uint8_t i;
unsigned long then;
// 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
// Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times.
//
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
// 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
// Stream has no idea of line speeds. FastSerial is quite OK with us calling
// ::begin any number of times.
//
for (i = 0; i < (sizeof(baudrates) / sizeof(baudrates[0])); i++) {
_fs->begin(baudrates[i]);
if (NULL != (gps = _detect())) {
_fs->begin(baudrates[i]);
if (NULL != (gps = _detect())) {
// configure the detected GPS and give it a chance to listen to its device
gps->init();
then = millis();
while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done
gps->new_data = false;
gps->update();
if (gps->new_data) {
Serial.println_P(PSTR("OK"));
*_gps = gps;
return true;
}
}
// GPS driver failed to parse any data from GPS,
// delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying"));
delete gps;
}
}
return false;
// configure the detected GPS and give it a chance to listen to its device
gps->init();
then = millis();
while ((millis() - then) < 1200) {
// if we get a successful update from the GPS, we are done
gps->new_data = false;
gps->update();
if (gps->new_data) {
Serial.println_P(PSTR("OK"));
*_gps = gps;
return true;
}
}
// GPS driver failed to parse any data from GPS,
// delete the driver and continue the process.
Serial.println_P(PSTR("failed, retrying"));
delete gps;
}
}
return false;
}
//
@ -96,126 +96,126 @@ AP_GPS_Auto::read(void)
GPS *
AP_GPS_Auto::_detect(void)
{
unsigned long then;
int fingerprint[4];
int tries;
GPS *gps;
unsigned long then;
int fingerprint[4];
int tries;
GPS *gps;
//
// Loop attempting to detect a recognized GPS
//
Serial.print('G');
gps = NULL;
for (tries = 0; tries < 2; tries++) {
//
// Loop attempting to detect a recognized GPS
//
Serial.print('G');
gps = NULL;
for (tries = 0; tries < 2; tries++) {
//
// Empty the serial buffer and wait for 50ms of quiet.
//
// XXX We can detect babble by counting incoming characters, but
// what would we do about it?
//
_port->flush();
then = millis();
do {
callback(1);
if (_port->available()) {
then = millis();
_port->read();
}
} while ((millis() - then) < 50);
//
// Empty the serial buffer and wait for 50ms of quiet.
//
// XXX We can detect babble by counting incoming characters, but
// what would we do about it?
//
_port->flush();
then = millis();
do {
callback(1);
if (_port->available()) {
then = millis();
_port->read();
}
} while ((millis() - then) < 50);
//
// Collect four characters to fingerprint a device
//
// If we take more than 1200ms to receive four characters, abort.
// This will normally only be the case where there is no GPS attached.
//
while (_port->available() < 4) {
callback(1);
if ((millis() - then) > 1200) {
Serial.print('!');
return NULL;
}
}
fingerprint[0] = _port->read();
fingerprint[1] = _port->read();
fingerprint[2] = _port->read();
fingerprint[3] = _port->read();
//
// Collect four characters to fingerprint a device
//
// If we take more than 1200ms to receive four characters, abort.
// This will normally only be the case where there is no GPS attached.
//
while (_port->available() < 4) {
callback(1);
if ((millis() - then) > 1200) {
Serial.print('!');
return NULL;
}
}
fingerprint[0] = _port->read();
fingerprint[1] = _port->read();
fingerprint[2] = _port->read();
fingerprint[3] = _port->read();
//
// ublox or MTK in DIYD binary mode (whose smart idea was
// it to make the MTK look sort-of like it was talking UBX?)
//
if ((0xb5 == fingerprint[0]) &&
(0x62 == fingerprint[1]) &&
(0x01 == fingerprint[2])) {
//
// ublox or MTK in DIYD binary mode (whose smart idea was
// it to make the MTK look sort-of like it was talking UBX?)
//
if ((0xb5 == fingerprint[0]) &&
(0x62 == fingerprint[1]) &&
(0x01 == fingerprint[2])) {
// message 5 is MTK pretending to talk UBX
if (0x05 == fingerprint[3]) {
gps = new AP_GPS_MTK(_port);
Serial.print_P(PSTR(" MTK1.4 "));
break;
}
// message 5 is MTK pretending to talk UBX
if (0x05 == fingerprint[3]) {
gps = new AP_GPS_MTK(_port);
Serial.print_P(PSTR(" MTK1.4 "));
break;
}
// any other message is ublox
gps = new AP_GPS_UBLOX(_port);
Serial.print_P(PSTR(" ublox "));
break;
}
// any other message is ublox
gps = new AP_GPS_UBLOX(_port);
Serial.print_P(PSTR(" ublox "));
break;
}
//
// MTK v1.6
//
if ((0xd0 == fingerprint[0]) &&
(0xdd == fingerprint[1]) &&
(0x20 == fingerprint[2])) {
gps = new AP_GPS_MTK16(_port);
Serial.print_P(PSTR(" MTK1.6 "));
break;
}
//
// MTK v1.6
//
if ((0xd0 == fingerprint[0]) &&
(0xdd == fingerprint[1]) &&
(0x20 == fingerprint[2])) {
gps = new AP_GPS_MTK16(_port);
Serial.print_P(PSTR(" MTK1.6 "));
break;
}
//
// SIRF in binary mode
//
if ((0xa0 == fingerprint[0]) &&
(0xa2 == fingerprint[1])) {
gps = new AP_GPS_SIRF(_port);
Serial.print_P(PSTR(" SiRF "));
break;
}
//
// SIRF in binary mode
//
if ((0xa0 == fingerprint[0]) &&
(0xa2 == fingerprint[1])) {
gps = new AP_GPS_SIRF(_port);
Serial.print_P(PSTR(" SiRF "));
break;
}
//
// If we haven't spammed the various init strings, send them now
// and retry to avoid a false-positive on the NMEA detector.
//
if (0 == tries) {
Serial.print('*');
// 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 *)_ublox_set_binary);
_fs->println_P((const prog_char_t *)_sirf_set_binary);
//
// If we haven't spammed the various init strings, send them now
// and retry to avoid a false-positive on the NMEA detector.
//
if (0 == tries) {
Serial.print('*');
// 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 *)_ublox_set_binary);
_fs->println_P((const prog_char_t *)_sirf_set_binary);
// give the GPS time to react to the settings
callback(100);
continue;
} else {
Serial.print('?');
}
// give the GPS time to react to the settings
callback(100);
continue;
} else {
Serial.print('?');
}
#if WITH_NMEA_MODE
//
// Something talking NMEA
//
if (('$' == fingerprint[0]) &&
(('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
//
// Something talking NMEA
//
if (('$' == fingerprint[0]) &&
(('G' == fingerprint[1]) || ('P' == fingerprint[1]))) {
// 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?
gps = new AP_GPS_NMEA(_port);
break;
}
// 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?
gps = new AP_GPS_NMEA(_port);
break;
}
#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
{
public:
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param port Stream connected to the GPS module.
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected.
///
AP_GPS_Auto(FastSerial *s, GPS **gps);
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param port Stream connected to the GPS module.
/// @param ptr Pointer to a GPS * that will be fixed up by ::init
/// when the GPS type has been detected.
///
AP_GPS_Auto(FastSerial *s, GPS **gps);
/// Dummy init routine, does nothing
virtual void init(void);
/// Dummy init routine, does nothing
virtual void init(void);
/// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected.
///
virtual bool read(void);
/// Detect and initialise the attached GPS unit. Updates the
/// pointer passed into the constructor when a GPS is detected.
///
virtual bool read(void);
private:
/// Copy of the port, known at construction time to be a real FastSerial port.
FastSerial *_fs;
/// Copy of the port, known at construction time to be a real FastSerial port.
FastSerial *_fs;
/// global GPS driver pointer, updated by auto-detection
///
GPS **_gps;
/// global GPS driver pointer, updated by auto-detection
///
GPS **_gps;
/// low-level auto-detect routine
///
GPS *_detect(void);
/// low-level auto-detect routine
///
GPS *_detect(void);
static const prog_char _mtk_set_binary[];
static const prog_char _ublox_set_binary[];
static const prog_char _sirf_set_binary[];
static const prog_char _mtk_set_binary[];
static const prog_char _ublox_set_binary[];
static const prog_char _sirf_set_binary[];
};
#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 //////////////////////////////////////////////////////////////
void AP_GPS_HIL::init(void)
{
idleTimeout = 1200;
{
idleTimeout = 1200;
}
bool AP_GPS_HIL::read(void)
{
bool result = _updated;
bool result = _updated;
// return true once for each update pushed in
_updated = false;
return result;
// return true once for each update pushed in
_updated = false;
return result;
}
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;
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;
num_sats = _num_sats;
fix = true;
new_data = true;
_updated = true;
new_data = true;
_updated = true;
}

10
libraries/AP_GPS/AP_GPS_HIL.h
View File

@ -16,9 +16,9 @@
class AP_GPS_HIL : public GPS {
public:
AP_GPS_HIL(Stream *s);
virtual void init(void);
virtual bool read(void);
AP_GPS_HIL(Stream *s);
virtual void init(void);
virtual bool read(void);
/**
* Hardware in the loop set function
@ -31,10 +31,10 @@ public:
* @param altitude - altitude in meters
*/
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:
bool _updated;
bool _updated;
};
#endif // AP_GPS_HIL_H

282
libraries/AP_GPS/AP_GPS_IMU.cpp
View File

@ -15,7 +15,7 @@
Methods:
init() : GPS initialization
update() : Call this funcion as often as you want to ensure you read the incomming gps data
Properties:
lattitude : lattitude * 10000000 (long value)
longitude : longitude * 10000000 (long value)
@ -25,7 +25,7 @@
new_data : 1 when a new data is received.
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.
*/
#include "AP_GPS_IMU.h"
#include "WProgram.h"
@ -38,192 +38,192 @@ AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s)
// Public Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_IMU::init(void)
{
// we expect the stream to already be open at the corret bitrate
idleTimeout = 1200;
// we expect the stream to already be open at the corret bitrate
idleTimeout = 1200;
}
// 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)
// 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)
{
byte data;
int numc = 0;
byte data;
int numc = 0;
numc = _port->available();
numc = _port->available();
if (numc > 0){
for (int i=0;i<numc;i++){ // Process bytes received
if (numc > 0) {
for (int i=0; i<numc; i++) { // Process bytes received
data = _port->read();
switch(step){ //Normally we start from zero. This is a state machine
case 0:
if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 2:
if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 3:
if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 4:
payload_length = data;
checksum(payload_length);
step++;
if (payload_length > 28){
step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0;
ck_a = 0;
ck_b = 0;
//payload_error_count++;
}
break;
case 5:
message_num = data;
checksum(data);
step++;
break;
case 6: // Payload data read...
// We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter >= payload_length) {
step++;
}
break;
case 7:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 8:
GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read...
// Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) {
join_data();
} else if (message_num == 0x03) {
GPS_join_data();
} else if (message_num == 0x04) {
join_data_xplane();
} else if (message_num == 0x0a) {
//PERF_join_data();
} else {
data = _port->read();
switch(step) { //Normally we start from zero. This is a state machine
case 0:
if(data == 0x44) // IMU sync char 1
step++; //OH first data packet is correct, so jump to the next step
break;
case 1:
if(data == 0x49) // IMU sync char 2
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 2:
if(data == 0x59) // IMU sync char 3
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 3:
if(data == 0x64) // IMU sync char 4
step++; //ooh! The second data packet is correct, jump to the step 2
else
step=0; //Nop, is not correct so restart to step zero and try again.
break;
case 4:
payload_length = data;
checksum(payload_length);
step++;
if (payload_length > 28) {
step = 0; //Bad data, so restart to step zero and try again.
payload_counter = 0;
ck_a = 0;
ck_b = 0;
//payload_error_count++;
}
break;
case 5:
message_num = data;
checksum(data);
step++;
break;
case 6: // Payload data read...
// We stay in this state until we reach the payload_length
buffer[payload_counter] = data;
checksum(data);
payload_counter++;
if (payload_counter >= payload_length) {
step++;
}
break;
case 7:
GPS_ck_a = data; // First checksum byte
step++;
break;
case 8:
GPS_ck_b = data; // Second checksum byte
// We end the IMU/GPS read...
// Verify the received checksum with the generated checksum..
if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)) {
if (message_num == 0x02) {
join_data();
} else if (message_num == 0x03) {
GPS_join_data();
} else if (message_num == 0x04) {
join_data_xplane();
} else if (message_num == 0x0a) {
//PERF_join_data();
} else {
// _error("Invalid message number = %d\n", (int)message_num);
}
} else {
}
} else {
// _error("XXX Checksum error\n"); //bad checksum
//imu_checksum_error_count++;
}
// Variable initialization
step = 0;
payload_counter = 0;
ck_a = 0;
ck_b = 0;
break;
}
}// End for...
}
return true;
//imu_checksum_error_count++;
}
// Variable initialization
step = 0;
payload_counter = 0;
ck_a = 0;
ck_b = 0;
break;
}
}// End for...
}
return true;
}
/****************************************************************
*
*
****************************************************************/
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..
//In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
//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.
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4];
imu_ok = true;
//Storing IMU heading (yaw)
ground_course = *(int *)&buffer[4];
imu_ok = true;
}
void AP_GPS_IMU::join_data_xplane()
{
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU roll
roll_sensor = *(int *)&buffer[0];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
//Storing IMU heading (yaw)
ground_course = *(unsigned int *)&buffer[4];
//Storing airspeed
airspeed = *(int *)&buffer[6];
//Storing IMU pitch
pitch_sensor = *(int *)&buffer[2];
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)
{
longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4];
longitude = *(long *)&buffer[0]; // degrees * 10e7
latitude = *(long *)&buffer[4];
//Storing GPS Height above the sea level
altitude = (long)*(int *)&buffer[8] * 10;
//Storing GPS Height above the sea level
altitude = (long)*(int *)&buffer[8] * 10;
//Storing Speed
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];
//Storing Speed
speed_3d = ground_speed = (float)*(int *)&buffer[10];
imu_health = buffer[15];
new_data = true;
fix = true;
//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];
new_data = true;
fix = true;
}
/****************************************************************
*
*
****************************************************************/
// checksum algorithm
void AP_GPS_IMU::checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
ck_a += data;
ck_b += ck_a;
}
@ -231,4 +231,4 @@ void AP_GPS_IMU::checksum(byte data)
* Unused
****************************************************************/
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
class AP_GPS_IMU : public GPS {
public:
public:
// Methods
AP_GPS_IMU(Stream *s);
virtual void init(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);
AP_GPS_IMU(Stream *s);
virtual void init(void);
virtual bool read(void);
private:
// Packet checksums
uint8_t ck_a;
uint8_t ck_b;
uint8_t GPS_ck_a;
uint8_t GPS_ck_b;
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);
// 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:
// Packet checksums
uint8_t ck_a;
uint8_t ck_b;
uint8_t GPS_ck_a;
uint8_t GPS_ck_b;
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

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

74
libraries/AP_GPS/AP_GPS_MTK.h
View File

@ -20,52 +20,52 @@
class AP_GPS_MTK : public GPS {
public:
AP_GPS_MTK(Stream *s);
virtual void init(void);
virtual bool read(void);
AP_GPS_MTK(Stream *s);
virtual void init(void);
virtual bool read(void);
private:
// XXX this is being ignored by the compiler #pragma pack(1)
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
};
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_time;
};
// #pragma pack(pop)
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
MESSAGE_CLASS = 1,
MESSAGE_ID = 5
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xb5,
PREAMBLE2 = 0x62,
MESSAGE_CLASS = 1,
MESSAGE_ID = 5
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
// Buffer parse & GPS state update
void _parse_gps();
// Buffer parse & GPS state update
void _parse_gps();
};
#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 //////////////////////////////////////////////////////////////
void
void
AP_GPS_MTK16::init(void)
{
_port->flush();
{
_port->flush();
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// initialize serial port for binary protocol use
// XXX should assume binary, let GPS_AUTO handle dynamic config?
_port->print(MTK_SET_BINARY);
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;
_time_offset = 0;
_offset_calculated = false;
idleTimeout = 1200;
// set 4Hz update rate
_port->print(MTK_OUTPUT_4HZ);
// set initial epoch code
_epoch = TIME_OF_DAY;
_time_offset = 0;
_offset_calculated = false;
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -53,109 +53,109 @@ AP_GPS_MTK16::init(void)
bool
AP_GPS_MTK16::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
restart:
switch(_step){
restart:
switch(_step) {
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (sizeof(_buffer) == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
_payload_counter = 0;
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
// Message preamble, class, ID detection
//
// If we fail to match any of the expected bytes, we
// reset the state machine and re-consider the failed
// byte as the first byte of the preamble. This
// improves our chances of recovering from a mismatch
// and makes it less likely that we will be fooled by
// the preamble appearing as data in some other message.
//
case 0:
if(PREAMBLE1 == data)
_step++;
break;
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
goto restart;
case 2:
if (sizeof(_buffer) == data) {
_step++;
_ck_b = _ck_a = data; // reset the checksum accumulators
_payload_counter = 0;
} else {
_step = 0; // reset and wait for a message of the right class
goto restart;
}
break;
// Receive message data
//
case 3:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Receive message data
//
case 3:
_buffer.bytes[_payload_counter++] = data;
_ck_b += (_ck_a += data);
if (_payload_counter == sizeof(_buffer))
_step++;
break;
// Checksum and message processing
//
case 4:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 5:
_step = 0;
if (_ck_b != data) {
break;
}
// Checksum and message processing
//
case 4:
_step++;
if (_ck_a != data) {
_step = 0;
}
break;
case 5:
_step = 0;
if (_ck_b != data) {
break;
}
fix = _buffer.msg.fix_type == FIX_3D;
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
altitude = _buffer.msg.altitude;
ground_speed = _buffer.msg.ground_speed;
ground_course = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop;
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;
fix = _buffer.msg.fix_type == FIX_3D;
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
altitude = _buffer.msg.altitude;
ground_speed = _buffer.msg.ground_speed;
ground_course = _buffer.msg.ground_course;
num_sats = _buffer.msg.satellites;
hdop = _buffer.msg.hdop;
date = _buffer.msg.utc_date;
parsed = 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;
long year = tempd1 - month * 100;
_time_offset = _calc_epoch_offset(day, month, year);
_epoch = UNIX_EPOCH;
_offset_calculated = TRUE;
}
*/
}
}
return parsed;
// 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;
/* Waiting on clarification of MAVLink protocol!
if(!_offset_calculated && parsed) {
long tempd1 = date;
long day = tempd1/10000;
tempd1 -= day * 10000;
long month = tempd1/100;
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 {
public:
AP_GPS_MTK16(Stream *s);
virtual void init(void);
virtual bool read(void);
AP_GPS_MTK16(Stream *s);
virtual void init(void);
virtual bool read(void);
private:
// XXX this is being ignored by the compiler #pragma pack(1)
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_date;
uint32_t utc_time;
uint16_t hdop;
};
struct diyd_mtk_msg {
int32_t latitude;
int32_t longitude;
int32_t altitude;
int32_t ground_speed;
int32_t ground_course;
uint8_t satellites;
uint8_t fix_type;
uint32_t utc_date;
uint32_t utc_time;
uint16_t hdop;
};
// #pragma pack(pop)
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_fix_type {
FIX_NONE = 1,
FIX_2D = 2,
FIX_3D = 3
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xd0,
PREAMBLE2 = 0xdd,
};
enum diyd_mtk_protocol_bytes {
PREAMBLE1 = 0xd0,
PREAMBLE2 = 0xdd,
};
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// Packet checksum accumulators
uint8_t _ck_a;
uint8_t _ck_b;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// Time from UNIX Epoch offset
long _time_offset;
bool _offset_calculated;
// State machine state
uint8_t _step;
uint8_t _payload_counter;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
// Time from UNIX Epoch offset
long _time_offset;
bool _offset_calculated;
// Receive buffer
union {
diyd_mtk_msg msg;
uint8_t bytes[];
} _buffer;
};
#endif // AP_GPS_MTK16_H

498
libraries/AP_GPS/AP_GPS_NMEA.cpp
View File

@ -41,17 +41,17 @@
// the autodetection process.
//
const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
"$PSRF103,1,0,0,1*25\r\n" // GLL off
"$PSRF103,2,0,0,1*26\r\n" // GSA off
"$PSRF103,3,0,0,1*27\r\n" // GSV off
"$PSRF103,4,0,1,1*20\r\n" // RMC off
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
"$PSRF103,6,0,0,1*22\r\n" // MSS off
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
"$PSRF106,21*0F\r\n" // datum = WGS84
"";
"$PSRF103,0,0,1,1*25\r\n" // GGA @ 1Hz
"$PSRF103,1,0,0,1*25\r\n" // GLL off
"$PSRF103,2,0,0,1*26\r\n" // GSA off
"$PSRF103,3,0,0,1*27\r\n" // GSV off
"$PSRF103,4,0,1,1*20\r\n" // RMC off
"$PSRF103,5,0,1,1*20\r\n" // VTG @ 1Hz
"$PSRF103,6,0,0,1*22\r\n" // MSS off
"$PSRF103,8,0,0,1*2C\r\n" // ZDA off
"$PSRF151,1*3F\r\n" // WAAS on (not always supported)
"$PSRF106,21*0F\r\n" // datum = WGS84
"";
// MediaTek init messages //////////////////////////////////////////////////////
//
@ -59,11 +59,11 @@ const prog_char AP_GPS_NMEA::_SiRF_init_string[] PROGMEM =
// MediaTek-based GPS.
//
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
"$PMTK330,0*2E\r\n" // datum = WGS84
"$PMTK313,1*2E\r\n" // SBAS on
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
"";
"$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
"$PMTK313,1*2E\r\n" // SBAS on
"$PMTK301,2*2E\r\n" // use SBAS data for DGPS
"";
// ublox init messages /////////////////////////////////////////////////////////
//
@ -75,10 +75,10 @@ const prog_char AP_GPS_NMEA::_MTK_init_string[] PROGMEM =
// and we don't know the baudrate.
//
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,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,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,rmc,0,0,0,0,0,0*67\r\n" // RMC off (XXX suppress other message types?)
"";
// NMEA message identifiers ////////////////////////////////////////////////////
//
@ -92,83 +92,83 @@ const char AP_GPS_NMEA::_gpvtg_string[] PROGMEM = "GPVTG";
// Constructors ////////////////////////////////////////////////////////////////
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
// 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...
//
fs->begin(0, 200, 16);
// 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.
// Leave the port speed alone as we don't actually know at what rate we're running...
//
fs->begin(0, 200, 16);
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_GPS_NMEA::init(void)
{
BetterStream *bs = (BetterStream *)_port;
BetterStream *bs = (BetterStream *)_port;
// send the SiRF init strings
bs->print_P((const prog_char_t *)_SiRF_init_string);
// send the SiRF init strings
bs->print_P((const prog_char_t *)_SiRF_init_string);
// send the MediaTek init strings
bs->print_P((const prog_char_t *)_MTK_init_string);
// send the MediaTek init strings
bs->print_P((const prog_char_t *)_MTK_init_string);
// send the ublox init strings
bs->print_P((const prog_char_t *)_ublox_init_string);
idleTimeout = 1200;
// send the ublox init strings
bs->print_P((const prog_char_t *)_ublox_init_string);
idleTimeout = 1200;
}
bool AP_GPS_NMEA::read(void)
{
int numc;
bool parsed = false;
int numc;
bool parsed = false;
numc = _port->available();
while (numc--) {
if (_decode(_port->read())) {
parsed = true;
}
}
return parsed;
numc = _port->available();
while (numc--) {
if (_decode(_port->read())) {
parsed = true;
}
}
return parsed;
}
bool AP_GPS_NMEA::_decode(char c)
{
bool valid_sentence = false;
bool valid_sentence = false;
switch (c) {
case ',': // term terminators
_parity ^= c;
case '\r':
case '\n':
case '*':
if (_term_offset < sizeof(_term)) {
_term[_term_offset] = 0;
valid_sentence = _term_complete();
}
++_term_number;
_term_offset = 0;
_is_checksum_term = c == '*';
return valid_sentence;
switch (c) {
case ',': // term terminators
_parity ^= c;
case '\r':
case '\n':
case '*':
if (_term_offset < sizeof(_term)) {
_term[_term_offset] = 0;
valid_sentence = _term_complete();
}
++_term_number;
_term_offset = 0;
_is_checksum_term = c == '*';
return valid_sentence;
case '$': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
return valid_sentence;
}
case '$': // sentence begin
_term_number = _term_offset = 0;
_parity = 0;
_sentence_type = _GPS_SENTENCE_OTHER;
_is_checksum_term = false;
_gps_data_good = false;
return valid_sentence;
}
// ordinary characters
if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c;
if (!_is_checksum_term)
_parity ^= c;
// ordinary characters
if (_term_offset < sizeof(_term) - 1)
_term[_term_offset++] = c;
if (!_is_checksum_term)
_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)
{
if (a >= 'A' && a <= 'F')
return a - 'A' + 10;
else if (a >= 'a' && a <= 'f')
return a - 'a' + 10;
else
return a - '0';
if (a >= 'A' && a <= 'F')
return a - 'A' + 10;
else if (a >= 'a' && a <= 'f')
return a - 'a' + 10;
else
return a - '0';
}
unsigned long AP_GPS_NMEA::_parse_decimal()
{
char *p = _term;
unsigned long ret = 100UL * atol(p);
while (isdigit(*p))
++p;
if (*p == '.') {
if (isdigit(p[1])) {
ret += 10 * (p[1] - '0');
if (isdigit(p[2]))
ret += p[2] - '0';
}
}
return ret;
char *p = _term;
unsigned long ret = 100UL * atol(p);
while (isdigit(*p))
++p;
if (*p == '.') {
if (isdigit(p[1])) {
ret += 10 * (p[1] - '0');
if (isdigit(p[2]))
ret += p[2] - '0';
}
}
return ret;
}
unsigned long AP_GPS_NMEA::_parse_degrees()
{
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
char *p, *q;
uint8_t deg = 0, min = 0;
unsigned int frac_min = 0;
// scan for decimal point or end of field
for (p = _term; isdigit(*p); p++)
;
q = _term;
// scan for decimal point or end of field
for (p = _term; isdigit(*p); p++)
;
q = _term;
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert degrees
while ((p - q) > 2) {
if (deg)
deg *= 10;
deg += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert minutes
while (p > q) {
if (min)
min *= 10;
min += DIGIT_TO_VAL(*q++);
}
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (int i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 100000UL + (min * 10000UL + frac_min) / 6;
// convert fractional minutes
// expect up to four digits, result is in
// ten-thousandths of a minute
if (*p == '.') {
q = p + 1;
for (int i = 0; i < 4; i++) {
frac_min *= 10;
if (isdigit(*q))
frac_min += *q++ - '0';
}
}
return deg * 100000UL + (min * 10000UL + frac_min) / 6;
}
// Processes a just-completed term
// Returns true if new sentence has just passed checksum test and is validated
bool AP_GPS_NMEA::_term_complete()
{
// handle the last term in a message
if (_is_checksum_term) {
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) {
if (_gps_data_good) {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
time = _new_time;
date = _new_date;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
ground_speed = _new_speed;
ground_course = _new_course;
fix = true;
break;
case _GPS_SENTENCE_GPGGA:
altitude = _new_altitude;
time = _new_time;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
num_sats = _new_satellite_count;
hdop = _new_hdop;
fix = true;
break;
case _GPS_SENTENCE_GPVTG:
ground_speed = _new_speed;
ground_course = _new_course;
// VTG has no fix indicator, can't change fix status
break;
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about
// fix status.
fix = false;
}
}
// we got a good message
return true;
}
// we got a bad message, ignore it
return false;
}
// handle the last term in a message
if (_is_checksum_term) {
uint8_t checksum = 16 * _from_hex(_term[0]) + _from_hex(_term[1]);
if (checksum == _parity) {
if (_gps_data_good) {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
time = _new_time;
date = _new_date;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
ground_speed = _new_speed;
ground_course = _new_course;
fix = true;
break;
case _GPS_SENTENCE_GPGGA:
altitude = _new_altitude;
time = _new_time;
latitude = _new_latitude * 100; // degrees*10e5 -> 10e7
longitude = _new_longitude * 100; // degrees*10e5 -> 10e7
num_sats = _new_satellite_count;
hdop = _new_hdop;
fix = true;
break;
case _GPS_SENTENCE_GPVTG:
ground_speed = _new_speed;
ground_course = _new_course;
// VTG has no fix indicator, can't change fix status
break;
}
} else {
switch (_sentence_type) {
case _GPS_SENTENCE_GPRMC:
case _GPS_SENTENCE_GPGGA:
// Only these sentences give us information about
// fix status.
fix = false;
}
}
// we got a good message
return true;
}
// we got a bad message, ignore it
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
if (!strcmp_P(_term, _gprmc_string)) {
_sentence_type = _GPS_SENTENCE_GPRMC;
} else if (!strcmp_P(_term, _gpgga_string)) {
_sentence_type = _GPS_SENTENCE_GPGGA;
} else if (!strcmp_P(_term, _gpvtg_string)) {
_sentence_type = _GPS_SENTENCE_GPVTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// the first term determines the sentence type
if (_term_number == 0) {
if (!strcmp_P(_term, _gprmc_string)) {
_sentence_type = _GPS_SENTENCE_GPRMC;
} else if (!strcmp_P(_term, _gpgga_string)) {
_sentence_type = _GPS_SENTENCE_GPGGA;
} else if (!strcmp_P(_term, _gpvtg_string)) {
_sentence_type = _GPS_SENTENCE_GPVTG;
// VTG may not contain a data qualifier, presume the solution is good
// unless it tells us otherwise.
_gps_data_good = true;
} else {
_sentence_type = _GPS_SENTENCE_OTHER;
}
return false;
}
// 32 = RMC, 64 = GGA, 96 = VTG
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// operational status
//
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0';
break;
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
_new_hdop = _parse_decimal();
break;
// 32 = RMC, 64 = GGA, 96 = VTG
if (_sentence_type != _GPS_SENTENCE_OTHER && _term[0]) {
switch (_sentence_type + _term_number) {
// operational status
//
case _GPS_SENTENCE_GPRMC + 2: // validity (RMC)
_gps_data_good = _term[0] == 'A';
break;
case _GPS_SENTENCE_GPGGA + 6: // Fix data (GGA)
_gps_data_good = _term[0] > '0';
break;
case _GPS_SENTENCE_GPVTG + 9: // validity (VTG) (we may not see this field)
_gps_data_good = _term[0] != 'N';
break;
case _GPS_SENTENCE_GPGGA + 7: // satellite count (GGA)
_new_satellite_count = atol(_term);
break;
case _GPS_SENTENCE_GPGGA + 8: // HDOP (GGA)
_new_hdop = _parse_decimal();
break;
// time and date
//
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
_new_time = _parse_decimal();
break;
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// time and date
//
case _GPS_SENTENCE_GPRMC + 1: // Time (RMC)
case _GPS_SENTENCE_GPGGA + 1: // Time (GGA)
_new_time = _parse_decimal();
break;
case _GPS_SENTENCE_GPRMC + 9: // Date (GPRMC)
_new_date = atol(_term);
break;
// location
//
case _GPS_SENTENCE_GPRMC + 3: // Latitude
case _GPS_SENTENCE_GPGGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 4: // N/S
case _GPS_SENTENCE_GPGGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_GPRMC + 5: // Longitude
case _GPS_SENTENCE_GPGGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 6: // E/W
case _GPS_SENTENCE_GPGGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal();
break;
// location
//
case _GPS_SENTENCE_GPRMC + 3: // Latitude
case _GPS_SENTENCE_GPGGA + 2:
_new_latitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 4: // N/S
case _GPS_SENTENCE_GPGGA + 3:
if (_term[0] == 'S')
_new_latitude = -_new_latitude;
break;
case _GPS_SENTENCE_GPRMC + 5: // Longitude
case _GPS_SENTENCE_GPGGA + 4:
_new_longitude = _parse_degrees();
break;
case _GPS_SENTENCE_GPRMC + 6: // E/W
case _GPS_SENTENCE_GPGGA + 5:
if (_term[0] == 'W')
_new_longitude = -_new_longitude;
break;
case _GPS_SENTENCE_GPGGA + 9: // Altitude (GPGGA)
_new_altitude = _parse_decimal();
break;
// course and speed
//
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break;
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
_new_course = _parse_decimal();
break;
}
}
// course and speed
//
case _GPS_SENTENCE_GPRMC + 7: // Speed (GPRMC)
case _GPS_SENTENCE_GPVTG + 5: // Speed (VTG)
_new_speed = (_parse_decimal() * 514) / 1000; // knots-> m/sec, approximiates * 0.514
break;
case _GPS_SENTENCE_GPRMC + 8: // Course (GPRMC)
case _GPS_SENTENCE_GPVTG + 1: // Course (VTG)
_new_course = _parse_decimal();
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
{
public:
/// Constructor
///
AP_GPS_NMEA(Stream *s);
/// Constructor
///
AP_GPS_NMEA(Stream *s);
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
virtual void init();
/// Perform a (re)initialisation of the GPS; sends the
/// protocol configuration messages.
///
virtual void init();
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state
/// accordingly.
///
virtual bool read();
/// Checks the serial receive buffer for characters,
/// attempts to parse NMEA data and updates internal state
/// accordingly.
///
virtual bool read();
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.
_GPS_SENTENCE_GPRMC = 32,
_GPS_SENTENCE_GPGGA = 64,
_GPS_SENTENCE_GPVTG = 96,
_GPS_SENTENCE_OTHER = 0
_GPS_SENTENCE_GPRMC = 32,
_GPS_SENTENCE_GPGGA = 64,
_GPS_SENTENCE_GPVTG = 96,
_GPS_SENTENCE_OTHER = 0
};
/// Update the decode state machine with a new character
///
/// @param c The next character in the NMEA input stream
/// @returns True if processing the character has resulted in
/// an update to the GPS state
///
bool _decode(char c);
/// Update the decode state machine with a new character
///
/// @param c The next character in the NMEA input stream
/// @returns True if processing the character has resulted in
/// an update to the GPS state
///
bool _decode(char c);
/// Return the numeric value of an ascii hex character
///
/// @param a The character to be converted
/// @returns The value of the character as a hex digit
///
int _from_hex(char a);
/// Return the numeric value of an ascii hex character
///
/// @param a The character to be converted
/// @returns The value of the character as a hex digit
///
int _from_hex(char a);
/// Parses the current term as a NMEA-style decimal number with
/// up to two decimal digits.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 100.
///
unsigned long _parse_decimal();
/// Parses the current term as a NMEA-style decimal number with
/// up to two decimal digits.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 100.
///
unsigned long _parse_decimal();
/// Parses the current term as a NMEA-style degrees + minutes
/// value with up to four decimal digits.
///
/// This gives a theoretical resolution limit of around 18cm.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 10000.
///
unsigned long _parse_degrees();
/// Parses the current term as a NMEA-style degrees + minutes
/// value with up to four decimal digits.
///
/// This gives a theoretical resolution limit of around 18cm.
///
/// @returns The value expressed by the string in _term,
/// multiplied by 10000.
///
unsigned long _parse_degrees();
/// Processes the current term when it has been deemed to be
/// complete.
///
/// Each GPS message is broken up into terms separated by commas.
/// Each term is then processed by this function as it is received.
///
/// @returns True if completing the term has resulted in
/// an update to the GPS state.
bool _term_complete();
/// Processes the current term when it has been deemed to be
/// complete.
///
/// Each GPS message is broken up into terms separated by commas.
/// Each term is then processed by this function as it is received.
///
/// @returns True if completing the term has resulted in
/// an update to the GPS state.
bool _term_complete();
uint8_t _parity; ///< NMEA message checksum accumulator
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
{
public:
AP_GPS_None(Stream *s) : GPS(s) {}
virtual void init(void) {};
virtual bool read(void) { return false; };
AP_GPS_None(Stream *s) : GPS(s) {}
virtual void init(void) {};
virtual bool read(void) {
return false;
};
};
#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.
//
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, 0x00, 0x29, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0xd0, 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
};
// Constructors ////////////////////////////////////////////////////////////////
@ -29,18 +29,18 @@ AP_GPS_SIRF::AP_GPS_SIRF(Stream *s) : GPS(s)
}
// Public Methods //////////////////////////////////////////////////////////////
void
void
AP_GPS_SIRF::init(void)
{
_port->flush();
{
_port->flush();
// For modules that default to something other than SiRF binary,
// the module-specific subclass should take care of switching to binary mode
// before calling us.
// For modules that default to something other than SiRF binary,
// the module-specific subclass should take care of switching to binary mode
// before calling us.
// send SiRF binary setup messages
_port->write(init_messages, sizeof(init_messages));
idleTimeout = 1200;
// send SiRF binary setup messages
_port->write(init_messages, sizeof(init_messages));
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -55,143 +55,143 @@ AP_GPS_SIRF::init(void)
bool
AP_GPS_SIRF::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
while(numc--) {
numc = _port->available();
while(numc--) {
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
switch(_step){
switch(_step) {
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message length
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_payload_length = (uint16_t)data << 8;
break;
case 3:
_step++;
_payload_length |= data;
_payload_counter = 0;
_checksum = 0;
break;
// Message length
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
_payload_length = (uint16_t)data << 8;
break;
case 3:
_step++;
_payload_length |= data;
_payload_counter = 0;
_checksum = 0;
break;
// Message header processing
//
// We sniff the message ID to determine whether we are going
// to gather the message bytes or just discard them.
//
case 4:
_step++;
_accumulate(data);
_payload_length--;
_gather = false;
switch(data) {
case MSG_GEONAV:
if (_payload_length == sizeof(sirf_geonav)) {
_gather = true;
_msg_id = data;
}
break;
}
break;
// Message header processing
//
// We sniff the message ID to determine whether we are going
// to gather the message bytes or just discard them.
//
case 4:
_step++;
_accumulate(data);
_payload_length--;
_gather = false;
switch(data) {
case MSG_GEONAV:
if (_payload_length == sizeof(sirf_geonav)) {
_gather = true;
_msg_id = data;
}
break;
}
break;
// Receive message data
//
// Note that we are effectively guaranteed by the protocol
// that the checksum and postamble cannot be mistaken for
// the preamble, so if we are discarding bytes in this
// message when the payload is done we return directly
// to the preamble detector rather than bothering with
// the checksum logic.
//
case 5:
if (_gather) { // gather data if requested
_accumulate(data);
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
} else {
if (++_payload_counter == _payload_length)
_step = 0;
}
break;
// Receive message data
//
// Note that we are effectively guaranteed by the protocol
// that the checksum and postamble cannot be mistaken for
// the preamble, so if we are discarding bytes in this
// message when the payload is done we return directly
// to the preamble detector rather than bothering with
// the checksum logic.
//
case 5:
if (_gather) { // gather data if requested
_accumulate(data);
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
} else {
if (++_payload_counter == _payload_length)
_step = 0;
}
break;
// Checksum and message processing
//
case 6:
_step++;
if ((_checksum >> 8) != data) {
_error("GPS_SIRF: checksum error\n");
_step = 0;
}
break;
case 7:
_step = 0;
if ((_checksum & 0xff) != data) {
_error("GPS_SIRF: checksum error\n");
break;
}
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return(parsed);
// Checksum and message processing
//
case 6:
_step++;
if ((_checksum >> 8) != data) {
_error("GPS_SIRF: checksum error\n");
_step = 0;
}
break;
case 7:
_step = 0;
if ((_checksum & 0xff) != data) {
_error("GPS_SIRF: checksum error\n");
break;
}
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return(parsed);
}
bool
AP_GPS_SIRF::_parse_gps(void)
{
switch(_msg_id) {
case MSG_GEONAV:
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);
latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude);
altitude = _swapl(&_buffer.nav.altitude_msl);
ground_speed = _swapi(&_buffer.nav.ground_speed);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
if (ground_speed > 50)
ground_course = _swapi(&_buffer.nav.ground_course);
num_sats = _buffer.nav.satellites;
switch(_msg_id) {
case MSG_GEONAV:
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);
latitude = _swapl(&_buffer.nav.latitude);
longitude = _swapl(&_buffer.nav.longitude);
altitude = _swapl(&_buffer.nav.altitude_msl);
ground_speed = _swapi(&_buffer.nav.ground_speed);
// at low speeds, ground course wanders wildly; suppress changes if we are not moving
if (ground_speed > 50)
ground_course = _swapi(&_buffer.nav.ground_course);
num_sats = _buffer.nav.satellites;
return true;
}
return false;
return true;
}
return false;
}
void
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 {
public:
AP_GPS_SIRF(Stream *s);
AP_GPS_SIRF(Stream *s);
virtual void init();
virtual bool read();
virtual void init();
virtual bool read();
private:
// XXX this is being ignored by the compiler #pragma pack(1)
struct sirf_geonav {
uint16_t fix_invalid;
uint16_t fix_type;
uint16_t week;
uint32_t time;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint16_t second;
uint32_t satellites_used;
int32_t latitude;
int32_t longitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
int8_t map_datum;
int16_t ground_speed;
int16_t ground_course;
int16_t res1;
int16_t climb_rate;
uint16_t heading_rate;
uint32_t horizontal_position_error;
uint32_t vertical_position_error;
uint32_t time_error;
int16_t horizontal_velocity_error;
int32_t clock_bias;
uint32_t clock_bias_error;
int32_t clock_drift;
uint32_t clock_drift_error;
uint32_t distance;
uint16_t distance_error;
uint16_t heading_error;
uint8_t satellites;
uint8_t hdop;
uint8_t mode_info;
};
struct sirf_geonav {
uint16_t fix_invalid;
uint16_t fix_type;
uint16_t week;
uint32_t time;
uint16_t year;
uint8_t month;
uint8_t day;
uint8_t hour;
uint8_t minute;
uint16_t second;
uint32_t satellites_used;
int32_t latitude;
int32_t longitude;
int32_t altitude_ellipsoid;
int32_t altitude_msl;
int8_t map_datum;
int16_t ground_speed;
int16_t ground_course;
int16_t res1;
int16_t climb_rate;
uint16_t heading_rate;
uint32_t horizontal_position_error;
uint32_t vertical_position_error;
uint32_t time_error;
int16_t horizontal_velocity_error;
int32_t clock_bias;
uint32_t clock_bias_error;
int32_t clock_drift;
uint32_t clock_drift_error;
uint32_t distance;
uint16_t distance_error;
uint16_t heading_error;
uint8_t satellites;
uint8_t hdop;
uint8_t mode_info;
};
// #pragma pack(pop)
enum sirf_protocol_bytes {
PREAMBLE1 = 0xa0,
PREAMBLE2 = 0xa2,
POSTAMBLE1 = 0xb0,
POSTAMBLE2 = 0xb3,
MSG_GEONAV = 0x29
};
enum sirf_fix_type {
FIX_3D = 0x6,
FIX_MASK = 0x7
};
enum sirf_protocol_bytes {
PREAMBLE1 = 0xa0,
PREAMBLE2 = 0xa2,
POSTAMBLE1 = 0xb0,
POSTAMBLE2 = 0xb3,
MSG_GEONAV = 0x29
};
enum sirf_fix_type {
FIX_3D = 0x6,
FIX_MASK = 0x7
};
// State machine state
uint8_t _step;
uint16_t _checksum;
bool _gather;
uint16_t _payload_length;
uint16_t _payload_counter;
uint8_t _msg_id;
// State machine state
uint8_t _step;
uint16_t _checksum;
bool _gather;
uint16_t _payload_length;
uint16_t _payload_counter;
uint8_t _msg_id;
// Message buffer
union {
sirf_geonav nav;
uint8_t bytes[];
} _buffer;
// Message buffer
union {
sirf_geonav nav;
uint8_t bytes[];
} _buffer;
bool _parse_gps(void);
void _accumulate(uint8_t val);
bool _parse_gps(void);
void _accumulate(uint8_t val);
};
#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
{
public:
AP_GPS_Shim() : GPS(NULL) {}
AP_GPS_Shim() : GPS(NULL) {}
virtual void init(void) {};
virtual bool read(void) {
bool updated = _updated;
_updated = false;
return updated;
}
virtual void init(void) {};
virtual bool read(void) {
bool updated = _updated;
_updated = false;
return updated;
}
/// Set-and-mark-updated macro for the public member variables; each instance
/// defines a member function set_<variable>(<type>)
///
/// Set-and-mark-updated macro for the public member variables; each instance
/// defines a member function set_<variable>(<type>)
///
#define __GPS_SHIM_SET(__type, __name) void set_##__name(__type v) { __name = v; _updated = true; }
__GPS_SHIM_SET(long, time);
__GPS_SHIM_SET(long, latitude);
__GPS_SHIM_SET(long, longitude);
__GPS_SHIM_SET(long, altitude);
__GPS_SHIM_SET(long, ground_speed);
__GPS_SHIM_SET(long, ground_course);
__GPS_SHIM_SET(long, speed_3d);
__GPS_SHIM_SET(int, hdop);
__GPS_SHIM_SET(long, time);
__GPS_SHIM_SET(long, latitude);
__GPS_SHIM_SET(long, longitude);
__GPS_SHIM_SET(long, altitude);
__GPS_SHIM_SET(long, ground_speed);
__GPS_SHIM_SET(long, ground_course);
__GPS_SHIM_SET(long, speed_3d);
__GPS_SHIM_SET(int, hdop);
#undef __GPS_SHIM_SET
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

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
AP_GPS_UBLOX::init(void)
{
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.
// XXX it might make sense to send some CFG_MSG,CFG_NMEA messages to get the
// right reporting configuration.
_port->flush();
_epoch = TIME_OF_WEEK;
idleTimeout = 1200;
_port->flush();
_epoch = TIME_OF_WEEK;
idleTimeout = 1200;
}
// Process bytes available from the stream
@ -44,122 +44,122 @@ AP_GPS_UBLOX::init(void)
bool
AP_GPS_UBLOX::read(void)
{
uint8_t data;
int numc;
bool parsed = false;
uint8_t data;
int numc;
bool parsed = false;
numc = _port->available();
for (int i = 0; i < numc; i++){ // Process bytes received
numc = _port->available();
for (int i = 0; i < numc; i++) { // Process bytes received
// read the next byte
data = _port->read();
// read the next byte
data = _port->read();
switch(_step){
switch(_step) {
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message preamble detection
//
// If we fail to match any of the expected bytes, we reset
// the state machine and re-consider the failed byte as
// the first byte of the preamble. This improves our
// chances of recovering from a mismatch and makes it less
// likely that we will be fooled by the preamble appearing
// as data in some other message.
//
case 1:
if (PREAMBLE2 == data) {
_step++;
break;
}
_step = 0;
// FALLTHROUGH
case 0:
if(PREAMBLE1 == data)
_step++;
break;
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
if (CLASS_NAV == data) {
_gather = true; // class is interesting, maybe gather
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_gather = false; // class is not interesting, discard
}
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
if (_gather) { // if class was interesting
switch(data) {
case MSG_POSLLH: // message is interesting
_expect = sizeof(ubx_nav_posllh);
break;
case MSG_STATUS:
_expect = sizeof(ubx_nav_status);
break;
case MSG_SOL:
_expect = sizeof(ubx_nav_solution);
break;
case MSG_VELNED:
_expect = sizeof(ubx_nav_velned);
break;
default:
_gather = false; // message is not interesting
}
}
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)data; // payload length high byte
_payload_counter = 0; // prepare to receive payload
if (_payload_length != _expect)
_gather = false;
break;
// Message header processing
//
// We sniff the class and message ID to decide whether we
// are going to gather the message bytes or just discard
// them.
//
// We always collect the length so that we can avoid being
// fooled by preamble bytes in messages.
//
case 2:
_step++;
if (CLASS_NAV == data) {
_gather = true; // class is interesting, maybe gather
_ck_b = _ck_a = data; // reset the checksum accumulators
} else {
_gather = false; // class is not interesting, discard
}
break;
case 3:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_msg_id = data;
if (_gather) { // if class was interesting
switch(data) {
case MSG_POSLLH: // message is interesting
_expect = sizeof(ubx_nav_posllh);
break;
case MSG_STATUS:
_expect = sizeof(ubx_nav_status);
break;
case MSG_SOL:
_expect = sizeof(ubx_nav_solution);
break;
case MSG_VELNED:
_expect = sizeof(ubx_nav_velned);
break;
default:
_gather = false; // message is not interesting
}
}
break;
case 4:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length = data; // payload length low byte
break;
case 5:
_step++;
_ck_b += (_ck_a += data); // checksum byte
_payload_length += (uint16_t)data; // payload length high byte
_payload_counter = 0; // prepare to receive payload
if (_payload_length != _expect)
_gather = false;
break;
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_gather) // gather data if requested
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
break;
// Receive message data
//
case 6:
_ck_b += (_ck_a += data); // checksum byte
if (_gather) // gather data if requested
_buffer.bytes[_payload_counter] = data;
if (++_payload_counter == _payload_length)
_step++;
break;
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data)
_step = 0; // bad checksum
break;
case 8:
_step = 0;
if (_ck_b != data)
break; // bad checksum
// Checksum and message processing
//
case 7:
_step++;
if (_ck_a != data)
_step = 0; // bad checksum
break;
case 8:
_step = 0;
if (_ck_b != data)
break; // bad checksum
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return parsed;
if (_gather) {
parsed = _parse_gps(); // Parse the new GPS packet
}
}
}
return parsed;
}
// Private Methods /////////////////////////////////////////////////////////////
@ -167,28 +167,28 @@ AP_GPS_UBLOX::read(void)
bool
AP_GPS_UBLOX::_parse_gps(void)
{
switch (_msg_id) {
case MSG_POSLLH:
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
break;
case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break;
case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
default:
return false;
}
return true;
switch (_msg_id) {
case MSG_POSLLH:
time = _buffer.posllh.time;
longitude = _buffer.posllh.longitude;
latitude = _buffer.posllh.latitude;
altitude = _buffer.posllh.altitude_msl / 10;
break;
case MSG_STATUS:
fix = (_buffer.status.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.status.fix_type == FIX_3D);
break;
case MSG_SOL:
fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D);
num_sats = _buffer.solution.satellites;
hdop = _buffer.solution.position_DOP;
break;
case MSG_VELNED:
speed_3d = _buffer.velned.speed_3d; // cm/s
ground_speed = _buffer.velned.speed_2d; // cm/s
ground_course = _buffer.velned.heading_2d / 1000; // Heading 2D deg * 100000 rescaled to deg * 100
break;
default:
return false;
}
return true;
}

184
libraries/AP_GPS/AP_GPS_UBLOX.h
View File

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

42
libraries/AP_GPS/GPS.cpp
View File

@ -6,30 +6,30 @@
void
GPS::update(void)
{
bool result;
bool result;
// call the GPS driver to process incoming data
result = read();
// call the GPS driver to process incoming data
result = read();
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((millis() - _idleTimer) > idleTimeout) {
_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;
// if we did not get a message, and the idle timer has expired, re-init
if (!result) {
if ((millis() - _idleTimer) > idleTimeout) {
_status = NO_GPS;
valid_read = true;
new_data = true;
init();
// 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
_idleTimer = millis();
}
valid_read = true;
new_data = true;
// reset the idle timer
_idleTimer = millis();
}
}
void
@ -42,5 +42,5 @@ GPS::setHIL(long _time, float _latitude, float _longitude, float _altitude,
void
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:
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
/// GPS drivers should not override this function; they should implement
/// ::read instead.
///
void update(void);
/// Update GPS state based on possible bytes received from the module.
///
/// This routine must be called periodically to process incoming data.
///
/// GPS drivers should not override this function; they should implement
/// ::read instead.
///
void update(void);
void (*callback)(unsigned long t);
void (*callback)(unsigned long t);
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons
///
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK = 2 ///< Receiving valid messages and locked
};
/// GPS status codes
///
/// \note Non-intuitive ordering for legacy reasons
///
enum GPS_Status {
NO_GPS = 0, ///< No GPS connected/detected
NO_FIX = 1, ///< Receiving valid GPS messages but no lock
GPS_OK = 2 ///< Receiving valid messages and locked
};
/// Query GPS status
///
/// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms.
///
/// @returns Current GPS status
///
GPS_Status status(void) { return _status; }
/// Query GPS status
///
/// The 'valid message' status indicates that a recognised message was
/// received from the GPS within the last 500ms.
///
/// @returns Current GPS status
///
GPS_Status status(void) {
return _status;
}
/// GPS time epoch codes
///
enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available
}; ///< SIFR?
/// GPS time epoch codes
///
enum GPS_Time_Epoch {
TIME_OF_DAY = 0, ///<
TIME_OF_WEEK = 1, ///< Ublox
TIME_OF_YEAR = 2, ///< MTK, NMEA
UNIX_EPOCH = 3 ///< If available
}; ///< SIFR?
/// Query GPS time epoch
///
/// @returns Current GPS time epoch code
///
GPS_Time_Epoch epoch(void) { return _epoch; }
/// Query GPS time epoch
///
/// @returns Current GPS time epoch code
///
GPS_Time_Epoch epoch(void) {
return _epoch;
}
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
/// Must be implemented by the GPS driver.
///
virtual void init(void) = 0;
/// Startup initialisation.
///
/// This routine performs any one-off initialisation required to set the
/// GPS up for use.
///
/// Must be implemented by the GPS driver.
///
virtual void init(void) = 0;
// Properties
long time; ///< GPS time (milliseconds from epoch)
long date; ///< GPS date (FORMAT TBD)
long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm
long ground_speed; ///< ground speed in cm/sec
long ground_course; ///< ground course in 100ths of a degree
long speed_3d; ///< 3D speed in cm/sec (not always available)
int hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
// Properties
long time; ///< GPS time (milliseconds from epoch)
long date; ///< GPS date (FORMAT TBD)
long latitude; ///< latitude in degrees * 10,000,000
long longitude; ///< longitude in degrees * 10,000,000
long altitude; ///< altitude in cm
long ground_speed; ///< ground speed in cm/sec
long ground_course; ///< ground course in 100ths of a degree
long speed_3d; ///< 3D speed in cm/sec (not always available)
int hdop; ///< horizontal dilution of precision in cm
uint8_t num_sats; ///< Number of visible satelites
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data;
/// Set to true when new data arrives. A client may set this
/// to false in order to avoid processing data they have
/// already seen.
bool new_data;
// Deprecated properties
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)
// Deprecated properties
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)
// Debug support
bool print_errors; ///< deprecated
// Debug support
bool print_errors; ///< deprecated
// HIL support
virtual void setHIL(long time, float latitude, float longitude, float altitude,
float ground_speed, float ground_course, float speed_3d, uint8_t num_sats);
// HIL support
virtual void setHIL(long time, float latitude, float longitude, float altitude,
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
/// sending us updates and attempt a re-init.
///
/// 1200ms allows a small amount of slack over the worst-case 1Hz update
/// rate.
///
unsigned long idleTimeout;
/// Time in milliseconds after which we will assume the GPS is no longer
/// sending us updates and attempt a re-init.
///
/// 1200ms allows a small amount of slack over the worst-case 1Hz update
/// rate.
///
unsigned long idleTimeout;
protected:
Stream *_port; ///< port the GPS is attached to
Stream *_port; ///< port the GPS is attached to
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param s Stream connected to the GPS module.
///
GPS(Stream *s) : _port(s) {};
/// Constructor
///
/// @note The stream is expected to be set up and configured for the
/// correct bitrate before ::init is called.
///
/// @param s Stream connected to the GPS module.
///
GPS(Stream *s) : _port(s) {};
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.
///
/// @returns true if a valid message was received from the GPS
///
virtual bool read(void) = 0;
/// read from the GPS stream and update properties
///
/// Must be implemented by the GPS driver.
///
/// @returns true if a valid message was received from the GPS
///
virtual bool read(void) = 0;
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const void *bytes);
/// perform an endian swap on a long
///
/// @param bytes pointer to a buffer containing bytes representing a
/// long in the wrong byte order
/// @returns endian-swapped value
///
long _swapl(const void *bytes);
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int16_t _swapi(const void *bytes);
/// perform an endian swap on an int
///
/// @param bytes pointer to a buffer containing bytes representing an
/// int in the wrong byte order
/// @returns endian-swapped value
int16_t _swapi(const void *bytes);
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
/// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits
///
void _error(const char *msg);
/// emit an error message
///
/// based on the value of print_errors, emits the printf-formatted message
/// in msg,... to stderr
///
/// @param fmt printf-like format string
///
/// @note deprecated as-is due to the difficulty of hooking up to a working
/// printf vs. the potential benefits
///
void _error(const char *msg);
/// Time epoch code for the gps in use
GPS_Time_Epoch _epoch;
/// Time epoch code for the gps in use
GPS_Time_Epoch _epoch;
private:
/// Last time that the GPS driver got a good packet from the GPS
///
unsigned long _idleTimer;
/// Last time that the GPS driver got a good packet from the GPS
///
unsigned long _idleTimer;
/// Our current status
GPS_Status _status;
/// Our current status
GPS_Status _status;
};
inline long
GPS::_swapl(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
long v;
uint8_t b[4];
} u;
const uint8_t *b = (const uint8_t *)bytes;
union {
long v;
uint8_t b[4];
} u;
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
u.b[0] = b[3];
u.b[1] = b[2];
u.b[2] = b[1];
u.b[3] = b[0];
return(u.v);
return(u.v);
}
inline int16_t
GPS::_swapi(const void *bytes)
{
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
const uint8_t *b = (const uint8_t *)bytes;
union {
int16_t v;
uint8_t b[2];
} u;
u.b[0] = b[1];
u.b[1] = b[0];
u.b[0] = b[1];
u.b[1] = b[0];
return(u.v);
return(u.v);
}
#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()
{
tone(A6, 1000, 200);
tone(A6, 1000, 200);
Serial.begin(38400, 16, 128);
Serial1.begin(57600, 128, 16);
stderr = stdout;
gps.print_errors = true;
Serial.begin(38400, 16, 128);
Serial1.begin(57600, 128, 16);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS 406 library test");
gps.init(); // GPS Initialization
delay(1000);
Serial.println("GPS 406 library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
delay(20);
gps.update();
if (gps.new_data) {
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
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()
{
Serial.begin(115200);
Serial1.begin(38400);
Serial.begin(115200);
Serial1.begin(38400);
Serial.println("GPS AUTO library test");
gps = &GPS;
gps->init();
Serial.println("GPS AUTO library test");
gps = &GPS;
gps->init();
}
void loop()
{
gps->update();
if (gps->new_data){
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
gps->new_data = false;
}
gps->update();
if (gps->new_data) {
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
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()
{
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization
delay(1000);
Serial.println("GPS MTK library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
delay(20);
gps.update();
if (gps.new_data) {
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
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
const uint8_t sirf_to_nmea[] = { 0xa0, 0xa2, // preamble
0x00, 0x18, // message length
0x81, 0x02, // switch to NMEA
0x01, 0x01, // GGA on with checksum
0x00, 0x01, // GLL off
0x00, 0x01, // GSA off
0x00, 0x01, // GSV off
0x01, 0x01, // RMC on with checksum
0x01, 0x01, // VTG on with checksum
0x00, 0x01, // MSS off
0x00, 0x01, // EPE off
0x00, 0x01, // ZPA off
0x00, 0x00, // pad
0x96, 0x00, // 38400
0x01, 0x25, // checksum TBD
0xb0, 0xb3 }; // postamble
0x00, 0x18, // message length
0x81, 0x02, // switch to NMEA
0x01, 0x01, // GGA on with checksum
0x00, 0x01, // GLL off
0x00, 0x01, // GSA off
0x00, 0x01, // GSV off
0x01, 0x01, // RMC on with checksum
0x01, 0x01, // VTG on with checksum
0x00, 0x01, // MSS off
0x00, 0x01, // EPE off
0x00, 0x01, // ZPA off
0x00, 0x00, // pad
0x96, 0x00, // 38400
0x01, 0x25, // checksum TBD
0xb0, 0xb3
}; // postamble
void setup()
{
Serial.begin(38400);
Serial1.begin(38400);
Serial.begin(38400);
Serial1.begin(38400);
// try to coerce a SiRF unit that's been traumatized by
// AP_GPS_AUTO back into NMEA mode so that we can test
// it.
for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++)
Serial1.write(sirf_to_nmea[i]);
// try to coerce a SiRF unit that's been traumatized by
// AP_GPS_AUTO back into NMEA mode so that we can test
// it.
for (uint8_t i = 0; i < sizeof(sirf_to_nmea); i++)
Serial1.write(sirf_to_nmea[i]);
Serial.println("GPS NMEA library test");
gps->init();
Serial.println("GPS NMEA library test");
gps->init();
}
void loop()
{
gps->update();
if (gps->new_data){
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
gps->new_data = false;
}
gps->update();
if (gps->new_data) {
if (gps->fix) {
Serial.printf("\nLat: %.7f Lon: %.7f Alt: %.2fm GSP: %.2fm/s CoG: %d SAT: %d TIM: %lu",
(float)gps->latitude / T7,
(float)gps->longitude / T7,
(float)gps->altitude / 100.0,
(float)gps->ground_speed / 100.0,
(int)gps->ground_course / 100,
gps->num_sats,
gps->time);
} else {
Serial.println("No fix");
}
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()
{
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.begin(38400);
Serial1.begin(38400);
stderr = stdout;
gps.print_errors = true;
Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization
delay(1000);
Serial.println("GPS UBLOX library test");
gps.init(); // GPS Initialization
delay(1000);
}
void loop()
{
delay(20);
gps.update();
if (gps.new_data){
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
delay(20);
gps.update();
if (gps.new_data) {
Serial.print("gps:");
Serial.print(" Lat:");
Serial.print((float)gps.latitude / T7, DEC);
Serial.print(" Lon:");
Serial.print((float)gps.longitude / T7, DEC);
Serial.print(" Alt:");
Serial.print((float)gps.altitude / 100.0, DEC);
Serial.print(" GSP:");
Serial.print(gps.ground_speed / 100.0);
Serial.print(" COG:");
Serial.print(gps.ground_course / 100.0, DEC);
Serial.print(" SAT:");
Serial.print(gps.num_sats, DEC);
Serial.print(" FIX:");
Serial.print(gps.fix, DEC);
Serial.print(" TIM:");
Serial.print(gps.time, DEC);
Serial.println();
gps.new_data = 0; // We have readed the data
}
}