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