From 7d5fbed7727fe9ae6c758d3a8b95eaf8fa6f30f0 Mon Sep 17 00:00:00 2001 From: analoguedevices Date: Sun, 29 Aug 2010 20:38:25 +0000 Subject: [PATCH] Added GPS_IMY git-svn-id: https://arducopter.googlecode.com/svn/trunk@340 f9c3cf11-9bcb-44bc-f272-b75c42450872 --- libraries/GPS_IMU/GPS_IMU.cpp | 279 ++++++++++++++++++ libraries/GPS_IMU/GPS_IMU.h | 69 +++++ .../examples/GPS_IMU_test/GPS_IMU_test.pde | 44 +++ libraries/GPS_IMU/keywords.txt | 16 + 4 files changed, 408 insertions(+) create mode 100644 libraries/GPS_IMU/GPS_IMU.cpp create mode 100644 libraries/GPS_IMU/GPS_IMU.h create mode 100644 libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde create mode 100644 libraries/GPS_IMU/keywords.txt diff --git a/libraries/GPS_IMU/GPS_IMU.cpp b/libraries/GPS_IMU/GPS_IMU.cpp new file mode 100644 index 0000000000..ae7260ce14 --- /dev/null +++ b/libraries/GPS_IMU/GPS_IMU.cpp @@ -0,0 +1,279 @@ +/* + GPS_IMU.cpp - IMU/X-Plane GPS library for Arduino +*/ + +#include "GPS_IMU.h" +#include +#include "WProgram.h" + + +// Constructors //////////////////////////////////////////////////////////////// +GPS_IMU_Class::GPS_IMU_Class() +{ +} + + +// Public Methods ////////////////////////////////////////////////////////////// +void GPS_IMU_Class::Init(void) +{ + ck_a = 0; + ck_b = 0; + IMU_step = 0; + NewData = 0; + Fix = 0; + PrintErrors = 0; + + IMU_timer = millis(); //Restarting timer... + // 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 +// 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. +void GPS_IMU_Class::Read(void) +{ + static unsigned long GPS_timer = 0; + 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 + + if (numc > 0){ + for (int i=0;i 28){ + IMU_step = 0; //Bad data, so restart to IMU_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); + IMU_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) { + IMU_step++; + } + break; + + case 7: + IMU_ck_a = data; // First checksum byte + IMU_step++; + break; + + case 8: + IMU_ck_b = data; // Second checksum byte + + // We end the IMU/GPS read... + // Verify the received checksum with the generated checksum.. + if((ck_a == IMU_ck_a) && (ck_b == IMU_ck_b)) { + if (message_num == 0x02) { + IMU_join_data(); + IMU_timer = millis(); + } else if (message_num == 0x03) { + GPS_join_data(); + GPS_timer = millis(); + } else if (message_num == 0x04) { + IMU2_join_data(); + IMU_timer = millis(); + } else if (message_num == 0x0a) { + //PERF_join_data(); + } else { + Serial.print("Invalid message number = "); + Serial.println(message_num,DEC); + } + } else { + Serial.println("XXX Checksum error"); //bad checksum + //imu_checksum_error_count++; + } + // Variable initialization + IMU_step = 0; + payload_counter = 0; + ck_a = 0; + ck_b = 0; + IMU_timer = millis(); //Restarting timer... + break; + } + }// End for... + } + // If we don't receive GPS packets in 2 seconds => Bad FIX state + if ((millis() - GPS_timer)>2000){ + Fix = 0; + } + if (PrintErrors){ + Serial.println("ERR:GPS_TIMEOUT!!"); + } +} + +/**************************************************************** + * + ****************************************************************/ + +void GPS_IMU_Class::IMU_join_data(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 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; + + //Storing IMU pitch + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + pitch_sensor = intUnion.word; + + //Storing IMU heading (yaw) + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Ground_Course = intUnion.word; + imu_ok = true; +} + +void GPS_IMU_Class::IMU2_join_data() +{ + int j=0; + + //Storing IMU roll + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + roll_sensor = intUnion.word; + + //Storing IMU pitch + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + pitch_sensor = intUnion.word; + + //Storing IMU heading (yaw) + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Ground_Course = (unsigned int)intUnion.word; + + //Storing airspeed + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + airspeed = intUnion.word; + + imu_ok = true; + +} + +void GPS_IMU_Class::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; + + //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 + + //Storing Speed (3-D) + intUnion.byte[0] = buffer[j++]; + intUnion.byte[1] = buffer[j++]; + Speed_3d = Ground_Speed = (float)intUnion.word; // Speed in M/S * 100 + + //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 + j +=4; + imu_health = buffer[j++]; + + NewData = 1; + Fix = 1; + +} + + +/**************************************************************** + * + ****************************************************************/ + // Join 4 bytes into a long +long GPS_IMU_Class::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); +} + + +/**************************************************************** + * + ****************************************************************/ +// checksum algorithm +void GPS_IMU_Class::checksum(byte IMU_data) +{ + ck_a+=IMU_data; + ck_b+=ck_a; +} + +GPS_IMU_Class GPS; \ No newline at end of file diff --git a/libraries/GPS_IMU/GPS_IMU.h b/libraries/GPS_IMU/GPS_IMU.h new file mode 100644 index 0000000000..ef36e0eeee --- /dev/null +++ b/libraries/GPS_IMU/GPS_IMU.h @@ -0,0 +1,69 @@ +#ifndef GPS_IMU_h +#define GPS_IMU_h + +#include + +#define IMU_MAXPAYLOAD 32 + +class GPS_IMU_Class +{ + private: + // Internal variables + union int_union { + int16_t word; + uint8_t byte[2]; + } intUnion; + + union long_union { + int32_t dword; + uint8_t byte[4]; + } longUnion; + + uint8_t ck_a; // Packet checksum + uint8_t ck_b; + uint8_t IMU_ck_a; + uint8_t IMU_ck_b; + + uint8_t IMU_step; + uint8_t IMU_class; + uint8_t message_num; + uint8_t payload_length; + uint8_t payload_counter; + uint8_t buffer[IMU_MAXPAYLOAD]; + + long IMU_timer; + long IMU_ecefVZ; + void IMU_join_data(); + void IMU2_join_data(); + void GPS_join_data(); + void checksum(unsigned char IMU_data); + long join_4_bytes(unsigned char Buffer[]); + + public: + // Methods + GPS_IMU_Class(); + void Init(); + void Read(); + // 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; + + 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 NumSats; // Number of visible satelites + uint8_t Fix; // 1:GPS FIX 0:No FIX (normal logic) + uint8_t NewData; // 1:New GPS Data + uint8_t PrintErrors; // 1: To Print GPS Errors (for debug) +}; + +extern GPS_IMU_Class GPS; + +#endif \ No newline at end of file diff --git a/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde b/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde new file mode 100644 index 0000000000..a2fd398e86 --- /dev/null +++ b/libraries/GPS_IMU/examples/GPS_IMU_test/GPS_IMU_test.pde @@ -0,0 +1,44 @@ +/* + Example of GPS IMU library. + Code by Jordi MuÔøΩoz, Jose Julio and, Jason Short . 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 + +void setup() +{ + Serial.begin(38400); + Serial.println("GPS IMU library test"); + GPS.Init(); // GPS Initialization + delay(1000); +} +void loop() +{ + GPS.Read(); + if (1){ // New GPS data? + + Serial.print("GPS:"); + Serial.print(" Lat:"); + Serial.print(GPS.Lattitude); + Serial.print(" Lon:"); + Serial.print(GPS.Longitude); + Serial.print(" Alt:"); + Serial.print((float)GPS.Altitude/100.0); + Serial.print(" GSP:"); + Serial.print((float)GPS.Ground_Speed/100.0); + Serial.print(" COG:"); + Serial.print(GPS.Ground_Course/1000000); + Serial.print(" SAT:"); + Serial.print((int)GPS.NumSats); + Serial.print(" FIX:"); + Serial.print((int)GPS.Fix); + Serial.print(" TIM:"); + Serial.print(GPS.Time); + Serial.println(); + GPS.NewData = 0; // We have readed the data + } + delay(20); +} diff --git a/libraries/GPS_IMU/keywords.txt b/libraries/GPS_IMU/keywords.txt new file mode 100644 index 0000000000..0c8d9ac33a --- /dev/null +++ b/libraries/GPS_IMU/keywords.txt @@ -0,0 +1,16 @@ +GPS KEYWORD1 +GPS_UBLOX KEYWORD1 +Init KEYWORD2 +Read KEYWORD2 +Time KEYWORD2 +Lattitude KEYWORD2 +Longitude KEYWORD2 +Altitude KEYWORD2 +Ground_Speed KETWORD2 +Ground_Course KEYWORD2 +Speed_3d KEYWORD2 +NumSats KEYWORD2 +Fix KEYWORD2 +NewData KEYWORD2 + +