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