Go to the documentation of this file.00001
00002
00003
00004
00005 #include "GPS_IMU.h"
00006 #include <avr/interrupt.h>
00007 #include "WProgram.h"
00008
00009
00010
00011 GPS_IMU_Class::GPS_IMU_Class()
00012 {
00013 }
00014
00015
00016
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();
00027
00028 #if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
00029 Serial1.begin(38400);
00030 #else
00031 Serial.begin(38400);
00032 #endif
00033 }
00034
00035
00036
00037
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++){
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){
00061 case 0:
00062 if(data == 0x44)
00063 IMU_step++;
00064 break;
00065
00066 case 1:
00067 if(data == 0x49)
00068 IMU_step++;
00069 else
00070 IMU_step=0;
00071 break;
00072
00073 case 2:
00074 if(data == 0x59)
00075 IMU_step++;
00076 else
00077 IMU_step=0;
00078 break;
00079
00080 case 3:
00081 if(data == 0x64)
00082 IMU_step++;
00083 else
00084 IMU_step=0;
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;
00093 payload_counter = 0;
00094 ck_a = 0;
00095 ck_b = 0;
00096
00097 }
00098 break;
00099
00100 case 5:
00101 message_num = data;
00102 checksum(data);
00103 IMU_step++;
00104 break;
00105
00106 case 6:
00107
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;
00118 IMU_step++;
00119 break;
00120
00121 case 8:
00122 IMU_ck_b = data;
00123
00124
00125
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
00138 } else {
00139 Serial.print("Invalid message number = ");
00140 Serial.println(message_num,DEC);
00141 }
00142 } else {
00143 Serial.println("XXX Checksum error");
00144
00145 }
00146
00147 IMU_step = 0;
00148 payload_counter = 0;
00149 ck_a = 0;
00150 ck_b = 0;
00151 IMU_timer = millis();
00152 break;
00153 }
00154 }
00155 }
00156
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
00175
00176
00177
00178 intUnion.byte[0] = buffer[j++];
00179 intUnion.byte[1] = buffer[j++];
00180 roll_sensor = intUnion.word;
00181
00182
00183 intUnion.byte[0] = buffer[j++];
00184 intUnion.byte[1] = buffer[j++];
00185 pitch_sensor = intUnion.word;
00186
00187
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
00199 intUnion.byte[0] = buffer[j++];
00200 intUnion.byte[1] = buffer[j++];
00201 roll_sensor = intUnion.word;
00202
00203
00204 intUnion.byte[0] = buffer[j++];
00205 intUnion.byte[1] = buffer[j++];
00206 pitch_sensor = intUnion.word;
00207
00208
00209 intUnion.byte[0] = buffer[j++];
00210 intUnion.byte[1] = buffer[j++];
00211 Ground_Course = (unsigned int)intUnion.word;
00212
00213
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
00225 int j = 0;
00226
00227 Longitude = join_4_bytes(&buffer[j]);
00228 j += 4;
00229
00230 Lattitude = join_4_bytes(&buffer[j]);
00231 j += 4;
00232
00233
00234 intUnion.byte[0] = buffer[j++];
00235 intUnion.byte[1] = buffer[j++];
00236 Altitude = (long)intUnion.word * 10;
00237
00238
00239 intUnion.byte[0] = buffer[j++];
00240 intUnion.byte[1] = buffer[j++];
00241 Speed_3d = Ground_Speed = (float)intUnion.word;
00242
00243
00244 j += 2;
00245 Time = join_4_bytes(&buffer[j]);
00246 j +=4;
00247 imu_health = buffer[j++];
00248
00249 NewData = 1;
00250 Fix = 1;
00251
00252 }
00253
00254
00255
00256
00257
00258
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
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;