Nathan Ewin
Prints signals from UM6 orientation sensor (CH Robotics) and GPS receiver, using MODSerial and MODgps. GPS causes UM6 signals to freeze on a fixed value, which timer and gps continue to print out.
Dependencies: MODGPS MODSERIAL mbed
Embed:
(wiki syntax)
Show/hide line numbers
UM6_config.h
00001 /* ------------------------------------------------------------------------------ 00002 File: UM6_config.h 00003 Author: CH Robotics, edited by LHiggs, & Nathan Ewin 00004 Version: 1.0 00005 00006 Description: Preprocessor definitions and function declarations for UM6 configuration 00007 00008 // added configuration for GPS signals from LS20031 sensor connected to UM6 00009 // GPS ground speed and heading angle setup outputs data 00010 // GPS latitude and longitude not setup correctly 00011 ------------------------------------------------------------------------------ */ 00012 #ifndef __UM6_CONFIG_H 00013 #define __UM6_CONFIG_H 00014 00015 #include "UM6_usart.h" 00016 00017 00018 00019 MODSERIAL um6_uart(p13, p14); // UM6 SERIAL OVER UART PINS 26 & 25 00020 00021 00022 00023 00024 00025 00026 00027 00028 // CONFIG_ARRAY_SIZE and DATA_ARRAY_SIZE specify the number of 32 bit configuration and data registers used by the firmware 00029 // (Note: The term "register" is used loosely here. These "registers" are not actually registers in the same sense of a 00030 // microcontroller register. They are simply index locations into arrays stored in global memory. Data and configuration 00031 // parameters are stored in arrays because it allows a common communication protocol to be used to access all data and 00032 // configuration. The software communicating with the sensor needs only specify the register address, and the communication 00033 // software running on the sensor knows exactly where to find it - it needn't know what the data is. The software communicatin 00034 // with the sensor, on the other hand, needs to know what it is asking for (naturally...) 00035 // This setup makes it easy to make more data immediately available when needed - simply increase the array size, add code in 00036 // the firmware that writes data to the new array location, and then make updates to the firmware definition on the PC side. 00037 #define CONFIG_ARRAY_SIZE 44 00038 #define DATA_ARRAY_SIZE 13 00039 #define COMMAND_COUNT 9 00040 00041 // original data array size 32 00042 // 00043 #define CONFIG_REG_START_ADDRESS 0 00044 #define DATA_REG_START_ADDRESS 85 00045 #define COMMAND_START_ADDRESS 170 00046 00047 // hex 0x55 = dec 85 00048 // hex 0xAA = dec 170 00049 // These preprocessor definitions make it easier to access specific configuration parameters in code 00050 // They specify array locations associated with each register name. Note that in the comments below, many of the values are 00051 // said to be 32-bit IEEE floating point. Obviously this isn't directly the case, since the arrays are actually 32-bit unsigned 00052 // integer arrays. Bit for bit, the data does correspond to the correct floating point value. Since you can't cast ints as floats, 00053 // special conversion has to happen to copy the float data to and from the array. 00054 // Starting with configuration register locations... 00055 00056 00057 // Now for data register locations. 00058 // In the communication protocol, data registers are labeled with number ranging from 128 to 255. 00059 // The value of 128 will be subtracted from these numbers 00060 // to produce the actual array index labeled below 00061 #define UM6_STATUS DATA_REG_START_ADDRESS // Status register defines error codes with individual bits 00062 #define UM6_GYRO_RAW_XY (DATA_REG_START_ADDRESS + 1) // Raw gyro data is stored in 16-bit signed integers 00063 #define UM6_GYRO_RAW_Z (DATA_REG_START_ADDRESS + 2) 00064 #define UM6_ACCEL_RAW_XY (DATA_REG_START_ADDRESS + 3) // Raw accel data is stored in 16-bit signed integers 00065 #define UM6_ACCEL_RAW_Z (DATA_REG_START_ADDRESS + 4) 00066 //#define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers 00067 //#define UM6_MAG_RAW_Z (DATA_REG_START_ADDRESS + 6) 00068 #define UM6_GYRO_PROC_XY (DATA_REG_START_ADDRESS + 7) // Processed gyro data has scale factors applied and alignment correction performed. Data is 16-bit signed integer. 00069 #define UM6_GYRO_PROC_Z (DATA_REG_START_ADDRESS + 8) 00070 #define UM6_ACCEL_PROC_XY (DATA_REG_START_ADDRESS + 9) // Processed accel data has scale factors applied and alignment correction performed. Data is 16-bit signed integer. 00071 #define UM6_ACCEL_PROC_Z (DATA_REG_START_ADDRESS + 10) 00072 //#define UM6_MAG_PROC_XY (DATA_REG_START_ADDRESS + 11) // Processed mag data has scale factors applied and alignment correction performed. Data is 16-bit signed integer. 00073 //#define UM6_MAG_PROC_Z (DATA_REG_START_ADDRESS + 12) 00074 #define UM6_EULER_PHI_THETA (DATA_REG_START_ADDRESS + 13) // Euler angles are 32-bit IEEE floating point 00075 #define UM6_EULER_PSI (DATA_REG_START_ADDRESS + 14) 00076 #define UM6_QUAT_AB (DATA_REG_START_ADDRESS + 15) // Quaternions are 16-bit signed integers. 00077 #define UM6_QUAT_CD (DATA_REG_START_ADDRESS + 16) 00078 /* 00079 #define UM6_ERROR_COV_00 (DATA_REG_START_ADDRESS + 17) // Error covariance is a 4x4 matrix of 32-bit IEEE floating point values 00080 #define UM6_ERROR_COV_01 (DATA_REG_START_ADDRESS + 18) 00081 #define UM6_ERROR_COV_02 (DATA_REG_START_ADDRESS + 19) 00082 #define UM6_ERROR_COV_03 (DATA_REG_START_ADDRESS + 20) 00083 #define UM6_ERROR_COV_10 (DATA_REG_START_ADDRESS + 21) 00084 #define UM6_ERROR_COV_11 (DATA_REG_START_ADDRESS + 22) 00085 #define UM6_ERROR_COV_12 (DATA_REG_START_ADDRESS + 23) 00086 #define UM6_ERROR_COV_13 (DATA_REG_START_ADDRESS + 24) 00087 #define UM6_ERROR_COV_20 (DATA_REG_START_ADDRESS + 25) 00088 #define UM6_ERROR_COV_21 (DATA_REG_START_ADDRESS + 26) 00089 #define UM6_ERROR_COV_22 (DATA_REG_START_ADDRESS + 27) 00090 #define UM6_ERROR_COV_23 (DATA_REG_START_ADDRESS + 28) 00091 #define UM6_ERROR_COV_30 (DATA_REG_START_ADDRESS + 29) 00092 #define UM6_ERROR_COV_31 (DATA_REG_START_ADDRESS + 30) 00093 #define UM6_ERROR_COV_32 (DATA_REG_START_ADDRESS + 31) 00094 #define UM6_ERROR_COV_33 (DATA_REG_START_ADDRESS + 32) 00095 */ 00096 // connected gps module directing to mbed using MODGPS 00097 //#define UM6_GPS_LONGITUDE (DATA_REG_START_ADDRESS + 34) 00098 //#define UM6_GPS_LATITUDE (DATA_REG_START_ADDRESS + 35) 00099 //#define UM6_GPS_ALTITUDE (DATA_REG_START_ADDRESS + 36) 00100 //#define UM6_GPS_COURSE_SPEED (DATA_REG_START_ADDRESS + 40) 00101 00102 00103 00104 00105 00106 00107 00108 /******************************************************************************* 00109 * Function Name : ComputeChecksum 00110 * Input : USARTPacket* new_packet 00111 * Output : None 00112 * Return : uint16_t 00113 * Description : Returns the two byte sum of all the individual bytes in the 00114 given packet. 00115 *******************************************************************************/ 00116 uint16_t ComputeChecksum( USARTPacket* new_packet ) { 00117 int32_t index; 00118 uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address; 00119 00120 for ( index = 0; index < new_packet->data_length; index++ ) { 00121 checksum += new_packet->packet_data[index]; 00122 } 00123 return checksum; 00124 } 00125 00126 00127 00128 00129 00130 static USARTPacket new_packet; 00131 00132 // Flag for storing the current USART state 00133 uint8_t gUSART_State = USART_STATE_WAIT; 00134 00135 00136 struct UM6{ 00137 float Gyro_Proc_X; 00138 float Gyro_Proc_Y; 00139 float Gyro_Proc_Z; 00140 float Accel_Proc_X; 00141 float Accel_Proc_Y; 00142 float Accel_Proc_Z; 00143 //float Mag_Proc_X; 00144 //float Mag_Proc_Y; 00145 //float Mag_Proc_Z; 00146 float Roll; 00147 float Pitch; 00148 float Yaw; 00149 //float GPS_long; 00150 //float GPS_lat; 00151 //float GPS_alt; 00152 //float GPS_course; 00153 //float GPS_speed; 00154 00155 }; 00156 UM6 data; 00157 00158 00159 00160 00161 void Process_um6_packet() { 00162 00163 int16_t MY_DATA_GYRO_PROC_X; 00164 int16_t MY_DATA_GYRO_PROC_Y; 00165 int16_t MY_DATA_GYRO_PROC_Z; 00166 int16_t MY_DATA_ACCEL_PROC_X; 00167 int16_t MY_DATA_ACCEL_PROC_Y; 00168 int16_t MY_DATA_ACCEL_PROC_Z; 00169 //int16_t MY_DATA_MAG_PROC_X; 00170 //int16_t MY_DATA_MAG_PROC_Y; 00171 //int16_t MY_DATA_MAG_PROC_Z; 00172 int16_t MY_DATA_EULER_PHI; 00173 int16_t MY_DATA_EULER_THETA; 00174 int16_t MY_DATA_EULER_PSI; 00175 /* 00176 int32_t MY_DATA_GPS_LONG; 00177 int32_t MY_DATA_GPS_LONG0; 00178 int32_t MY_DATA_GPS_LONG1; 00179 int32_t MY_DATA_GPS_LONG2; 00180 int32_t MY_DATA_GPS_LAT; 00181 int32_t MY_DATA_GPS_LAT0; 00182 int32_t MY_DATA_GPS_LAT1; 00183 int32_t MY_DATA_GPS_LAT2; 00184 int32_t MY_DATA_GPS_ALT; 00185 int32_t MY_DATA_GPS_ALT0; 00186 int32_t MY_DATA_GPS_ALT1; 00187 int32_t MY_DATA_GPS_ALT2; 00188 int16_t MY_DATA_GPS_COURSE; 00189 int16_t MY_DATA_GPS_SPEED; 00190 */ 00191 00192 00193 static uint8_t data_counter = 0; 00194 00195 00196 00197 // The next action should depend on the USART state. 00198 switch ( gUSART_State ) { 00199 // USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes 00200 // that signals a new incoming packet. 00201 case USART_STATE_WAIT: 00202 if ( data_counter == 0 ) { // Waiting on 's' character 00203 if ( um6_uart.getc() == 's' ) { 00204 00205 data_counter++; 00206 } else { 00207 data_counter = 0; 00208 } 00209 } else if ( data_counter == 1 ) { // Waiting on 'n' character 00210 if ( um6_uart.getc() == 'n' ) { 00211 data_counter++; 00212 00213 } else { 00214 data_counter = 0; 00215 } 00216 } else if ( data_counter == 2 ) { // Waiting on 'p' character 00217 if ( um6_uart.getc() == 'p' ) { 00218 // The full 'snp' sequence was received. Reset data_counter (it will be used again 00219 // later) and transition to the next state. 00220 data_counter = 0; 00221 gUSART_State = USART_STATE_TYPE; 00222 00223 } else { 00224 data_counter = 0; 00225 } 00226 } 00227 break; 00228 00229 // USART in the TYPE state. In this state, the USART has just received the sequence of bytes that 00230 // indicates a new packet is about to arrive. Now, the USART expects to see the packet type. 00231 case USART_STATE_TYPE: 00232 00233 new_packet.PT = um6_uart.getc(); 00234 00235 gUSART_State = USART_STATE_ADDRESS; 00236 00237 break; 00238 00239 // USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating 00240 // the address that the packet applies to 00241 case USART_STATE_ADDRESS: 00242 new_packet.address = um6_uart.getc(); 00243 00244 // For convenience, identify the type of packet this is and copy to the packet structure 00245 // (this will be used by the packet handler later) 00246 if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) { 00247 new_packet.address_type = ADDRESS_TYPE_CONFIG; 00248 } else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) { 00249 new_packet.address_type = ADDRESS_TYPE_DATA; 00250 } else { 00251 new_packet.address_type = ADDRESS_TYPE_COMMAND; 00252 } 00253 00254 // Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command) 00255 // If this is a read operation, jump directly to the USART_STATE_CHECKSUM state - there is no more data in the packet 00256 if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) { 00257 gUSART_State = USART_STATE_CHECKSUM; 00258 } 00259 00260 // If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data 00261 else { 00262 gUSART_State = USART_STATE_DATA; 00263 // Determine the expected number of bytes in this data packet based on the packet type. A write operation 00264 // consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size, 00265 // where the batch size is also given in the packet type byte. 00266 if ( new_packet.PT & PACKET_IS_BATCH ) { 00267 new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK); 00268 00269 } else { 00270 new_packet.data_length = 4; 00271 } 00272 } 00273 break; 00274 00275 // USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of 00276 // data. 00277 case USART_STATE_DATA: 00278 new_packet.packet_data[data_counter] = um6_uart.getc(); 00279 data_counter++; 00280 00281 // If the expected number of bytes has been received, transition to the CHECKSUM state. 00282 if ( data_counter == new_packet.data_length ) { 00283 // Reset data_counter, since it will be used in the CHECKSUM state. 00284 data_counter = 0; 00285 gUSART_State = USART_STATE_CHECKSUM; 00286 } 00287 break; 00288 00289 00290 00291 // USART in CHECKSUM state. In this state, the entire packet has been received, with the exception 00292 // of the 16-bit checksum. 00293 case USART_STATE_CHECKSUM: 00294 // Get the highest-order byte 00295 if ( data_counter == 0 ) { 00296 new_packet.checksum = ((uint16_t)um6_uart.getc() << 8); 00297 data_counter++; 00298 } else { // ( data_counter == 1 ) 00299 // Get lower-order byte 00300 new_packet.checksum = new_packet.checksum | ((uint16_t)um6_uart.getc() & 0x0FF); 00301 00302 // Both checksum bytes have been received. Make sure that the checksum is valid. 00303 uint16_t checksum = ComputeChecksum( &new_packet ); 00304 00305 00306 00307 // If checksum does not match, exit function 00308 if ( checksum != new_packet.checksum ) { 00309 return; 00310 } // end if(checksum check) 00311 00312 00313 00314 else 00315 00316 { 00317 00318 // Packet was received correctly. 00319 00320 //----------------------------------------------------------------------------------------------- 00321 //----------------------------------------------------------------------------------------------- 00322 // 00323 // CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!! 00324 00325 00326 // IF DATA ADDRESS 00327 if (new_packet.address_type == ADDRESS_TYPE_DATA) { 00328 00329 //------------------------------------------------------------ 00330 // UM6_GYRO_PROC_XY (0x5C) 00331 // To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees 00332 // per second, the data should be multiplied by the scale factor 0.0610352 as shown below 00333 // angular_rate = register_data*0.0610352 00334 00335 if (new_packet.address == UM6_GYRO_PROC_XY) { 00336 00337 // GYRO_PROC_X 00338 MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it 00339 MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1]; 00340 data.Gyro_Proc_X = MY_DATA_GYRO_PROC_X*0.0610352; 00341 00342 MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it 00343 MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3]; 00344 data.Gyro_Proc_Y = MY_DATA_GYRO_PROC_Y*0.0610352; 00345 00346 00347 //------------------------------------------------------------ 00348 00349 00350 //------------------------------------------------------------ 00351 // UM6_GYRO_PROC_Z (0x5D) 00352 // To convert the register data from a 16-bit 2's complement integer to the actual angular rate in 00353 // degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below. 00354 00355 00356 // GYRO_PROC_Z 00357 MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it 00358 MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];; 00359 data.Gyro_Proc_Z = MY_DATA_GYRO_PROC_Z*0.0610352; 00360 00361 00362 } // end if(MY_DATA_GYRO_PROC) 00363 //------------------------------------------------------------ 00364 00365 00366 //------------------------------------------------------------ 00367 // UM6_ACCEL_PROC_XY (0x5E) 00368 // To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities, 00369 // the data should be multiplied by the scale factor 0.000183105 as shown below. 00370 // acceleration = register_data* 0.000183105 00371 if (new_packet.address == UM6_ACCEL_PROC_XY) { 00372 00373 // ACCEL_PROC_X 00374 MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it 00375 MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1]; 00376 data.Accel_Proc_X = MY_DATA_ACCEL_PROC_X*0.000183105; 00377 00378 00379 // ACCEL_PROC_Y 00380 MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it 00381 MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3]; 00382 data.Accel_Proc_Y = MY_DATA_ACCEL_PROC_Y*0.000183105; 00383 00384 //------------------------------------------------------------ 00385 00386 00387 //------------------------------------------------------------ 00388 // UM6_ACCEL_PROC_Z (0x5F) 00389 // To convert the register data from a 16-bit 2's complement integer to the actual acceleration in 00390 // gravities, the data should be multiplied by the scale factor 0.000183105 as shown below. 00391 00392 // ACCEL_PROC_Z 00393 MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it 00394 MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5]; 00395 data.Accel_Proc_Z = MY_DATA_ACCEL_PROC_Z*0.000183105; 00396 00397 } // end if(MY_DATA_ACCEL_PROC) 00398 00399 //------------------------------------------------------------ 00400 00401 00402 //------------------------------------------------------------ 00403 // UM6_MAG_PROC_XY (0x60) 00404 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper 00405 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as 00406 // shown below. 00407 // magnetic field = register_data* 0.000305176 00408 /* 00409 if (new_packet.address == UM6_MAG_PROC_XY) { 00410 00411 // MAG_PROC_X 00412 MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it 00413 MY_DATA_MAG_PROC_X |= new_packet.packet_data[1]; 00414 data.Mag_Proc_X = MY_DATA_MAG_PROC_X*0.000305176; 00415 00416 00417 // MAG_PROC_Y 00418 MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it 00419 MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3]; 00420 data.Mag_Proc_Y = MY_DATA_MAG_PROC_Y*0.000305176; 00421 00422 //------------------------------------------------------------ 00423 00424 00425 //------------------------------------------------------------ 00426 // UM6_MAG_PROC_Z (0x61) 00427 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper 00428 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as 00429 // shown below. 00430 // magnetic field = register_data*0.000305176 00431 00432 // MAG_PROC_Z 00433 MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it 00434 MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5]; 00435 data.Mag_Proc_Z = MY_DATA_MAG_PROC_Z*0.000305176; 00436 00437 } // end if(UM6_MAG_PROC) 00438 */ 00439 //------------------------------------------------------------ 00440 00441 00442 00443 //------------------------------------------------------------ 00444 // UM6_EULER_PHI_THETA (0x62) 00445 // Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle 00446 // estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in 00447 // degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below 00448 // angle estimate = register_data* 0.0109863 00449 if (new_packet.address == UM6_EULER_PHI_THETA) { 00450 // EULER_PHI (ROLL) 00451 MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it 00452 MY_DATA_EULER_PHI |= new_packet.packet_data[1]; 00453 data.Roll = MY_DATA_EULER_PHI*0.0109863; 00454 00455 00456 00457 00458 00459 // EULER_THETA (PITCH) 00460 MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it 00461 MY_DATA_EULER_THETA |= new_packet.packet_data[3]; 00462 data.Pitch = MY_DATA_EULER_THETA*0.0109863; 00463 00464 //------------------------------------------------------------ 00465 00466 //------------------------------------------------------------ 00467 // UM6_EULER_PSI (0x63) (YAW) 00468 // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16- 00469 // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data 00470 // should be multiplied by the scale factor 0.0109863 as shown below 00471 00472 MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it 00473 MY_DATA_EULER_PSI |= new_packet.packet_data[5]; 00474 data.Yaw = MY_DATA_EULER_PSI*0.0109863; 00475 00476 00477 } 00478 /* 00479 //------------------------------------------------------------------- 00480 // GPS Ground/Speed 00481 if (new_packet.address == UM6_GPS_COURSE_SPEED) { 00482 // Ground course 00483 MY_DATA_GPS_COURSE = (int16_t)new_packet.packet_data[0]<<8; //bitshift it 00484 MY_DATA_GPS_COURSE |= new_packet.packet_data[1]; 00485 data.GPS_course = MY_DATA_GPS_COURSE*0.01; // scaled by 0.01 to get ground course in degrees 00486 00487 // Speed 00488 MY_DATA_GPS_SPEED = (int16_t)new_packet.packet_data[2]<<8; //bitshift it 00489 MY_DATA_GPS_SPEED |= new_packet.packet_data[3]; 00490 data.GPS_speed = MY_DATA_GPS_SPEED*0.01; // scaled by 0.01 to get speed in m/s 00491 00492 00493 //------------------------------------------------------------ 00494 } 00495 //------------------------------------------------------------------- 00496 // GPS long 00497 if (new_packet.address == UM6_GPS_LONGITUDE) { 00498 // Longitude 00499 MY_DATA_GPS_LONG0 = (int32_t)new_packet.packet_data[0]<<24; 00500 MY_DATA_GPS_LONG1 = (int32_t)new_packet.packet_data[1]<<16; 00501 MY_DATA_GPS_LONG2 = (int32_t)new_packet.packet_data[2]<<8; 00502 MY_DATA_GPS_LONG = MY_DATA_GPS_LONG0|MY_DATA_GPS_LONG1|MY_DATA_GPS_LONG2|new_packet.packet_data[3]; 00503 memcpy(&data.GPS_long,&MY_DATA_GPS_LONG,sizeof(int)); 00504 } 00505 //------------------------------------------------------------ 00506 //------------------------------------------------------------------- 00507 // GPS lat 00508 if (new_packet.address == UM6_GPS_LATITUDE) { 00509 // Latitude 00510 //MY_DATA_GPS_LAT0 = (int32_t)new_packet.packet_data[0]<<24; 00511 //MY_DATA_GPS_LAT1 = (int32_t)new_packet.packet_data[1]<<16; 00512 //MY_DATA_GPS_LAT2 = (int32_t)new_packet.packet_data[2]<<8; 00513 //MY_DATA_GPS_LAT = MY_DATA_GPS_LAT0|MY_DATA_GPS_LAT1|MY_DATA_GPS_LAT2|new_packet.packet_data[3]; 00514 //memcpy(&data.GPS_lat,&MY_DATA_GPS_LAT,sizeof(int)); 00515 // data.GPS_lat_raw = new_packet.packet_data[0]; 00516 } 00517 //------------------------------------------------------------ 00518 //------------------------------------------------------------------- 00519 // GPS altitude 00520 if (new_packet.address == UM6_GPS_ALTITUDE) { 00521 // Longitude 00522 MY_DATA_GPS_ALT0 = (int32_t)new_packet.packet_data[0]<<24; 00523 MY_DATA_GPS_ALT1 = (int32_t)new_packet.packet_data[1]<<16; 00524 MY_DATA_GPS_ALT2 = (int32_t)new_packet.packet_data[2]<<8; 00525 MY_DATA_GPS_ALT = MY_DATA_GPS_ALT0|MY_DATA_GPS_ALT1|MY_DATA_GPS_ALT2|new_packet.packet_data[3]; 00526 memcpy(&data.GPS_alt,&MY_DATA_GPS_ALT,sizeof(int)); 00527 // data.GPS_alt_raw = MY_DATA_GPS_ALT0; 00528 00529 } 00530 */ //------------------------------------------------------------ 00531 } // end if(ADDRESS_TYPE_DATA) 00532 00533 00534 // A full packet has been received. 00535 // Put the USART back into the WAIT state and reset 00536 // the data_counter variable so that it can be used to receive the next packet. 00537 data_counter = 0; 00538 gUSART_State = USART_STATE_WAIT; 00539 00540 00541 } // end else(GET_DATA) 00542 00543 } 00544 break; 00545 00546 } // end switch ( gUSART_State ) 00547 00548 return; 00549 00550 } // end get_gyro_x() 00551 00552 #endif 00553
Generated on Thu Jul 14 2022 07:35:42 by
1.7.2