Nathan Ewin / Mbed 2 deprecated LogData_UM6-to-SDcard

A project similar to http://mbed.org/users/lhiggs/code/UM6_IMU_AHRS_2012/, where I'm trying to log data from a UM6 (CH Robotics orientation sensor) and a GPS transceiver to an sd card. I've adapted LHiggs code to include ModGPS. For sum reason a soon as I pick up a gps signal the UM6 data freezes i.e. the time and gps signals continue to print out but the UM6 signals fixes on a single value.

Dependencies:   MODGPS MODSERIAL SDFileSystem mbed

UM6_config/UM6_config.h@7:af9f373ac87b, 2013-05-25 (annotated)

Committer:
njewin
Date:
Sat May 25 14:29:36 2013 +0000
Revision:
7:af9f373ac87b
Parent:
6:fae3d66a4e21
Child:
8:0ce247da6370
prints GPS speed to SD card

Who changed what in which revision?

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