lhiggs CSUM / Mbed 2 deprecated UM6_IMU_AHRS_2012

Communication program for the chrobotics UM6 9-DOF IMU AHRS.

Dependencies:   MODSERIAL mbed

UM6_config/UM6_config.h@0:03c649c76388, 2012-09-28 (annotated)

Committer:
lhiggs
Date:
Fri Sep 28 00:40:29 2012 +0000
Revision:
0:03c649c76388
A UM6 IMU AHRS INTERFACE

Who changed what in which revision?

UserRevisionLine numberNew contents of line
lhiggs 0:03c649c76388 1 /* ------------------------------------------------------------------------------
lhiggs 0:03c649c76388 2 File: UM6_config.h
lhiggs 0:03c649c76388 3 Author: CH Robotics
lhiggs 0:03c649c76388 4 Version: 1.0
lhiggs 0:03c649c76388 5
lhiggs 0:03c649c76388 6 Description: Preprocessor definitions and function declarations for UM6 configuration
lhiggs 0:03c649c76388 7 ------------------------------------------------------------------------------ */
lhiggs 0:03c649c76388 8 #ifndef __UM6_CONFIG_H
lhiggs 0:03c649c76388 9 #define __UM6_CONFIG_H
lhiggs 0:03c649c76388 10
lhiggs 0:03c649c76388 11 #include "UM6_usart.h"
lhiggs 0:03c649c76388 12
lhiggs 0:03c649c76388 13
lhiggs 0:03c649c76388 14
lhiggs 0:03c649c76388 15 MODSERIAL um6_uart(p26, p25); // UM6 SERIAL OVER UART PINS 26 & 25
lhiggs 0:03c649c76388 16
lhiggs 0:03c649c76388 17
lhiggs 0:03c649c76388 18
lhiggs 0:03c649c76388 19
lhiggs 0:03c649c76388 20
lhiggs 0:03c649c76388 21
lhiggs 0:03c649c76388 22
lhiggs 0:03c649c76388 23
lhiggs 0:03c649c76388 24 // CONFIG_ARRAY_SIZE and DATA_ARRAY_SIZE specify the number of 32 bit configuration and data registers used by the firmware
lhiggs 0:03c649c76388 25 // (Note: The term "register" is used loosely here. These "registers" are not actually registers in the same sense of a
lhiggs 0:03c649c76388 26 // microcontroller register. They are simply index locations into arrays stored in global memory. Data and configuration
lhiggs 0:03c649c76388 27 // parameters are stored in arrays because it allows a common communication protocol to be used to access all data and
lhiggs 0:03c649c76388 28 // configuration. The software communicating with the sensor needs only specify the register address, and the communication
lhiggs 0:03c649c76388 29 // software running on the sensor knows exactly where to find it - it needn't know what the data is. The software communicatin
lhiggs 0:03c649c76388 30 // with the sensor, on the other hand, needs to know what it is asking for (naturally...)
lhiggs 0:03c649c76388 31 // This setup makes it easy to make more data immediately available when needed - simply increase the array size, add code in
lhiggs 0:03c649c76388 32 // the firmware that writes data to the new array location, and then make updates to the firmware definition on the PC side.
lhiggs 0:03c649c76388 33 #define CONFIG_ARRAY_SIZE 44
lhiggs 0:03c649c76388 34 #define DATA_ARRAY_SIZE 33
lhiggs 0:03c649c76388 35 #define COMMAND_COUNT 9
lhiggs 0:03c649c76388 36
lhiggs 0:03c649c76388 37 //
lhiggs 0:03c649c76388 38 #define CONFIG_REG_START_ADDRESS 0
lhiggs 0:03c649c76388 39 #define DATA_REG_START_ADDRESS 85
lhiggs 0:03c649c76388 40 #define COMMAND_START_ADDRESS 170
lhiggs 0:03c649c76388 41
lhiggs 0:03c649c76388 42 // These preprocessor definitions make it easier to access specific configuration parameters in code
lhiggs 0:03c649c76388 43 // They specify array locations associated with each register name. Note that in the comments below, many of the values are
lhiggs 0:03c649c76388 44 // said to be 32-bit IEEE floating point. Obviously this isn't directly the case, since the arrays are actually 32-bit unsigned
lhiggs 0:03c649c76388 45 // integer arrays. Bit for bit, the data does correspond to the correct floating point value. Since you can't cast ints as floats,
lhiggs 0:03c649c76388 46 // special conversion has to happen to copy the float data to and from the array.
lhiggs 0:03c649c76388 47 // Starting with configuration register locations...
lhiggs 0:03c649c76388 48
lhiggs 0:03c649c76388 49
lhiggs 0:03c649c76388 50 // Now for data register locations.
lhiggs 0:03c649c76388 51 // 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
lhiggs 0:03c649c76388 52 // to produce the actual array index labeled below
lhiggs 0:03c649c76388 53 #define UM6_STATUS DATA_REG_START_ADDRESS // Status register defines error codes with individual bits
lhiggs 0:03c649c76388 54 #define UM6_GYRO_RAW_XY (DATA_REG_START_ADDRESS + 1) // Raw gyro data is stored in 16-bit signed integers
lhiggs 0:03c649c76388 55 #define UM6_GYRO_RAW_Z (DATA_REG_START_ADDRESS + 2)
lhiggs 0:03c649c76388 56 #define UM6_ACCEL_RAW_XY (DATA_REG_START_ADDRESS + 3) // Raw accel data is stored in 16-bit signed integers
lhiggs 0:03c649c76388 57 #define UM6_ACCEL_RAW_Z (DATA_REG_START_ADDRESS + 4)
lhiggs 0:03c649c76388 58 #define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers
lhiggs 0:03c649c76388 59 #define UM6_MAG_RAW_Z (DATA_REG_START_ADDRESS + 6)
lhiggs 0:03c649c76388 60 #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.
lhiggs 0:03c649c76388 61 #define UM6_GYRO_PROC_Z (DATA_REG_START_ADDRESS + 8)
lhiggs 0:03c649c76388 62 #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.
lhiggs 0:03c649c76388 63 #define UM6_ACCEL_PROC_Z (DATA_REG_START_ADDRESS + 10)
lhiggs 0:03c649c76388 64 #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.
lhiggs 0:03c649c76388 65 #define UM6_MAG_PROC_Z (DATA_REG_START_ADDRESS + 12)
lhiggs 0:03c649c76388 66 #define UM6_EULER_PHI_THETA (DATA_REG_START_ADDRESS + 13) // Euler angles are 32-bit IEEE floating point
lhiggs 0:03c649c76388 67 #define UM6_EULER_PSI (DATA_REG_START_ADDRESS + 14)
lhiggs 0:03c649c76388 68 #define UM6_QUAT_AB (DATA_REG_START_ADDRESS + 15) // Quaternions are 16-bit signed integers.
lhiggs 0:03c649c76388 69 #define UM6_QUAT_CD (DATA_REG_START_ADDRESS + 16)
lhiggs 0:03c649c76388 70 #define UM6_ERROR_COV_00 (DATA_REG_START_ADDRESS + 17) // Error covariance is a 4x4 matrix of 32-bit IEEE floating point values
lhiggs 0:03c649c76388 71 #define UM6_ERROR_COV_01 (DATA_REG_START_ADDRESS + 18)
lhiggs 0:03c649c76388 72 #define UM6_ERROR_COV_02 (DATA_REG_START_ADDRESS + 19)
lhiggs 0:03c649c76388 73 #define UM6_ERROR_COV_03 (DATA_REG_START_ADDRESS + 20)
lhiggs 0:03c649c76388 74 #define UM6_ERROR_COV_10 (DATA_REG_START_ADDRESS + 21)
lhiggs 0:03c649c76388 75 #define UM6_ERROR_COV_11 (DATA_REG_START_ADDRESS + 22)
lhiggs 0:03c649c76388 76 #define UM6_ERROR_COV_12 (DATA_REG_START_ADDRESS + 23)
lhiggs 0:03c649c76388 77 #define UM6_ERROR_COV_13 (DATA_REG_START_ADDRESS + 24)
lhiggs 0:03c649c76388 78 #define UM6_ERROR_COV_20 (DATA_REG_START_ADDRESS + 25)
lhiggs 0:03c649c76388 79 #define UM6_ERROR_COV_21 (DATA_REG_START_ADDRESS + 26)
lhiggs 0:03c649c76388 80 #define UM6_ERROR_COV_22 (DATA_REG_START_ADDRESS + 27)
lhiggs 0:03c649c76388 81 #define UM6_ERROR_COV_23 (DATA_REG_START_ADDRESS + 28)
lhiggs 0:03c649c76388 82 #define UM6_ERROR_COV_30 (DATA_REG_START_ADDRESS + 29)
lhiggs 0:03c649c76388 83 #define UM6_ERROR_COV_31 (DATA_REG_START_ADDRESS + 30)
lhiggs 0:03c649c76388 84 #define UM6_ERROR_COV_32 (DATA_REG_START_ADDRESS + 31)
lhiggs 0:03c649c76388 85 #define UM6_ERROR_COV_33 (DATA_REG_START_ADDRESS + 32)
lhiggs 0:03c649c76388 86
lhiggs 0:03c649c76388 87
lhiggs 0:03c649c76388 88
lhiggs 0:03c649c76388 89
lhiggs 0:03c649c76388 90
lhiggs 0:03c649c76388 91
lhiggs 0:03c649c76388 92
lhiggs 0:03c649c76388 93
lhiggs 0:03c649c76388 94 /*******************************************************************************
lhiggs 0:03c649c76388 95 * Function Name : ComputeChecksum
lhiggs 0:03c649c76388 96 * Input : USARTPacket* new_packet
lhiggs 0:03c649c76388 97 * Output : None
lhiggs 0:03c649c76388 98 * Return : uint16_t
lhiggs 0:03c649c76388 99 * Description : Returns the two byte sum of all the individual bytes in the
lhiggs 0:03c649c76388 100 given packet.
lhiggs 0:03c649c76388 101 *******************************************************************************/
lhiggs 0:03c649c76388 102 uint16_t ComputeChecksum( USARTPacket* new_packet ) {
lhiggs 0:03c649c76388 103 int32_t index;
lhiggs 0:03c649c76388 104 uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address;
lhiggs 0:03c649c76388 105
lhiggs 0:03c649c76388 106 for ( index = 0; index < new_packet->data_length; index++ ) {
lhiggs 0:03c649c76388 107 checksum += new_packet->packet_data[index];
lhiggs 0:03c649c76388 108 }
lhiggs 0:03c649c76388 109 return checksum;
lhiggs 0:03c649c76388 110 }
lhiggs 0:03c649c76388 111
lhiggs 0:03c649c76388 112
lhiggs 0:03c649c76388 113
lhiggs 0:03c649c76388 114
lhiggs 0:03c649c76388 115
lhiggs 0:03c649c76388 116 static USARTPacket new_packet;
lhiggs 0:03c649c76388 117
lhiggs 0:03c649c76388 118 // Flag for storing the current USART state
lhiggs 0:03c649c76388 119 uint8_t gUSART_State = USART_STATE_WAIT;
lhiggs 0:03c649c76388 120
lhiggs 0:03c649c76388 121
lhiggs 0:03c649c76388 122 struct UM6{
lhiggs 0:03c649c76388 123 float Gyro_Proc_X;
lhiggs 0:03c649c76388 124 float Gyro_Proc_Y;
lhiggs 0:03c649c76388 125 float Gyro_Proc_Z;
lhiggs 0:03c649c76388 126 float Accel_Proc_X;
lhiggs 0:03c649c76388 127 float Accel_Proc_Y;
lhiggs 0:03c649c76388 128 float Accel_Proc_Z;
lhiggs 0:03c649c76388 129 float Mag_Proc_X;
lhiggs 0:03c649c76388 130 float Mag_Proc_Y;
lhiggs 0:03c649c76388 131 float Mag_Proc_Z;
lhiggs 0:03c649c76388 132 float Roll;
lhiggs 0:03c649c76388 133 float Pitch;
lhiggs 0:03c649c76388 134 float Yaw;
lhiggs 0:03c649c76388 135 };
lhiggs 0:03c649c76388 136 UM6 data;
lhiggs 0:03c649c76388 137
lhiggs 0:03c649c76388 138
lhiggs 0:03c649c76388 139
lhiggs 0:03c649c76388 140
lhiggs 0:03c649c76388 141 void Process_um6_packet() {
lhiggs 0:03c649c76388 142
lhiggs 0:03c649c76388 143 int16_t MY_DATA_GYRO_PROC_X;
lhiggs 0:03c649c76388 144 int16_t MY_DATA_GYRO_PROC_Y;
lhiggs 0:03c649c76388 145 int16_t MY_DATA_GYRO_PROC_Z;
lhiggs 0:03c649c76388 146 int16_t MY_DATA_ACCEL_PROC_X;
lhiggs 0:03c649c76388 147 int16_t MY_DATA_ACCEL_PROC_Y;
lhiggs 0:03c649c76388 148 int16_t MY_DATA_ACCEL_PROC_Z;
lhiggs 0:03c649c76388 149 int16_t MY_DATA_MAG_PROC_X;
lhiggs 0:03c649c76388 150 int16_t MY_DATA_MAG_PROC_Y;
lhiggs 0:03c649c76388 151 int16_t MY_DATA_MAG_PROC_Z;
lhiggs 0:03c649c76388 152 int16_t MY_DATA_EULER_PHI;
lhiggs 0:03c649c76388 153 int16_t MY_DATA_EULER_THETA;
lhiggs 0:03c649c76388 154 int16_t MY_DATA_EULER_PSI;
lhiggs 0:03c649c76388 155
lhiggs 0:03c649c76388 156
lhiggs 0:03c649c76388 157
lhiggs 0:03c649c76388 158 static uint8_t data_counter = 0;
lhiggs 0:03c649c76388 159
lhiggs 0:03c649c76388 160
lhiggs 0:03c649c76388 161
lhiggs 0:03c649c76388 162 // The next action should depend on the USART state.
lhiggs 0:03c649c76388 163 switch ( gUSART_State ) {
lhiggs 0:03c649c76388 164 // USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes
lhiggs 0:03c649c76388 165 // that signals a new incoming packet.
lhiggs 0:03c649c76388 166 case USART_STATE_WAIT:
lhiggs 0:03c649c76388 167 if ( data_counter == 0 ) { // Waiting on 's' character
lhiggs 0:03c649c76388 168 if ( um6_uart.getc() == 's' ) {
lhiggs 0:03c649c76388 169
lhiggs 0:03c649c76388 170 data_counter++;
lhiggs 0:03c649c76388 171 } else {
lhiggs 0:03c649c76388 172 data_counter = 0;
lhiggs 0:03c649c76388 173 }
lhiggs 0:03c649c76388 174 } else if ( data_counter == 1 ) { // Waiting on 'n' character
lhiggs 0:03c649c76388 175 if ( um6_uart.getc() == 'n' ) {
lhiggs 0:03c649c76388 176 data_counter++;
lhiggs 0:03c649c76388 177
lhiggs 0:03c649c76388 178 } else {
lhiggs 0:03c649c76388 179 data_counter = 0;
lhiggs 0:03c649c76388 180 }
lhiggs 0:03c649c76388 181 } else if ( data_counter == 2 ) { // Waiting on 'p' character
lhiggs 0:03c649c76388 182 if ( um6_uart.getc() == 'p' ) {
lhiggs 0:03c649c76388 183 // The full 'snp' sequence was received. Reset data_counter (it will be used again
lhiggs 0:03c649c76388 184 // later) and transition to the next state.
lhiggs 0:03c649c76388 185 data_counter = 0;
lhiggs 0:03c649c76388 186 gUSART_State = USART_STATE_TYPE;
lhiggs 0:03c649c76388 187
lhiggs 0:03c649c76388 188 } else {
lhiggs 0:03c649c76388 189 data_counter = 0;
lhiggs 0:03c649c76388 190 }
lhiggs 0:03c649c76388 191 }
lhiggs 0:03c649c76388 192 break;
lhiggs 0:03c649c76388 193
lhiggs 0:03c649c76388 194 // USART in the TYPE state. In this state, the USART has just received the sequence of bytes that
lhiggs 0:03c649c76388 195 // indicates a new packet is about to arrive. Now, the USART expects to see the packet type.
lhiggs 0:03c649c76388 196 case USART_STATE_TYPE:
lhiggs 0:03c649c76388 197
lhiggs 0:03c649c76388 198 new_packet.PT = um6_uart.getc();
lhiggs 0:03c649c76388 199
lhiggs 0:03c649c76388 200 gUSART_State = USART_STATE_ADDRESS;
lhiggs 0:03c649c76388 201
lhiggs 0:03c649c76388 202 break;
lhiggs 0:03c649c76388 203
lhiggs 0:03c649c76388 204 // USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating
lhiggs 0:03c649c76388 205 // the address that the packet applies to
lhiggs 0:03c649c76388 206 case USART_STATE_ADDRESS:
lhiggs 0:03c649c76388 207 new_packet.address = um6_uart.getc();
lhiggs 0:03c649c76388 208
lhiggs 0:03c649c76388 209 // For convenience, identify the type of packet this is and copy to the packet structure
lhiggs 0:03c649c76388 210 // (this will be used by the packet handler later)
lhiggs 0:03c649c76388 211 if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) {
lhiggs 0:03c649c76388 212 new_packet.address_type = ADDRESS_TYPE_CONFIG;
lhiggs 0:03c649c76388 213 } else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) {
lhiggs 0:03c649c76388 214 new_packet.address_type = ADDRESS_TYPE_DATA;
lhiggs 0:03c649c76388 215 } else {
lhiggs 0:03c649c76388 216 new_packet.address_type = ADDRESS_TYPE_COMMAND;
lhiggs 0:03c649c76388 217 }
lhiggs 0:03c649c76388 218
lhiggs 0:03c649c76388 219 // Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command)
lhiggs 0:03c649c76388 220 // If this is a read operation, jump directly to the USART_STATE_CHECKSUM state - there is no more data in the packet
lhiggs 0:03c649c76388 221 if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) {
lhiggs 0:03c649c76388 222 gUSART_State = USART_STATE_CHECKSUM;
lhiggs 0:03c649c76388 223 }
lhiggs 0:03c649c76388 224
lhiggs 0:03c649c76388 225 // If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data
lhiggs 0:03c649c76388 226 else {
lhiggs 0:03c649c76388 227 gUSART_State = USART_STATE_DATA;
lhiggs 0:03c649c76388 228 // Determine the expected number of bytes in this data packet based on the packet type. A write operation
lhiggs 0:03c649c76388 229 // consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size,
lhiggs 0:03c649c76388 230 // where the batch size is also given in the packet type byte.
lhiggs 0:03c649c76388 231 if ( new_packet.PT & PACKET_IS_BATCH ) {
lhiggs 0:03c649c76388 232 new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK);
lhiggs 0:03c649c76388 233
lhiggs 0:03c649c76388 234 } else {
lhiggs 0:03c649c76388 235 new_packet.data_length = 4;
lhiggs 0:03c649c76388 236 }
lhiggs 0:03c649c76388 237 }
lhiggs 0:03c649c76388 238 break;
lhiggs 0:03c649c76388 239
lhiggs 0:03c649c76388 240 // USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of
lhiggs 0:03c649c76388 241 // data.
lhiggs 0:03c649c76388 242 case USART_STATE_DATA:
lhiggs 0:03c649c76388 243 new_packet.packet_data[data_counter] = um6_uart.getc();
lhiggs 0:03c649c76388 244 data_counter++;
lhiggs 0:03c649c76388 245
lhiggs 0:03c649c76388 246 // If the expected number of bytes has been received, transition to the CHECKSUM state.
lhiggs 0:03c649c76388 247 if ( data_counter == new_packet.data_length ) {
lhiggs 0:03c649c76388 248 // Reset data_counter, since it will be used in the CHECKSUM state.
lhiggs 0:03c649c76388 249 data_counter = 0;
lhiggs 0:03c649c76388 250 gUSART_State = USART_STATE_CHECKSUM;
lhiggs 0:03c649c76388 251 }
lhiggs 0:03c649c76388 252 break;
lhiggs 0:03c649c76388 253
lhiggs 0:03c649c76388 254
lhiggs 0:03c649c76388 255
lhiggs 0:03c649c76388 256 // USART in CHECKSUM state. In this state, the entire packet has been received, with the exception
lhiggs 0:03c649c76388 257 // of the 16-bit checksum.
lhiggs 0:03c649c76388 258 case USART_STATE_CHECKSUM:
lhiggs 0:03c649c76388 259 // Get the highest-order byte
lhiggs 0:03c649c76388 260 if ( data_counter == 0 ) {
lhiggs 0:03c649c76388 261 new_packet.checksum = ((uint16_t)um6_uart.getc() << 8);
lhiggs 0:03c649c76388 262 data_counter++;
lhiggs 0:03c649c76388 263 } else { // ( data_counter == 1 )
lhiggs 0:03c649c76388 264 // Get lower-order byte
lhiggs 0:03c649c76388 265 new_packet.checksum = new_packet.checksum | ((uint16_t)um6_uart.getc() & 0x0FF);
lhiggs 0:03c649c76388 266
lhiggs 0:03c649c76388 267 // Both checksum bytes have been received. Make sure that the checksum is valid.
lhiggs 0:03c649c76388 268 uint16_t checksum = ComputeChecksum( &new_packet );
lhiggs 0:03c649c76388 269
lhiggs 0:03c649c76388 270
lhiggs 0:03c649c76388 271
lhiggs 0:03c649c76388 272 // If checksum does not match, exit function
lhiggs 0:03c649c76388 273 if ( checksum != new_packet.checksum ) {
lhiggs 0:03c649c76388 274 return;
lhiggs 0:03c649c76388 275 } // end if(checksum check)
lhiggs 0:03c649c76388 276
lhiggs 0:03c649c76388 277
lhiggs 0:03c649c76388 278
lhiggs 0:03c649c76388 279 else
lhiggs 0:03c649c76388 280
lhiggs 0:03c649c76388 281 {
lhiggs 0:03c649c76388 282
lhiggs 0:03c649c76388 283 // Packet was received correctly.
lhiggs 0:03c649c76388 284
lhiggs 0:03c649c76388 285 //-----------------------------------------------------------------------------------------------
lhiggs 0:03c649c76388 286 //-----------------------------------------------------------------------------------------------
lhiggs 0:03c649c76388 287 //
lhiggs 0:03c649c76388 288 // CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!!
lhiggs 0:03c649c76388 289
lhiggs 0:03c649c76388 290
lhiggs 0:03c649c76388 291 // IF DATA ADDRESS
lhiggs 0:03c649c76388 292 if (new_packet.address_type == ADDRESS_TYPE_DATA) {
lhiggs 0:03c649c76388 293
lhiggs 0:03c649c76388 294 //------------------------------------------------------------
lhiggs 0:03c649c76388 295 // UM6_GYRO_PROC_XY (0x5C)
lhiggs 0:03c649c76388 296 // To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees
lhiggs 0:03c649c76388 297 // per second, the data should be multiplied by the scale factor 0.0610352 as shown below
lhiggs 0:03c649c76388 298 // angular_rate = register_data*0.0610352
lhiggs 0:03c649c76388 299
lhiggs 0:03c649c76388 300 if (new_packet.address == UM6_GYRO_PROC_XY) {
lhiggs 0:03c649c76388 301
lhiggs 0:03c649c76388 302 // GYRO_PROC_X
lhiggs 0:03c649c76388 303 MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:03c649c76388 304 MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1];
lhiggs 0:03c649c76388 305 data.Gyro_Proc_X = MY_DATA_GYRO_PROC_X*0.0610352;
lhiggs 0:03c649c76388 306
lhiggs 0:03c649c76388 307 MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:03c649c76388 308 MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3];
lhiggs 0:03c649c76388 309 data.Gyro_Proc_Y = MY_DATA_GYRO_PROC_Y*0.0610352;
lhiggs 0:03c649c76388 310
lhiggs 0:03c649c76388 311
lhiggs 0:03c649c76388 312 //------------------------------------------------------------
lhiggs 0:03c649c76388 313
lhiggs 0:03c649c76388 314
lhiggs 0:03c649c76388 315 //------------------------------------------------------------
lhiggs 0:03c649c76388 316 // UM6_GYRO_PROC_Z (0x5D)
lhiggs 0:03c649c76388 317 // To convert the register data from a 16-bit 2's complement integer to the actual angular rate in
lhiggs 0:03c649c76388 318 // degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below.
lhiggs 0:03c649c76388 319
lhiggs 0:03c649c76388 320
lhiggs 0:03c649c76388 321 // GYRO_PROC_Z
lhiggs 0:03c649c76388 322 MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:03c649c76388 323 MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];;
lhiggs 0:03c649c76388 324 data.Gyro_Proc_Z = MY_DATA_GYRO_PROC_Z*0.0610352;
lhiggs 0:03c649c76388 325
lhiggs 0:03c649c76388 326
lhiggs 0:03c649c76388 327 } // end if(MY_DATA_GYRO_PROC)
lhiggs 0:03c649c76388 328 //------------------------------------------------------------
lhiggs 0:03c649c76388 329
lhiggs 0:03c649c76388 330
lhiggs 0:03c649c76388 331 //------------------------------------------------------------
lhiggs 0:03c649c76388 332 // UM6_ACCEL_PROC_XY (0x5E)
lhiggs 0:03c649c76388 333 // To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities,
lhiggs 0:03c649c76388 334 // the data should be multiplied by the scale factor 0.000183105 as shown below.
lhiggs 0:03c649c76388 335 // acceleration = register_data* 0.000183105
lhiggs 0:03c649c76388 336 if (new_packet.address == UM6_ACCEL_PROC_XY) {
lhiggs 0:03c649c76388 337
lhiggs 0:03c649c76388 338 // ACCEL_PROC_X
lhiggs 0:03c649c76388 339 MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:03c649c76388 340 MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1];
lhiggs 0:03c649c76388 341 data.Accel_Proc_X = MY_DATA_ACCEL_PROC_X*0.000183105;
lhiggs 0:03c649c76388 342
lhiggs 0:03c649c76388 343 // ACCEL_PROC_Y
lhiggs 0:03c649c76388 344 MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:03c649c76388 345 MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3];
lhiggs 0:03c649c76388 346 data.Accel_Proc_Y = MY_DATA_ACCEL_PROC_Y*0.000183105;
lhiggs 0:03c649c76388 347
lhiggs 0:03c649c76388 348 //------------------------------------------------------------
lhiggs 0:03c649c76388 349
lhiggs 0:03c649c76388 350
lhiggs 0:03c649c76388 351 //------------------------------------------------------------
lhiggs 0:03c649c76388 352 // UM6_ACCEL_PROC_Z (0x5F)
lhiggs 0:03c649c76388 353 // To convert the register data from a 16-bit 2's complement integer to the actual acceleration in
lhiggs 0:03c649c76388 354 // gravities, the data should be multiplied by the scale factor 0.000183105 as shown below.
lhiggs 0:03c649c76388 355
lhiggs 0:03c649c76388 356 // ACCEL_PROC_Z
lhiggs 0:03c649c76388 357 MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:03c649c76388 358 MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5];
lhiggs 0:03c649c76388 359 data.Accel_Proc_Z = MY_DATA_ACCEL_PROC_Z*0.000183105;
lhiggs 0:03c649c76388 360
lhiggs 0:03c649c76388 361 } // end if(MY_DATA_ACCEL_PROC)
lhiggs 0:03c649c76388 362
lhiggs 0:03c649c76388 363 //------------------------------------------------------------
lhiggs 0:03c649c76388 364
lhiggs 0:03c649c76388 365
lhiggs 0:03c649c76388 366 //------------------------------------------------------------
lhiggs 0:03c649c76388 367 // UM6_MAG_PROC_XY (0x60)
lhiggs 0:03c649c76388 368 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
lhiggs 0:03c649c76388 369 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
lhiggs 0:03c649c76388 370 // shown below.
lhiggs 0:03c649c76388 371 // magnetic field = register_data* 0.000305176
lhiggs 0:03c649c76388 372 if (new_packet.address == UM6_MAG_PROC_XY) {
lhiggs 0:03c649c76388 373
lhiggs 0:03c649c76388 374 // MAG_PROC_X
lhiggs 0:03c649c76388 375 MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:03c649c76388 376 MY_DATA_MAG_PROC_X |= new_packet.packet_data[1];
lhiggs 0:03c649c76388 377 data.Mag_Proc_X = MY_DATA_MAG_PROC_X*0.000305176;
lhiggs 0:03c649c76388 378
lhiggs 0:03c649c76388 379
lhiggs 0:03c649c76388 380 // MAG_PROC_Y
lhiggs 0:03c649c76388 381 MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:03c649c76388 382 MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3];
lhiggs 0:03c649c76388 383 data.Mag_Proc_Y = MY_DATA_MAG_PROC_Y*0.000305176;
lhiggs 0:03c649c76388 384
lhiggs 0:03c649c76388 385 //------------------------------------------------------------
lhiggs 0:03c649c76388 386
lhiggs 0:03c649c76388 387
lhiggs 0:03c649c76388 388 //------------------------------------------------------------
lhiggs 0:03c649c76388 389 // UM6_MAG_PROC_Z (0x61)
lhiggs 0:03c649c76388 390 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
lhiggs 0:03c649c76388 391 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
lhiggs 0:03c649c76388 392 // shown below.
lhiggs 0:03c649c76388 393 // magnetic field = register_data*0.000305176
lhiggs 0:03c649c76388 394
lhiggs 0:03c649c76388 395 // MAG_PROC_Z
lhiggs 0:03c649c76388 396 MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:03c649c76388 397 MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5];
lhiggs 0:03c649c76388 398 data.Mag_Proc_Z = MY_DATA_MAG_PROC_Z*0.000305176;
lhiggs 0:03c649c76388 399
lhiggs 0:03c649c76388 400 } // end if(UM6_MAG_PROC)
lhiggs 0:03c649c76388 401 //------------------------------------------------------------
lhiggs 0:03c649c76388 402
lhiggs 0:03c649c76388 403
lhiggs 0:03c649c76388 404
lhiggs 0:03c649c76388 405 //------------------------------------------------------------
lhiggs 0:03c649c76388 406 // UM6_EULER_PHI_THETA (0x62)
lhiggs 0:03c649c76388 407 // Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle
lhiggs 0:03c649c76388 408 // estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in
lhiggs 0:03c649c76388 409 // degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below
lhiggs 0:03c649c76388 410 // angle estimate = register_data* 0.0109863
lhiggs 0:03c649c76388 411 if (new_packet.address == UM6_EULER_PHI_THETA) {
lhiggs 0:03c649c76388 412 // EULER_PHI (ROLL)
lhiggs 0:03c649c76388 413 MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
lhiggs 0:03c649c76388 414 MY_DATA_EULER_PHI |= new_packet.packet_data[1];
lhiggs 0:03c649c76388 415 data.Roll = MY_DATA_EULER_PHI*0.0109863;
lhiggs 0:03c649c76388 416
lhiggs 0:03c649c76388 417
lhiggs 0:03c649c76388 418
lhiggs 0:03c649c76388 419
lhiggs 0:03c649c76388 420
lhiggs 0:03c649c76388 421 // EULER_THETA (PITCH)
lhiggs 0:03c649c76388 422 MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
lhiggs 0:03c649c76388 423 MY_DATA_EULER_THETA |= new_packet.packet_data[3];
lhiggs 0:03c649c76388 424 data.Pitch = MY_DATA_EULER_THETA*0.0109863;
lhiggs 0:03c649c76388 425
lhiggs 0:03c649c76388 426 //------------------------------------------------------------
lhiggs 0:03c649c76388 427
lhiggs 0:03c649c76388 428 //------------------------------------------------------------
lhiggs 0:03c649c76388 429 // UM6_EULER_PSI (0x63) (YAW)
lhiggs 0:03c649c76388 430 // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
lhiggs 0:03c649c76388 431 // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
lhiggs 0:03c649c76388 432 // should be multiplied by the scale factor 0.0109863 as shown below
lhiggs 0:03c649c76388 433
lhiggs 0:03c649c76388 434 MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
lhiggs 0:03c649c76388 435 MY_DATA_EULER_PSI |= new_packet.packet_data[5];
lhiggs 0:03c649c76388 436 data.Yaw = MY_DATA_EULER_PSI*0.0109863;
lhiggs 0:03c649c76388 437
lhiggs 0:03c649c76388 438
lhiggs 0:03c649c76388 439 } // end if(UM6_EULER_PHI_THETA)
lhiggs 0:03c649c76388 440 //------------------------------------------------------------
lhiggs 0:03c649c76388 441
lhiggs 0:03c649c76388 442 } // end if(ADDRESS_TYPE_DATA)
lhiggs 0:03c649c76388 443
lhiggs 0:03c649c76388 444
lhiggs 0:03c649c76388 445 // A full packet has been received.
lhiggs 0:03c649c76388 446 // Put the USART back into the WAIT state and reset
lhiggs 0:03c649c76388 447 // the data_counter variable so that it can be used to receive the next packet.
lhiggs 0:03c649c76388 448 data_counter = 0;
lhiggs 0:03c649c76388 449 gUSART_State = USART_STATE_WAIT;
lhiggs 0:03c649c76388 450
lhiggs 0:03c649c76388 451
lhiggs 0:03c649c76388 452 } // end else(GET_DATA)
lhiggs 0:03c649c76388 453
lhiggs 0:03c649c76388 454 }
lhiggs 0:03c649c76388 455 break;
lhiggs 0:03c649c76388 456
lhiggs 0:03c649c76388 457 } // end switch ( gUSART_State )
lhiggs 0:03c649c76388 458
lhiggs 0:03c649c76388 459 return;
lhiggs 0:03c649c76388 460
lhiggs 0:03c649c76388 461 } // end get_gyro_x()
lhiggs 0:03c649c76388 462
lhiggs 0:03c649c76388 463 #endif