From 57301ce6472819d05dbf48285e8782296bee4d98 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 28 Oct 2011 14:52:50 -0400 Subject: [PATCH] Cleaned up AP_GPS formatting. --- Tools/scripts/format.sh | 1 + libraries/AP_GPS/AP_GPS_406.cpp | 72 +-- libraries/AP_GPS/AP_GPS_406.h | 8 +- libraries/AP_GPS/AP_GPS_Auto.cpp | 288 +++++----- libraries/AP_GPS/AP_GPS_Auto.h | 54 +- libraries/AP_GPS/AP_GPS_HIL.cpp | 18 +- libraries/AP_GPS/AP_GPS_HIL.h | 10 +- libraries/AP_GPS/AP_GPS_IMU.cpp | 282 +++++----- libraries/AP_GPS/AP_GPS_IMU.h | 66 +-- libraries/AP_GPS/AP_GPS_MTK.cpp | 208 ++++---- libraries/AP_GPS/AP_GPS_MTK.h | 74 +-- libraries/AP_GPS/AP_GPS_MTK16.cpp | 220 ++++---- libraries/AP_GPS/AP_GPS_MTK16.h | 78 +-- libraries/AP_GPS/AP_GPS_NMEA.cpp | 498 +++++++++--------- libraries/AP_GPS/AP_GPS_NMEA.h | 110 ++-- libraries/AP_GPS/AP_GPS_None.h | 8 +- libraries/AP_GPS/AP_GPS_SIRF.cpp | 262 ++++----- libraries/AP_GPS/AP_GPS_SIRF.h | 130 ++--- libraries/AP_GPS/AP_GPS_Shim.h | 38 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 276 +++++----- libraries/AP_GPS/AP_GPS_UBLOX.h | 184 +++---- libraries/AP_GPS/GPS.cpp | 42 +- libraries/AP_GPS/GPS.h | 288 +++++----- .../examples/GPS_406_test/GPS_406_test.pde | 62 +-- .../examples/GPS_AUTO_test/GPS_AUTO_test.pde | 42 +- .../examples/GPS_MTK_test/GPS_MTK_test.pde | 60 +-- .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 81 +-- .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 60 +-- 28 files changed, 1764 insertions(+), 1756 deletions(-) diff --git a/Tools/scripts/format.sh b/Tools/scripts/format.sh index c907f5fdf5..cef831de59 100755 --- a/Tools/scripts/format.sh +++ b/Tools/scripts/format.sh @@ -10,3 +10,4 @@ format ArduRover format ArduBoat format libraries/APO format libraries/AP_Common +format libraries/AP_GPS diff --git a/libraries/AP_GPS/AP_GPS_406.cpp b/libraries/AP_GPS/AP_GPS_406.cpp index 5e9cb9f675..30ca72a62c 100644 --- a/libraries/AP_GPS/AP_GPS_406.cpp +++ b/libraries/AP_GPS/AP_GPS_406.cpp @@ -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); } diff --git a/libraries/AP_GPS/AP_GPS_406.h b/libraries/AP_GPS/AP_GPS_406.h index 6c34b1a6a3..daad1e6ae7 100644 --- a/libraries/AP_GPS/AP_GPS_406.h +++ b/libraries/AP_GPS/AP_GPS_406.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_Auto.cpp b/libraries/AP_GPS/AP_GPS_Auto.cpp index 6afc23cb23..b3a93b8a68 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.cpp +++ b/libraries/AP_GPS/AP_GPS_Auto.cpp @@ -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); } diff --git a/libraries/AP_GPS/AP_GPS_Auto.h b/libraries/AP_GPS/AP_GPS_Auto.h index 61be2de201..db38de96af 100644 --- a/libraries/AP_GPS/AP_GPS_Auto.h +++ b/libraries/AP_GPS/AP_GPS_Auto.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_HIL.cpp b/libraries/AP_GPS/AP_GPS_HIL.cpp index d55beca3f9..38db02ba05 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.cpp +++ b/libraries/AP_GPS/AP_GPS_HIL.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_HIL.h b/libraries/AP_GPS/AP_GPS_HIL.h index 9fa644efe8..fab213d042 100644 --- a/libraries/AP_GPS/AP_GPS_HIL.h +++ b/libraries/AP_GPS/AP_GPS_HIL.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp index 84cf24d045..71fc6f2e23 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.cpp +++ b/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -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 0) { + for (int i=0; iread(); - - 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) {}; diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h index 1a31c615f0..ed5ee20326 100644 --- a/libraries/AP_GPS/AP_GPS_IMU.h +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 6375f61ab6..a50088e43e 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index 8f37a635d3..8697f23a26 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_MTK16.cpp b/libraries/AP_GPS/AP_GPS_MTK16.cpp index bf7180f112..96cc189b9c 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK16.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_MTK16.h b/libraries/AP_GPS/AP_GPS_MTK16.h index 46a7bceb23..07d939c084 100644 --- a/libraries/AP_GPS/AP_GPS_MTK16.h +++ b/libraries/AP_GPS/AP_GPS_MTK16.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 78e01fe8ab..473601b9d6 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index e62483b58f..546480e97e 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_None.h b/libraries/AP_GPS/AP_GPS_None.h index 6b93cd83e8..17202f5180 100644 --- a/libraries/AP_GPS/AP_GPS_None.h +++ b/libraries/AP_GPS/AP_GPS_None.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_SIRF.cpp b/libraries/AP_GPS/AP_GPS_SIRF.cpp index 4b247153ca..e08b5d0eda 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.cpp +++ b/libraries/AP_GPS/AP_GPS_SIRF.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_SIRF.h b/libraries/AP_GPS/AP_GPS_SIRF.h index 483d7cefca..702aea9610 100644 --- a/libraries/AP_GPS/AP_GPS_SIRF.h +++ b/libraries/AP_GPS/AP_GPS_SIRF.h @@ -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 diff --git a/libraries/AP_GPS/AP_GPS_Shim.h b/libraries/AP_GPS/AP_GPS_Shim.h index c96ceb927e..38ffc7c568 100644 --- a/libraries/AP_GPS/AP_GPS_Shim.h +++ b/libraries/AP_GPS/AP_GPS_Shim.h @@ -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_() - /// + /// Set-and-mark-updated macro for the public member variables; each instance + /// defines a member function set_() + /// #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 diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 74339f87de..2c991e293c 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -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; } diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 02fc0f7be4..ac05169b36 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -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 diff --git a/libraries/AP_GPS/GPS.cpp b/libraries/AP_GPS/GPS.cpp index 57502643da..db5ef00d6d 100644 --- a/libraries/AP_GPS/GPS.cpp +++ b/libraries/AP_GPS/GPS.cpp @@ -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); } diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index 045149f1ec..397fbfad5b 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -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 diff --git a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde index 533f27a635..0d2267de53 100644 --- a/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde +++ b/libraries/AP_GPS/examples/GPS_406_test/GPS_406_test.pde @@ -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 + } } diff --git a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde index e638f085f0..c6de4f3bf4 100644 --- a/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde +++ b/libraries/AP_GPS/examples/GPS_AUTO_test/GPS_AUTO_test.pde @@ -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; + } } diff --git a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde index 2af983ab9f..afac202a4e 100644 --- a/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde +++ b/libraries/AP_GPS/examples/GPS_MTK_test/GPS_MTK_test.pde @@ -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 + } } diff --git a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde index 3ecfd595f5..9e0cadf912 100644 --- a/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde +++ b/libraries/AP_GPS/examples/GPS_NMEA_test/GPS_NMEA_test.pde @@ -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; + } } diff --git a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde index f40aba2254..edebec4c7b 100644 --- a/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde +++ b/libraries/AP_GPS/examples/GPS_UBLOX_test/GPS_UBLOX_test.pde @@ -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 + } }