From 96a80f1c6629dcbfaa4f314c3823a0cec0a52304 Mon Sep 17 00:00:00 2001 From: "DrZiplok@gmail.com" Date: Mon, 6 Sep 2010 09:20:44 +0000 Subject: [PATCH] Cleanup. Teach AP_GPS about FastSerial (in the few places it needs to know) and about Stream everywhere else. Do some minor code cleanup. Tested with Mega and uBlox. Some issues (e.g. reporting 0 satelites) remain. git-svn-id: https://arducopter.googlecode.com/svn/trunk@404 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/AP_GPS/AP_GPS_406.cpp | 176 +++++------------- libraries/AP_GPS/AP_GPS_406.h | 10 +- libraries/AP_GPS/AP_GPS_IMU.cpp | 113 +++-------- libraries/AP_GPS/AP_GPS_IMU.h | 7 +- libraries/AP_GPS/AP_GPS_MTK.cpp | 82 ++------ libraries/AP_GPS/AP_GPS_MTK.h | 26 +-- libraries/AP_GPS/AP_GPS_NMEA.cpp | 44 ++--- libraries/AP_GPS/AP_GPS_NMEA.h | 5 +- libraries/AP_GPS/AP_GPS_UBLOX.cpp | 97 +++------- libraries/AP_GPS/AP_GPS_UBLOX.h | 5 +- libraries/AP_GPS/GPS.h | 126 ++++++++++--- .../examples/GPS_406_test/GPS_406_test.pde | 21 ++- .../examples/GPS_MTK_test/GPS_MTK_test.pde | 16 +- .../examples/GPS_NMEA_test/GPS_NMEA_test.pde | 20 +- .../GPS_UBLOX_test/GPS_UBLOX_test.pde | 18 +- 15 files changed, 302 insertions(+), 464 deletions(-) diff --git a/libraries/AP_GPS/AP_GPS_406.cpp b/libraries/AP_GPS/AP_GPS_406.cpp index d9caf55a97..6d7137c2b8 100644 --- a/libraries/AP_GPS/AP_GPS_406.cpp +++ b/libraries/AP_GPS/AP_GPS_406.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* GPS_406.cpp - 406 GPS library for Arduino Code by Jason Short, Jordi Muñoz and Jose Julio. DIYDrones.com @@ -16,7 +17,7 @@ update() : Call this funcion as often as you want to ensure you read the incomming gps data Properties: - Lattitude : Lattitude * 10,000,000 (long value) + Latitude : Latitude * 10,000,000 (long value) Longitude : Longitude * 10,000,000 (long value) altitude : altitude * 100 (meters) (long value) ground_speed : Speed (m/s) * 100 (long value) @@ -27,58 +28,42 @@ */ +#include "../FastSerial/FastSerial.h" // because we need to change baud rates... ugh. #include "AP_GPS_406.h" #include "WProgram.h" +#include uint8_t AP_GPS_406::buffer[MAXPAYLOAD] = {0x24,0x50,0x53,0x52,0x46,0x31,0x30,0x30,0x2C,0x30,0x2C,0x35,0x37,0x36,0x30,0x30,0x2C,0x38,0x2C,0x31,0x2C,0x30,0x2A,0x33,0x37,0x0D,0x0A}; - #define PAYLOAD_LENGTH 92 #define BAUD_RATE 57600 // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_406::AP_GPS_406() +AP_GPS_406::AP_GPS_406(Stream *s) : GPS(s) { } - // Public Methods //////////////////////////////////////////////////////////////////// void AP_GPS_406::init(void) { - change_to_sirf_protocol(); //Changes to SIFR protocol and sets baud rate - delay(100); //Waits fot the GPS to start_UP - configure_gps(); //Function to configure GPS, to output only the desired msg's - - step = 0; - new_data = 0; - fix = 0; - print_errors = 0; - + change_to_sirf_protocol(); // Changes to SIRF protocol and sets baud rate + delay(100); // Waits fot the GPS to start_UP + configure_gps(); // Function to configure GPS, to output only the desired msg's } -// optimization : This code don´t wait for data, only proccess the data available +// optimization : This code doesn´t wait for data, 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_gps() to parse and update the GPS info. void AP_GPS_406::update(void) { byte data; int numc; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif + + numc = _port->available(); if (numc > 0){ for (int i = 0; i < numc; i++){ // Process bytes received - #if defined(__AVR_ATmega1280__) - data = Serial1.read(); -//Serial.print(data,HEX); -//Serial.println(" "); - #else - data = Serial.read(); - #endif + data = _port->read(); switch(step){ case 0: @@ -143,57 +128,27 @@ AP_GPS_406::parse_gps(void) { uint8_t j; - fix = (buffer[1] > 0) ? 0:1; + fix = buffer[1] > 0; - j = 22; - lattitude = join_4_bytes(&buffer[j]); // lat * 10, 000, 000 + latitude = _swapl(&buffer[22]); // lat * 10, 000, 000 + longitude = _swapl(&buffer[26]); // lon * 10, 000, 000 + altitude = _swapl(&buffer[34]); // alt in meters * 100 + ground_speed = _swapi(&buffer[39]); // meters / second * 100 - j = 26; - longitude = join_4_bytes(&buffer[j]); // lon * 10, 000, 000 - - j = 34; - altitude = join_4_bytes(&buffer[j]); // alt in meters * 100 - - j = 39; - ground_speed = join_2_bytes(&buffer[j]); // meters / second * 100 - - if(ground_speed >= 50){ - //Only updates data if we are really moving... - j = 41; - ground_course = (unsigned int)join_2_bytes(&buffer[j]); // meters / second * 100 + if (ground_speed >= 50) { // Only updates data if we are really moving... + ground_course = (unsigned int)_swapi(&buffer[41]); // meters / second * 100 } - j = 45; - //climb_rate = join_2_bytes(&buffer[j]); // meters / second * 100 + //climb_rate = _swapi(&buffer[45]); // meters / second * 100 - if(lattitude == 0){ + if (latitude == 0) { new_data = false; - fix = 0; - }else{ + fix = false; + } else { new_data = true; } } - // Join 4 bytes into a long -int32_t -AP_GPS_406::join_4_bytes(unsigned char Buffer[]) -{ - longUnion.byte[3] = *Buffer; - longUnion.byte[2] = *(Buffer + 1); - longUnion.byte[1] = *(Buffer + 2); - longUnion.byte[0] = *(Buffer + 3); - return(longUnion.dword); -} - -// Join 2 bytes into an int -int16_t -AP_GPS_406::join_2_bytes(unsigned char Buffer[]) -{ - intUnion.byte[1] = *Buffer; - intUnion.byte[0] = *(Buffer + 1); - return(intUnion.word); -} - void AP_GPS_406::configure_gps(void) { @@ -201,82 +156,37 @@ AP_GPS_406::configure_gps(void) 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}; - const uint8_t cero = 0x00; - #if defined(__AVR_ATmega1280__) for(int z = 0; z < 2; z++){ for(int x = 0; x < 5; x++){ - for(int y = 0; y < 6; y++){ - Serial1.print(gps_header[y]); // Prints the msg header, is the same header for all msg.. - } - Serial1.print(gps_payload[x]); // Prints the payload, is not the same for every msg - for(int y = 0; y < 6; y++){ - Serial1.print(cero); // Prints 6 zeros - } - Serial1.print(gps_checksum[x]); // Print the Checksum - Serial1.print(gps_ender[0]); // Print the Ender of the string, is same on all msg's. - Serial1.print(gps_ender[1]); // ender + _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 } } - #else - for(int z = 0; z < 2; z++){ - for(int x = 0; x < 5; x++){ - for(int y = 0; y < 6; y++){ - Serial.print(gps_header[y]); // Prints the msg header, is the same header for all msg.. - } - Serial.print(gps_payload[x]); // Prints the payload, is not the same for every msg - for(int y = 0; y < 6; y++){ - Serial.print(cero); // Prints 6 zeros - } - Serial.print(gps_checksum[x]); // Print the Checksum - Serial.print(gps_ender[0]); // Print the Ender of the string, is same on all msg's. - Serial.print(gps_ender[1]); // ender - } - } - #endif } void AP_GPS_406::change_to_sirf_protocol(void) { - #if defined(__AVR_ATmega1280__) - Serial1.begin(4800); // Serial port 1 on ATMega1280 First try in 4800 - delay(300); - for (byte x = 0; x <= 28; x++){ - Serial1.print(buffer[x]); // Sending special bytes declared at the beginning - } - delay(300); + FastSerial *fs = (FastSerial *)_port; // this is a bit grody... + + fs->begin(4800); // First try at 4800bps + delay(300); + _port->write(buffer, 28); // Sending special bytes declared at the beginning + delay(300); - Serial1.begin(9600); // Then try in 9600 - delay(300); - for (byte x = 0; x <= 28; x++){ - Serial1.print(buffer[x]); - } - delay(300); - - Serial1.begin(BAUD_RATE); // - delay(300); - for (byte x = 0; x <= 28; x++){ - Serial1.print(buffer[x]); - } - delay(300); - - #else - Serial.begin(4800); // Serial port (0) on AT168/328 First try in 4800 - delay(300); - for (byte x = 0; x <= 28; x++){ - Serial.print(buffer[x]); // Sending special bytes declared at the beginning - } - delay(300); + fs->begin(9600); // Then try at 9600bps + delay(300); + _port->write(buffer, 28); + delay(300); - Serial.begin(9600); // Then try in 9600 - delay(300); - for (byte x = 0; x <= 28; x++){ - Serial.print(buffer[x]); - } - - Serial.begin(BAUD_RATE); // Universal Sincronus Asyncronus Receiveing Transmiting - - #endif + fs->begin(BAUD_RATE); // Finally try at the configured bit rate (57600?) + delay(300); + _port->write(buffer, 28); } diff --git a/libraries/AP_GPS/AP_GPS_406.h b/libraries/AP_GPS/AP_GPS_406.h index 9ed5f45ae9..e9dbf63023 100644 --- a/libraries/AP_GPS/AP_GPS_406.h +++ b/libraries/AP_GPS/AP_GPS_406.h @@ -1,3 +1,5 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + #ifndef AP_GPS_406_h #define AP_GPS_406_h @@ -6,21 +8,19 @@ class AP_GPS_406 : public GPS { - public: +public: // Methods - AP_GPS_406(); + AP_GPS_406(Stream *port); void init(); void update(); - private: +private: // Internal variables uint8_t step; uint8_t payload_counter; static uint8_t buffer[MAXPAYLOAD]; void parse_gps(); - int32_t join_4_bytes(uint8_t Buffer[]); - int16_t join_2_bytes(uint8_t Buffer[]); void change_to_sirf_protocol(void); void configure_gps(void); }; diff --git a/libraries/AP_GPS/AP_GPS_IMU.cpp b/libraries/AP_GPS/AP_GPS_IMU.cpp index dcfe13380c..4ce66292b6 100755 --- a/libraries/AP_GPS/AP_GPS_IMU.cpp +++ b/libraries/AP_GPS/AP_GPS_IMU.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* GPS_MTK.cpp - Ublox GPS library for Arduino Code by Jordi Muñoz and Jose Julio. DIYDrones.com @@ -31,7 +32,7 @@ // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_IMU::AP_GPS_IMU() +AP_GPS_IMU::AP_GPS_IMU(Stream *s) : GPS(s) { } @@ -39,19 +40,7 @@ AP_GPS_IMU::AP_GPS_IMU() // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_IMU::init(void) { - ck_a = 0; - ck_b = 0; - step = 0; - new_data = 0; - fix = 0; - print_errors = 0; - - // initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif + // we expect the stream to already be open at the corret bitrate } // optimization : This code doesn't wait for data. It only proccess the data available. @@ -62,21 +51,13 @@ void AP_GPS_IMU::update(void) byte data; int numc = 0; static byte message_num = 0; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial.available(); - #else - numc = Serial.available(); - #endif - + + numc = _port->available(); + if (numc > 0){ for (int i=0;iread(); switch(step){ //Normally we start from zero. This is a state machine case 0: @@ -154,11 +135,10 @@ void AP_GPS_IMU::update(void) } else if (message_num == 0x0a) { //PERF_join_data(); } else { - Serial.print("Invalid message number = "); - Serial.println(message_num, DEC); + _error("Invalid message number = %d\n", (int)message_num); } } else { - Serial.println("XXX Checksum error"); //bad checksum + _error("XXX Checksum error\n"); //bad checksum //imu_checksum_error_count++; } // Variable initialization @@ -178,50 +158,34 @@ void AP_GPS_IMU::update(void) void AP_GPS_IMU::join_data(void) { - int j = 0; //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 - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - roll_sensor = intUnion.word; + roll_sensor = *(int *)&buffer[0]; //Storing IMU pitch - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - pitch_sensor = intUnion.word; + pitch_sensor = *(int *)&buffer[2]; //Storing IMU heading (yaw) - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - ground_course = intUnion.word; + ground_course = *(int *)&buffer[4]; imu_ok = true; } void AP_GPS_IMU::join_data_xplane() { - int j = 0; - //Storing IMU roll - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - roll_sensor = intUnion.word; + roll_sensor = *(int *)&buffer[0]; + //Storing IMU pitch - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - pitch_sensor = intUnion.word; + pitch_sensor = *(int *)&buffer[2]; //Storing IMU heading (yaw) - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - ground_course = (unsigned int)intUnion.word; + ground_course = *(int *)&buffer[4]; //Storing airspeed - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - airspeed = intUnion.word; + airspeed = *(int *)&buffer[6]; imu_ok = true; @@ -229,50 +193,25 @@ void AP_GPS_IMU::join_data_xplane() void AP_GPS_IMU::GPS_join_data(void) { - //gps_messages_received++; - int j = 0; - - longitude = join_4_bytes(&buffer[j]); // Lat and Lon * 10**7 - j += 4; - - lattitude = join_4_bytes(&buffer[j]); - j += 4; + longitude = *(long *)&buffer[0]; // degrees * 10e7 + latitude = *(long *)&buffer[4]; //Storing GPS Height above the sea level - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - altitude = (long)intUnion.word * 10; // Altitude in meters * 100 + altitude = (long)*(int *)&buffer[8] * 10; //Storing Speed - intUnion.byte[0] = buffer[j++]; - intUnion.byte[1] = buffer[j++]; - speed_3d = ground_speed = (float)intUnion.word; // Speed in M/S * 100 + 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 - j += 2; - time = join_4_bytes(&buffer[j]); // Time of Week in milliseconds + time = *(long *)&buffer[14]; - j += 4; - imu_health = buffer[j++]; + imu_health = buffer[15]; - new_data = 1; - fix = 1; + new_data = true; + fix = true; } -/**************************************************************** - * - ****************************************************************/ - // Join 4 bytes into a long -long AP_GPS_IMU::join_4_bytes(unsigned char Buffer[]) -{ - longUnion.byte[0] = *Buffer; - longUnion.byte[1] = *(Buffer + 1); - longUnion.byte[2] = *(Buffer + 2); - longUnion.byte[3] = *(Buffer + 3); - return(longUnion.dword); -} - /**************************************************************** * diff --git a/libraries/AP_GPS/AP_GPS_IMU.h b/libraries/AP_GPS/AP_GPS_IMU.h index 19bc4d4ba2..72c883c536 100755 --- a/libraries/AP_GPS/AP_GPS_IMU.h +++ b/libraries/AP_GPS/AP_GPS_IMU.h @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #ifndef AP_GPS_IMU_h #define AP_GPS_IMU_h @@ -7,8 +8,9 @@ class AP_GPS_IMU : public GPS { public: + // Methods - AP_GPS_IMU(); + AP_GPS_IMU(Stream *s); void init(); void update(); @@ -37,7 +39,6 @@ class AP_GPS_IMU : public GPS void join_data_xplane(); void GPS_join_data(); void checksum(unsigned char data); - long join_4_bytes(unsigned char Buffer[]); }; -#endif \ No newline at end of file +#endif diff --git a/libraries/AP_GPS/AP_GPS_MTK.cpp b/libraries/AP_GPS/AP_GPS_MTK.cpp index 34c303c029..8defd43b8e 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.cpp +++ b/libraries/AP_GPS/AP_GPS_MTK.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* GPS_MTK.cpp - Ublox GPS library for Arduino Code by Jordi Munoz and Jose Julio. DIYDrones.com @@ -31,7 +32,7 @@ #include "WProgram.h" // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_MTK::AP_GPS_MTK() +AP_GPS_MTK::AP_GPS_MTK(Stream *s) : GPS(s) { } @@ -39,45 +40,23 @@ AP_GPS_MTK::AP_GPS_MTK() // Public Methods ////////////////////////////////////////////////////////////// void AP_GPS_MTK::init(void) { - ck_a = 0; - ck_b = 0; - step = 0; - new_data = 0; - fix = 0; - print_errors = 0; - // initialize serial port for binary protocol use - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - Serial1.print(MTK_SET_BINARY); - Serial1.print(MTK_OUTPUT_4HZ); - #else - Serial.begin(38400); - Serial.print(MTK_SET_BINARY); - Serial.print(MTK_OUTPUT_4HZ); - #endif + _port->print(MTK_SET_BINARY); + _port->print(MTK_OUTPUT_4HZ); } -// optimization : This code donÂ¥t wait for data, only proccess the data available +// optimization : This code doesn't wait for data, 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_gps() to parse and update the GPS info. void AP_GPS_MTK::update(void) { byte data; int numc; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif + + numc = _port->available(); if (numc > 0) for (int i = 0; i < numc; i++){ // Process bytes received - #if defined(__AVR_ATmega1280__) - data = Serial1.read(); - #else - data = Serial.read(); - #endif + data = _port->read(); switch(step){ case 0: @@ -126,8 +105,7 @@ void AP_GPS_MTK::update(void) if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum.. parse_gps(); // Parse the new GPS packet }else { - if (print_errors) - Serial.println("ERR:GPS_CHK!!"); + _error("ERR:GPS_CHK!!\n"); } // Variable initialization step = 0; @@ -143,50 +121,26 @@ void AP_GPS_MTK::update(void) void AP_GPS_MTK::parse_gps(void) { - int j; //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet. if(msg_class == 0x01) { switch(id){ //Checking the UBX ID case 0x05: // ID Custom - j = 0; - lattitude= join_4_bytes(&buffer[j]) * 10; // lon * 10000000 - j += 4; - longitude = join_4_bytes(&buffer[j]) * 10; // lat * 10000000 - j += 4; - altitude = join_4_bytes(&buffer[j]); // MSL - j += 4; - speed_3d = ground_speed = join_4_bytes(&buffer[j]); - j += 4; - ground_course = join_4_bytes(&buffer[j]) / 10000; - j += 4; - num_sats = buffer[j]; - j++; - fix = buffer[j]; - if (fix==3) - fix = 1; - else - fix = 0; - j++; - time = join_4_bytes(&buffer[j]); - new_data = 1; + latitude = _swapl(&buffer[0]); + longitude = _swapl(&buffer[4]); + altitude = _swapl(&buffer[8]); + speed_3d = ground_speed = _swapl(&buffer[12]); + ground_course = _swapl(&buffer[16]) / 10000; + num_sats = buffer[20]; + fix = buffer[21] == 3; + time = _swapl(&buffer[22]); + new_data = true; break; } } } - // Join 4 bytes into a long -long -AP_GPS_MTK::join_4_bytes(unsigned char buffer[]) -{ - longUnion.byte[3] = *buffer; - longUnion.byte[2] = *(buffer + 1); - longUnion.byte[1] = *(buffer + 2); - longUnion.byte[0] = *(buffer + 3); - return(longUnion.dword); -} - // checksum algorithm void AP_GPS_MTK::checksum(byte data) diff --git a/libraries/AP_GPS/AP_GPS_MTK.h b/libraries/AP_GPS/AP_GPS_MTK.h index f0d81740df..98f00ee74c 100644 --- a/libraries/AP_GPS/AP_GPS_MTK.h +++ b/libraries/AP_GPS/AP_GPS_MTK.h @@ -1,31 +1,32 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #ifndef AP_GPS_MTK_h #define AP_GPS_MTK_h #include #define MAXPAYLOAD 32 -#define MTK_SET_BINARY      "$PGCMD,16,0,0,0,0,0*6A\r\n" -#define MTK_SET_NMEA        "$PGCMD,16,1,1,1,1,1*6B\r\n" +#define MTK_SET_BINARY "$PGCMD,16,0,0,0,0,0*6A\r\n" +#define MTK_SET_NMEA "$PGCMD,16,1,1,1,1,1*6B\r\n" -#define MTK_OUTPUT_1HZ      "$PMTK220,1000*1F\r\n" -#define MTK_OUTPUT_2HZ      "$PMTK220,500*2B\r\n" -#define MTK_OUTPUT_4HZ      "$PMTK220,250*29\r\n" -#define MTK_OTUPUT_5HZ      "$PMTK220,200*2C\r\n" -#define MTK_OUTPUT_10HZ     "$PMTK220,100*2F\r\n" +#define MTK_OUTPUT_1HZ "$PMTK220,1000*1F\r\n" +#define MTK_OUTPUT_2HZ "$PMTK220,500*2B\r\n" +#define MTK_OUTPUT_4HZ "$PMTK220,250*29\r\n" +#define MTK_OTUPUT_5HZ "$PMTK220,200*2C\r\n" +#define MTK_OUTPUT_10HZ "$PMTK220,100*2F\r\n" #define MTK_BAUD_RATE_38400 "$PMTK251,38400*27\r\n" -#define SBAS_ON             "$PMTK313,1*2E\r\n" -#define SBAS_OFF            "$PMTK313,0*2F\r\n" +#define SBAS_ON "$PMTK313,1*2E\r\n" +#define SBAS_OFF "$PMTK313,0*2F\r\n" -#define WAAS_ON             "$PSRF151,1*3F\r\n" -#define WAAS_OFF            "$PSRF151,0*3E\r\n" +#define WAAS_ON "$PSRF151,1*3F\r\n" +#define WAAS_OFF "$PSRF151,0*3E\r\n" class AP_GPS_MTK : public GPS { public: // Methods - AP_GPS_MTK(); + AP_GPS_MTK(Stream *s); void init(); void update(); @@ -46,6 +47,5 @@ class AP_GPS_MTK : public GPS void parse_gps(); void checksum(unsigned char data); - long join_4_bytes(unsigned char Buffer[]); }; #endif diff --git a/libraries/AP_GPS/AP_GPS_NMEA.cpp b/libraries/AP_GPS/AP_GPS_NMEA.cpp index 1d9be6f34f..bf06be334d 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.cpp +++ b/libraries/AP_GPS/AP_GPS_NMEA.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* GPS_NMEA.cpp - Generic NMEA GPS library for Arduino Code by Jordi Muñoz and Jose Julio. DIYDrones.com @@ -19,7 +20,7 @@ update() : Call this funcion as often as you want to ensure you read the incomming gps data Properties: - lattitude : lattitude * 10000000 (long value) + latitude : latitude * 10000000 (long value) longitude : longitude * 10000000 (long value) altitude : altitude * 1000 (milimeters) (long value) ground_speed : Speed (m / s) * 100 (long value) @@ -40,7 +41,7 @@ #include "WProgram.h" // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_NMEA::AP_GPS_NMEA() +AP_GPS_NMEA::AP_GPS_NMEA(Stream *s) : GPS(s) { } @@ -49,18 +50,6 @@ void AP_GPS_NMEA::init(void) { //Type = 2; - GPS_checksum_calc = false; - bufferidx = 0; - new_data = 0; - fix = 0; - quality = 0; - print_errors = 0; - // Initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif } // This code don´t wait for data, only proccess the data available on serial port @@ -72,20 +61,12 @@ AP_GPS_NMEA::update(void) char c; int numc; int i; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif + + numc = _port->available(); if (numc > 0){ for (i = 0; i < numc; i++){ - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - c = Serial1.read(); - #else - c = Serial.read(); - #endif + c = _port->read(); if (c == '$'){ // NMEA Start bufferidx = 0; buffer[bufferidx++] = c; @@ -138,10 +119,10 @@ AP_GPS_NMEA::parse_nmea_gps(void) // aux_deg = parsedecimal(parseptr, 2); // degrees aux_min = parsenumber(parseptr + 2, 4); // minutes (sexagesimal) => Convert to decimal - lattitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5) + latitude = aux_deg * 10000000 + (aux_min * 50) / 3; // degrees + minutes / 0.6 ( * 10000000) (0.6 = 3 / 5) parseptr = strchr(parseptr, ', ')+1; if ( * parseptr == 'S') - lattitude = -1 * lattitude; // South lattitudes are negative + latitude = -1 * latitude; // South latitudes are negative parseptr = strchr(parseptr, ', ')+1; // W longitudes are Negative aux_deg = parsedecimal(parseptr, 3); // degrees @@ -170,8 +151,7 @@ AP_GPS_NMEA::parse_nmea_gps(void) else quality = 4; // Good (HDOP < 25) } else { - if (print_errors) - Serial.println("GPSERR: Checksum error!!"); + _error("GPSERR: Checksum error!!\n"); } } } else if (strncmp(buffer,"$GPVTG",6)==0){ // Check if sentence begins with $GPVTG @@ -190,14 +170,12 @@ AP_GPS_NMEA::parse_nmea_gps(void) ground_speed = parsenumber(parseptr, 2) * 10 / 36; // Convert Km / h to m / s ( * 100) //GPS_line = true; } else { - if (print_errors) - Serial.println("GPSERR: Checksum error!!"); + _error("GPSERR: Checksum error!!\n"); } } } else { bufferidx = 0; - if (print_errors) - Serial.println("GPSERR: Bad sentence!!"); + _error("GPSERR: Bad sentence!!\n"); } } diff --git a/libraries/AP_GPS/AP_GPS_NMEA.h b/libraries/AP_GPS/AP_GPS_NMEA.h index 492e479131..48c4cd9a6e 100644 --- a/libraries/AP_GPS/AP_GPS_NMEA.h +++ b/libraries/AP_GPS/AP_GPS_NMEA.h @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #ifndef AP_GPS_NMEA_h #define AP_GPS_NMEA_h @@ -8,7 +9,7 @@ class AP_GPS_NMEA : public GPS { public: // Methods - AP_GPS_NMEA(); + AP_GPS_NMEA(Stream *s); void init(); void update(); @@ -30,4 +31,4 @@ class AP_GPS_NMEA : public GPS }; -#endif \ No newline at end of file +#endif diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.cpp b/libraries/AP_GPS/AP_GPS_UBLOX.cpp index 1665db6699..20ed5c63b9 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.cpp +++ b/libraries/AP_GPS/AP_GPS_UBLOX.cpp @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- /* GPS_UBLOX.cpp - Ublox GPS library for Arduino Code by Jordi Muñoz and Jose Julio. DIYDrones.com @@ -37,7 +38,7 @@ #include "WProgram.h" // Constructors //////////////////////////////////////////////////////////////// -AP_GPS_UBLOX::AP_GPS_UBLOX() +AP_GPS_UBLOX::AP_GPS_UBLOX(Stream *s) : GPS(s) { } @@ -45,42 +46,21 @@ AP_GPS_UBLOX::AP_GPS_UBLOX() // Public Methods //////////////////////////////////////////////////////////////////// void AP_GPS_UBLOX::init(void) { - ck_a = 0; - ck_b = 0; - step = 0; - new_data = 0; - fix = 0; - print_errors = 0; - - // initialize serial port - #if defined(__AVR_ATmega1280__) - Serial1.begin(38400); // Serial port 1 on ATMega1280 - #else - Serial.begin(38400); - #endif } -// optimization : This code don´t wait for data, only proccess the data available +// optimization : This code doesn't wait for data, 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_gps() to parse and update the GPS info. void AP_GPS_UBLOX::update(void) { byte data; int numc; - - #if defined(__AVR_ATmega1280__) // If AtMega1280 then Serial port 1... - numc = Serial1.available(); - #else - numc = Serial.available(); - #endif - + + numc = _port->available(); + if (numc > 0){ for (int i = 0;i < numc;i++){ // Process bytes received - #if defined(__AVR_ATmega1280__) - data = Serial1.read(); - #else - data = Serial.read(); - #endif + data = _port->read(); switch(step){ case 0: @@ -113,8 +93,7 @@ void AP_GPS_UBLOX::update(void) step++; // We check if the payload lenght is valid... if (payload_length_hi >= MAXPAYLOAD){ - if (print_errors) - Serial.println("ERR:GPS_BAD_PAYLOAD_LENGTH!!"); + _error("ERR:GPS_BAD_PAYLOAD_LENGTH!!\n"); step = 0; // Bad data, so restart to step zero and try again. ck_a = 0; ck_b = 0; @@ -148,7 +127,7 @@ void AP_GPS_UBLOX::update(void) if((ck_a == GPS_ck_a) && (ck_b == GPS_ck_b)){ // Verify the received checksum with the generated checksum.. parse_gps(); // Parse the new GPS packet }else{ - if (print_errors) Serial.println("ERR:GPS_CHK!!"); + _error("ERR:GPS_CHK!!\n"); } // Variable initialization step = 0; @@ -164,73 +143,41 @@ void AP_GPS_UBLOX::update(void) void AP_GPS_UBLOX::parse_gps(void) { - int j; //Verifing if we are in msg_class 1, you can change this "IF" for a "Switch" in case you want to use other UBX classes.. // In this case all the message im using are in msg_class 1, to know more about classes check PAGE 60 of DataSheet. if(msg_class == 0x01){ switch(id) {//Checking the UBX ID case 0x02: // ID NAV - POSLLH - j = 0; - time = join_4_bytes(&buffer[j]); // ms Time of week - j += 4; - longitude = join_4_bytes(&buffer[j]); // lon * 10,000,000 - j += 4; - lattitude = join_4_bytes(&buffer[j]); // lat * 10,000,000 - j += 4; - //altitude = join_4_bytes(&buffer[j]); // elipsoid heigth mm - j += 4; - altitude = (float)join_4_bytes(&buffer[j]) / 10; // MSL heigth mm - //j+=4; + time = *(long *)&buffer[0]; // ms Time of week + longitude = *(long *)&buffer[4]; // lon * 10,000,000 + latitude = *(long *)&buffer[8]; // lat * 10,000,000 + //altitude = *(long *)&buffer[12]; // elipsoid heigth mm + altitude = *(long *)&buffer[16] / 10; // MSL heigth mm /* - hacc = (float)join_4_bytes(&buffer[j]) / (float)1000; - j += 4; - vacc = (float)join_4_bytes(&buffer[j]) / (float)1000; - j += 4; + hacc = (float)*(long *)&buffer[20]; + vacc = (float)*(long *)&buffer[24]; */ - new_data = 1; + new_data = true; break; case 0x03: //ID NAV - STATUS - //if(buffer[4] >= 0x03) - if((buffer[4] >= 0x03) && (buffer[5] & 0x01)) - fix = 1; // valid position - else - fix = 0; // invalid position + fix = ((buffer[4] >= 0x03) && (buffer[5] & 0x01)); break; case 0x06: //ID NAV - SOL - if((buffer[10] >= 0x03) && (buffer[11] & 0x01)) - fix = 1; // valid position - else - fix = 0; // invalid position - //ecefVZ = join_4_bytes(&buffer[36]); // Vertical Speed in cm / s + fix = ((buffer[10] >= 0x03) && (buffer[11] & 0x01)); num_sats = buffer[47]; // Number of sats... break; case 0x12: // ID NAV - VELNED - j = 16; - speed_3d = join_4_bytes(&buffer[j]); // cm / s - j += 4; - ground_speed = join_4_bytes(&buffer[j]); // Ground speed 2D cm / s - j += 4; - ground_course = join_4_bytes(&buffer[j]); // Heading 2D deg * 100000 - ground_course /= 1000; // Rescale heading to deg * 100 - j += 4; + speed_3d = *(long *)&buffer[16]; // cm / s + ground_speed = *(long *)&buffer[20]; // Ground speed 2D cm / s + ground_course = *(long *)&buffer[24] / 1000; // Heading 2D deg * 100000 rescaled to deg * 100 break; } } } - // Join 4 bytes into a long -long AP_GPS_UBLOX::join_4_bytes(unsigned char Buffer[]) -{ - longUnion.byte[0] = *Buffer; - longUnion.byte[1] = *(Buffer + 1); - longUnion.byte[2] = *(Buffer + 2); - longUnion.byte[3] = *(Buffer + 3); - return(longUnion.dword); -} - // Ublox checksum algorithm void AP_GPS_UBLOX::checksum(byte data) { diff --git a/libraries/AP_GPS/AP_GPS_UBLOX.h b/libraries/AP_GPS/AP_GPS_UBLOX.h index 90e6c8ee48..d9fc2e6dcd 100644 --- a/libraries/AP_GPS/AP_GPS_UBLOX.h +++ b/libraries/AP_GPS/AP_GPS_UBLOX.h @@ -1,3 +1,4 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- #ifndef AP_GPS_UBLOX_h #define AP_GPS_UBLOX_h @@ -8,7 +9,7 @@ class AP_GPS_UBLOX : public GPS { public: // Methods - AP_GPS_UBLOX(); + AP_GPS_UBLOX(Stream *s); void init(); void update(); @@ -32,4 +33,4 @@ class AP_GPS_UBLOX : public GPS long join_4_bytes(unsigned char Buffer[]); }; -#endif \ No newline at end of file +#endif diff --git a/libraries/AP_GPS/GPS.h b/libraries/AP_GPS/GPS.h index b69baa76e5..9d08f085b5 100644 --- a/libraries/AP_GPS/GPS.h +++ b/libraries/AP_GPS/GPS.h @@ -1,38 +1,116 @@ +// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*- + +/// @file GPS.h +/// @brief Interface definition for the various GPS drivers. + #ifndef GPS_h #define GPS_h #include +#include +/// @class GPS +/// @brief Abstract base class for GPS receiver drivers. class GPS { - public: - // Methods - virtual void init(); - virtual void update(); +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. + /// + GPS(Stream *port) : _port(port) {}; + + /// Startup initialisation. + /// + /// This routine performs any one-off initialisation required to set the + /// GPS up for use. + /// + virtual void init(); + + /// Update GPS state based on possible bytes received from the module. + /// + /// This routine must be called periodically to process incoming data. + /// + virtual void update(); // Properties - long time; //GPS Millisecond Time of Week - long lattitude; // Geographic coordinates - long longitude; - long altitude; - long ground_speed; - long ground_course; - long speed_3d; - uint8_t num_sats; // Number of visible satelites - uint8_t fix; // 1:GPS FIX 0:No FIX (normal logic) - uint8_t new_data; // 1:New GPS Data - uint8_t print_errors; // 1: To Print GPS Errors (for debug) - long GPS_timer; + long time; ///< GPS time in milliseconds from the start of the week + 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) + uint8_t num_sats; ///< Number of visible satelites + bool fix; ///< true if we have a position fix + + /// 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; - union long_union { - int32_t dword; - uint8_t byte[4]; - } longUnion; + bool print_errors; ///< if true, errors will be printed to stderr - union int_union { - int16_t word; - uint8_t byte[2]; - } intUnion; +protected: + Stream *_port; ///< stream port the GPS is attached to + + /// 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 uint8_t *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 + int _swapi(const uint8_t *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 + /// + void _error(const char *msg, ...); }; +inline long +GPS::_swapl(const uint8_t *bytes) +{ + union { + long v; + uint8_t b[4]; + } u; + + u.b[0] = bytes[3]; + u.b[1] = bytes[2]; + u.b[2] = bytes[1]; + u.b[3] = bytes[0]; + + return(u.v); +} + +inline int16_t +GPS::_swapi(const uint8_t *bytes) +{ + union { + int16_t v; + uint8_t b[2]; + } u; + + u.b[0] = bytes[1]; + u.b[1] = bytes[0]; + + 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 df0ee4bd9a..72ecab4df6 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 @@ -1,23 +1,28 @@ /* - Example of GPS MTK library. + Example of GPS 406 library. Code by Jordi Munoz and Jose Julio. DIYDrones.com Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 */ -#include // UBLOX GPS Library +#include +#include +#include -AP_GPS_406 gps; +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_406 gps(&Serial1); #define T6 1000000 #define T7 10000000 void setup() { Serial.begin(38400); - Serial.println("Switching to 57600 Baud"); - delay(500); - Serial.begin(57600); + Serial1.begin(57600); + stderr = stdout; + gps.print_errors = true; + Serial.println("GPS 406 library test"); gps.init(); // GPS Initialization delay(1000); @@ -29,7 +34,7 @@ void loop() if (gps.new_data){ Serial.print("gps:"); Serial.print(" Lat:"); - Serial.print((float)gps.lattitude / T7, DEC); + Serial.print((float)gps.latitude / T7, DEC); Serial.print(" Lon:"); Serial.print((float)gps.longitude / T7, DEC); Serial.print(" Alt:"); 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 5f6fcfbaa2..f26c99b687 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 @@ -3,18 +3,26 @@ Code by Jordi Munoz and Jose Julio. DIYDrones.com Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 */ -#include // UBLOX GPS Library +#include +#include +#include -AP_GPS_MTK gps; +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_MTK gps(&Serial); #define T6 1000000 #define T7 10000000 void setup() { Serial.begin(38400); + Serial1.begin(57600); + stderr = stdout; + gps.print_errors = true; + Serial.println("GPS MTK library test"); gps.init(); // GPS Initialization delay(1000); @@ -26,7 +34,7 @@ void loop() if (gps.new_data){ Serial.print("gps:"); Serial.print(" Lat:"); - Serial.print((float)gps.lattitude / T7, DEC); + Serial.print((float)gps.latitude / T7, DEC); Serial.print(" Lon:"); Serial.print((float)gps.longitude / T7, DEC); Serial.print(" Alt:"); 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 8720011db4..5a737ac63a 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 @@ -1,20 +1,28 @@ /* - Example of GPS MTK library. + Example of GPS NMEA library. Code by Jordi Munoz and Jose Julio. DIYDrones.com Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 */ -#include // UBLOX GPS Library +#include +#include +#include -AP_GPS_NMEA gps; +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_NMEA gps(&Serial1); #define T6 1000000 #define T7 10000000 void setup() { - Serial.begin(57600); + Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; + Serial.println("GPS NMEA library test"); gps.init(); // GPS Initialization delay(1000); @@ -27,7 +35,7 @@ void loop() if (gps.new_data){ Serial.print("gps:"); Serial.print(" Lat:"); - Serial.print((float)gps.lattitude / T7, DEC); + Serial.print((float)gps.latitude / T7, DEC); Serial.print(" Lon:"); Serial.print((float)gps.longitude / T7, DEC); Serial.print(" Alt:"); 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 f90ecbb860..e88251ac7d 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 @@ -1,20 +1,28 @@ /* - Example of GPS MTK library. + Example of GPS UBlox library. Code by Jordi Munoz and Jose Julio. DIYDrones.com Works with Ardupilot Mega Hardware (GPS on Serial Port1) - and with standard ATMega168 and ATMega328 on Serial Port 0 */ -#include // UBLOX GPS Library +#include +#include +#include -AP_GPS_UBLOX gps; +FastSerialPort0(Serial); +FastSerialPort1(Serial1); + +AP_GPS_UBLOX gps(&Serial1); #define T6 1000000 #define T7 10000000 void setup() { Serial.begin(38400); + Serial1.begin(38400); + stderr = stdout; + gps.print_errors = true; + Serial.println("GPS UBLOX library test"); gps.init(); // GPS Initialization delay(1000); @@ -26,7 +34,7 @@ void loop() if (gps.new_data){ Serial.print("gps:"); Serial.print(" Lat:"); - Serial.print((float)gps.lattitude / T7, DEC); + Serial.print((float)gps.latitude / T7, DEC); Serial.print(" Lon:"); Serial.print((float)gps.longitude / T7, DEC); Serial.print(" Alt:");