• Main Page
  • Namespaces
  • Classes
  • Files
  • File List
  • File Members

/home/jgoppert/Projects/ap/libraries/GPS_IMU/GPS_IMU.cpp

Go to the documentation of this file.
00001 /*
00002         GPS_IMU.cpp - IMU/X-Plane GPS library for Arduino
00003 */
00004 
00005 #include "GPS_IMU.h"
00006 #include <avr/interrupt.h>
00007 #include "WProgram.h"
00008 
00009 
00010 // Constructors ////////////////////////////////////////////////////////////////
00011 GPS_IMU_Class::GPS_IMU_Class()
00012 {
00013 }
00014 
00015 
00016 // Public Methods //////////////////////////////////////////////////////////////
00017 void GPS_IMU_Class::Init(void)
00018 {
00019         ck_a            = 0;
00020         ck_b            = 0;
00021         IMU_step        = 0;
00022         NewData         = 0;
00023         Fix             = 0;
00024         PrintErrors     = 0;
00025         
00026         IMU_timer = millis();    //Restarting timer...
00027         // Initialize serial port
00028         #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00029                 Serial1.begin(38400);                            // Serial port 1 on ATMega1280/2560
00030         #else
00031                 Serial.begin(38400);
00032         #endif
00033 }
00034 
00035 // optimization : This code donīt wait for data, only proccess the data available
00036 // We can call this function on the main loop (50Hz loop)
00037 // If we get a complete packet this function calls parse_IMU_gps() to parse and update the GPS info.
00038 void GPS_IMU_Class::Read(void)
00039 {
00040         static unsigned long GPS_timer = 0;
00041         byte data;
00042         int numc = 0;
00043         static byte message_num = 0;
00044         
00045         #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)          // If AtMega1280/2560 then Serial port 1...
00046                 numc = Serial.available();
00047         #else
00048                 numc = Serial.available();
00049         #endif
00050         
00051         if (numc > 0){
00052                 for (int i=0;i<numc;i++){       // Process bytes received               
00053         
00054                         #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00055                                 data = Serial.read();
00056                         #else
00057                                 data = Serial.read();
00058                         #endif
00059                         
00060                         switch(IMU_step){                //Normally we start from zero. This is a state machine
00061                                 case 0: 
00062                                         if(data == 0x44)        // IMU sync char 1
00063                                                 IMU_step++;      //OH first data packet is correct, so jump to the next IMU_step
00064                                         break; 
00065                                         
00066                                 case 1: 
00067                                         if(data == 0x49)        // IMU sync char 2
00068                                                 IMU_step++;      //ooh! The second data packet is correct, jump to the IMU_step 2
00069                                         else 
00070                                                 IMU_step=0;      //Nop, is not correct so restart to IMU_step zero and try again.                
00071                                         break;
00072         
00073                                 case 2: 
00074                                         if(data == 0x59)        // IMU sync char 3
00075                                                 IMU_step++;      //ooh! The second data packet is correct, jump to the IMU_step 2
00076                                         else 
00077                                                 IMU_step=0;      //Nop, is not correct so restart to IMU_step zero and try again.                
00078                                         break;
00079         
00080                                 case 3: 
00081                                         if(data == 0x64)        // IMU sync char 4
00082                                                 IMU_step++;      //ooh! The second data packet is correct, jump to the IMU_step 2
00083                                         else 
00084                                                 IMU_step=0;      //Nop, is not correct so restart to IMU_step zero and try again.                
00085                                         break;
00086                                                                                 
00087                                 case 4:
00088                                         payload_length = data;
00089                                         checksum(payload_length);
00090                                         IMU_step++;
00091                                         if (payload_length > 28){
00092                                                 IMU_step = 0;    //Bad data, so restart to IMU_step zero and try again.          
00093                                                 payload_counter = 0;
00094                                                 ck_a = 0;
00095                                                 ck_b = 0;
00096                                                 //payload_error_count++;
00097                                         } 
00098                                         break;
00099                                         
00100                                 case 5:
00101                                         message_num = data;
00102                                         checksum(data);
00103                                         IMU_step++;
00104                                         break;
00105         
00106                                 case 6: // Payload data read...
00107                                         // We stay in this state until we reach the payload_length
00108                                         buffer[payload_counter] = data;
00109                                         checksum(data);
00110                                         payload_counter++;
00111                                         if (payload_counter >= payload_length) { 
00112                                                 IMU_step++; 
00113                                         }
00114                                         break;
00115                                         
00116                                 case 7:
00117                                         IMU_ck_a = data;         // First checksum byte
00118                                         IMU_step++;
00119                                         break;
00120                                                                                 
00121                                 case 8:
00122                                         IMU_ck_b = data;         // Second checksum byte
00123         
00124                                         // We end the IMU/GPS read...
00125                                         // Verify the received checksum with the generated checksum.. 
00126                                         if((ck_a == IMU_ck_a) && (ck_b == IMU_ck_b)) {
00127                                                 if (message_num == 0x02) {
00128                                                         IMU_join_data();
00129                                                         IMU_timer = millis();
00130                                                 } else if (message_num == 0x03) {
00131                                                         GPS_join_data();
00132                                                         GPS_timer = millis();
00133                                                 } else if (message_num == 0x04) {
00134                                                         IMU2_join_data();
00135                                                         IMU_timer = millis();
00136                                                 } else if (message_num == 0x0a) {
00137                                                         //PERF_join_data();
00138                                                 } else {
00139                                                         Serial.print("Invalid message number = ");
00140                                                         Serial.println(message_num,DEC);
00141                                                 }
00142                                         } else {
00143                                                 Serial.println("XXX Checksum error");   //bad checksum
00144                                                 //imu_checksum_error_count++;
00145                                         }                                                
00146                                         // Variable initialization
00147                                         IMU_step = 0;
00148                                         payload_counter = 0;
00149                                         ck_a = 0;
00150                                         ck_b = 0;
00151                                         IMU_timer = millis(); //Restarting timer...
00152                                         break;
00153                         }
00154                 }// End for...
00155         }
00156         // If we don't receive GPS packets in 2 seconds => Bad FIX state
00157         if ((millis() - GPS_timer) > 3000){
00158                 Fix = 0;
00159         }
00160         if (PrintErrors){
00161                 Serial.println("ERR:GPS_TIMEOUT!!");
00162         }
00163 }
00164 
00165 /****************************************************************
00166  * 
00167  ****************************************************************/
00168 
00169 void GPS_IMU_Class::IMU_join_data(void)
00170 {
00171         int j;
00172 
00173         
00174         //Verifing if we are in class 1, you can change this "IF" for a "Switch" in case you want to use other IMU classes.. 
00175         //In this case all the message im using are in class 1, to know more about classes check PAGE 60 of DataSheet.
00176 
00177          //Storing IMU roll
00178         intUnion.byte[0] = buffer[j++];
00179         intUnion.byte[1] = buffer[j++];
00180         roll_sensor = intUnion.word;
00181 
00182          //Storing IMU pitch
00183         intUnion.byte[0] = buffer[j++];
00184         intUnion.byte[1] = buffer[j++];
00185         pitch_sensor = intUnion.word;
00186 
00187          //Storing IMU heading (yaw)
00188         intUnion.byte[0] = buffer[j++];
00189         intUnion.byte[1] = buffer[j++];
00190         Ground_Course = intUnion.word;
00191         imu_ok = true;
00192 }
00193 
00194 void GPS_IMU_Class::IMU2_join_data()
00195 {
00196         int j=0;
00197 
00198          //Storing IMU roll
00199         intUnion.byte[0] = buffer[j++];
00200         intUnion.byte[1] = buffer[j++];
00201         roll_sensor = intUnion.word;
00202 
00203          //Storing IMU pitch
00204         intUnion.byte[0] = buffer[j++];
00205         intUnion.byte[1] = buffer[j++];
00206         pitch_sensor = intUnion.word;
00207 
00208          //Storing IMU heading (yaw)
00209         intUnion.byte[0] = buffer[j++];
00210         intUnion.byte[1] = buffer[j++];
00211         Ground_Course = (unsigned int)intUnion.word;
00212         
00213          //Storing airspeed
00214         intUnion.byte[0] = buffer[j++];
00215         intUnion.byte[1] = buffer[j++];
00216         airspeed = intUnion.word;
00217 
00218         imu_ok = true;
00219 
00220 }
00221 
00222 void GPS_IMU_Class::GPS_join_data(void)
00223 {
00224         //gps_messages_received++;
00225         int j = 0;                               
00226 
00227         Longitude = join_4_bytes(&buffer[j]);           // Lat and Lon * 10**7
00228         j += 4;
00229 
00230         Lattitude = join_4_bytes(&buffer[j]);
00231         j += 4;
00232 
00233         //Storing GPS Height above the sea level
00234         intUnion.byte[0] = buffer[j++];
00235         intUnion.byte[1] = buffer[j++];
00236         Altitude = (long)intUnion.word * 10;             //     Altitude in meters * 100 
00237 
00238         //Storing Speed (3-D) 
00239         intUnion.byte[0] = buffer[j++];
00240         intUnion.byte[1] = buffer[j++];
00241         Speed_3d = Ground_Speed = (float)intUnion.word;                 // Speed in M/S * 100
00242         
00243         //We skip the gps ground course because we use yaw value from the IMU for ground course
00244         j += 2;
00245         Time = join_4_bytes(&buffer[j]);                //      Time of Week in milliseconds
00246     j +=4;
00247     imu_health = buffer[j++];
00248         
00249         NewData = 1;
00250         Fix = 1;
00251 
00252 }
00253 
00254 
00255 /****************************************************************
00256  * 
00257  ****************************************************************/
00258  // Join 4 bytes into a long
00259 long GPS_IMU_Class::join_4_bytes(unsigned char Buffer[])
00260 {
00261         longUnion.byte[0] = *Buffer;
00262         longUnion.byte[1] = *(Buffer+1);
00263         longUnion.byte[2] = *(Buffer+2);
00264         longUnion.byte[3] = *(Buffer+3);
00265         return(longUnion.dword);
00266 }
00267 
00268 
00269 /****************************************************************
00270  * 
00271  ****************************************************************/
00272 // checksum algorithm
00273 void GPS_IMU_Class::checksum(byte IMU_data)
00274 {
00275         ck_a+=IMU_data;
00276         ck_b+=ck_a; 
00277 }
00278 
00279 GPS_IMU_Class GPS;

Generated for ArduPilot Libraries by doxygen