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@12:894e648638e4, 2013-07-08 (annotated)

Committer:
njewin
Date:
Mon Jul 08 16:28:57 2013 +0000
Revision:
12:894e648638e4
Parent:
10:d96e068f3595 
gps working.  UM6 data signal freeze when gps signals are received.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
njewin 4:8dcf0bdc25c8 1 /* ------------------------------------------------------------------------------
njewin 4:8dcf0bdc25c8 2 File: UM6_config.h
njewin 9:7dcfa24d5e7a 3 Author: CH Robotics, edited by LHiggs, & 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 12:894e648638e4 38 #define DATA_ARRAY_SIZE 13
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 12:894e648638e4 66 //#define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers
njewin 12:894e648638e4 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 12:894e648638e4 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 12:894e648638e4 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 12:894e648638e4 78 /*
njewin 4:8dcf0bdc25c8 79 #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 80 #define UM6_ERROR_COV_01 (DATA_REG_START_ADDRESS + 18)
njewin 4:8dcf0bdc25c8 81 #define UM6_ERROR_COV_02 (DATA_REG_START_ADDRESS + 19)
njewin 4:8dcf0bdc25c8 82 #define UM6_ERROR_COV_03 (DATA_REG_START_ADDRESS + 20)
njewin 4:8dcf0bdc25c8 83 #define UM6_ERROR_COV_10 (DATA_REG_START_ADDRESS + 21)
njewin 4:8dcf0bdc25c8 84 #define UM6_ERROR_COV_11 (DATA_REG_START_ADDRESS + 22)
njewin 4:8dcf0bdc25c8 85 #define UM6_ERROR_COV_12 (DATA_REG_START_ADDRESS + 23)
njewin 4:8dcf0bdc25c8 86 #define UM6_ERROR_COV_13 (DATA_REG_START_ADDRESS + 24)
njewin 4:8dcf0bdc25c8 87 #define UM6_ERROR_COV_20 (DATA_REG_START_ADDRESS + 25)
njewin 4:8dcf0bdc25c8 88 #define UM6_ERROR_COV_21 (DATA_REG_START_ADDRESS + 26)
njewin 4:8dcf0bdc25c8 89 #define UM6_ERROR_COV_22 (DATA_REG_START_ADDRESS + 27)
njewin 4:8dcf0bdc25c8 90 #define UM6_ERROR_COV_23 (DATA_REG_START_ADDRESS + 28)
njewin 4:8dcf0bdc25c8 91 #define UM6_ERROR_COV_30 (DATA_REG_START_ADDRESS + 29)
njewin 4:8dcf0bdc25c8 92 #define UM6_ERROR_COV_31 (DATA_REG_START_ADDRESS + 30)
njewin 4:8dcf0bdc25c8 93 #define UM6_ERROR_COV_32 (DATA_REG_START_ADDRESS + 31)
njewin 4:8dcf0bdc25c8 94 #define UM6_ERROR_COV_33 (DATA_REG_START_ADDRESS + 32)
njewin 12:894e648638e4 95 */
njewin 12:894e648638e4 96 // connected gps module directing to mbed using MODGPS
njewin 12:894e648638e4 97 //#define UM6_GPS_LONGITUDE (DATA_REG_START_ADDRESS + 34)
njewin 12:894e648638e4 98 //#define UM6_GPS_LATITUDE (DATA_REG_START_ADDRESS + 35)
njewin 12:894e648638e4 99 //#define UM6_GPS_ALTITUDE (DATA_REG_START_ADDRESS + 36)
njewin 12:894e648638e4 100 //#define UM6_GPS_COURSE_SPEED (DATA_REG_START_ADDRESS + 40)
njewin 4:8dcf0bdc25c8 101
njewin 4:8dcf0bdc25c8 102
njewin 4:8dcf0bdc25c8 103
njewin 4:8dcf0bdc25c8 104
njewin 4:8dcf0bdc25c8 105
njewin 4:8dcf0bdc25c8 106
njewin 4:8dcf0bdc25c8 107
njewin 4:8dcf0bdc25c8 108 /*******************************************************************************
njewin 4:8dcf0bdc25c8 109 * Function Name : ComputeChecksum
njewin 4:8dcf0bdc25c8 110 * Input : USARTPacket* new_packet
njewin 4:8dcf0bdc25c8 111 * Output : None
njewin 4:8dcf0bdc25c8 112 * Return : uint16_t
njewin 4:8dcf0bdc25c8 113 * Description : Returns the two byte sum of all the individual bytes in the
njewin 4:8dcf0bdc25c8 114 given packet.
njewin 4:8dcf0bdc25c8 115 *******************************************************************************/
njewin 4:8dcf0bdc25c8 116 uint16_t ComputeChecksum( USARTPacket* new_packet ) {
njewin 4:8dcf0bdc25c8 117 int32_t index;
njewin 4:8dcf0bdc25c8 118 uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address;
njewin 4:8dcf0bdc25c8 119
njewin 4:8dcf0bdc25c8 120 for ( index = 0; index < new_packet->data_length; index++ ) {
njewin 4:8dcf0bdc25c8 121 checksum += new_packet->packet_data[index];
njewin 4:8dcf0bdc25c8 122 }
njewin 4:8dcf0bdc25c8 123 return checksum;
njewin 4:8dcf0bdc25c8 124 }
njewin 4:8dcf0bdc25c8 125
njewin 4:8dcf0bdc25c8 126
njewin 4:8dcf0bdc25c8 127
njewin 4:8dcf0bdc25c8 128
njewin 4:8dcf0bdc25c8 129
njewin 4:8dcf0bdc25c8 130 static USARTPacket new_packet;
njewin 4:8dcf0bdc25c8 131
njewin 4:8dcf0bdc25c8 132 // Flag for storing the current USART state
njewin 4:8dcf0bdc25c8 133 uint8_t gUSART_State = USART_STATE_WAIT;
njewin 4:8dcf0bdc25c8 134
njewin 4:8dcf0bdc25c8 135
njewin 4:8dcf0bdc25c8 136 struct UM6{
njewin 4:8dcf0bdc25c8 137 float Gyro_Proc_X;
njewin 4:8dcf0bdc25c8 138 float Gyro_Proc_Y;
njewin 4:8dcf0bdc25c8 139 float Gyro_Proc_Z;
njewin 4:8dcf0bdc25c8 140 float Accel_Proc_X;
njewin 4:8dcf0bdc25c8 141 float Accel_Proc_Y;
njewin 4:8dcf0bdc25c8 142 float Accel_Proc_Z;
njewin 12:894e648638e4 143 //float Mag_Proc_X;
njewin 12:894e648638e4 144 //float Mag_Proc_Y;
njewin 12:894e648638e4 145 //float Mag_Proc_Z;
njewin 4:8dcf0bdc25c8 146 float Roll;
njewin 4:8dcf0bdc25c8 147 float Pitch;
njewin 4:8dcf0bdc25c8 148 float Yaw;
njewin 12:894e648638e4 149 //float GPS_long;
njewin 12:894e648638e4 150 //float GPS_lat;
njewin 12:894e648638e4 151 //float GPS_alt;
njewin 12:894e648638e4 152 //float GPS_course;
njewin 12:894e648638e4 153 //float GPS_speed;
njewin 9:7dcfa24d5e7a 154
njewin 4:8dcf0bdc25c8 155 };
njewin 4:8dcf0bdc25c8 156 UM6 data;
njewin 4:8dcf0bdc25c8 157
njewin 4:8dcf0bdc25c8 158
njewin 4:8dcf0bdc25c8 159
njewin 4:8dcf0bdc25c8 160
njewin 4:8dcf0bdc25c8 161 void Process_um6_packet() {
njewin 4:8dcf0bdc25c8 162
njewin 4:8dcf0bdc25c8 163 int16_t MY_DATA_GYRO_PROC_X;
njewin 4:8dcf0bdc25c8 164 int16_t MY_DATA_GYRO_PROC_Y;
njewin 4:8dcf0bdc25c8 165 int16_t MY_DATA_GYRO_PROC_Z;
njewin 4:8dcf0bdc25c8 166 int16_t MY_DATA_ACCEL_PROC_X;
njewin 4:8dcf0bdc25c8 167 int16_t MY_DATA_ACCEL_PROC_Y;
njewin 4:8dcf0bdc25c8 168 int16_t MY_DATA_ACCEL_PROC_Z;
njewin 12:894e648638e4 169 //int16_t MY_DATA_MAG_PROC_X;
njewin 12:894e648638e4 170 //int16_t MY_DATA_MAG_PROC_Y;
njewin 12:894e648638e4 171 //int16_t MY_DATA_MAG_PROC_Z;
njewin 4:8dcf0bdc25c8 172 int16_t MY_DATA_EULER_PHI;
njewin 4:8dcf0bdc25c8 173 int16_t MY_DATA_EULER_THETA;
njewin 4:8dcf0bdc25c8 174 int16_t MY_DATA_EULER_PSI;
njewin 12:894e648638e4 175 /*
njewin 9:7dcfa24d5e7a 176 int32_t MY_DATA_GPS_LONG;
njewin 9:7dcfa24d5e7a 177 int32_t MY_DATA_GPS_LONG0;
njewin 9:7dcfa24d5e7a 178 int32_t MY_DATA_GPS_LONG1;
njewin 9:7dcfa24d5e7a 179 int32_t MY_DATA_GPS_LONG2;
njewin 9:7dcfa24d5e7a 180 int32_t MY_DATA_GPS_LAT;
njewin 9:7dcfa24d5e7a 181 int32_t MY_DATA_GPS_LAT0;
njewin 9:7dcfa24d5e7a 182 int32_t MY_DATA_GPS_LAT1;
njewin 9:7dcfa24d5e7a 183 int32_t MY_DATA_GPS_LAT2;
njewin 10:d96e068f3595 184 int32_t MY_DATA_GPS_ALT;
njewin 10:d96e068f3595 185 int32_t MY_DATA_GPS_ALT0;
njewin 10:d96e068f3595 186 int32_t MY_DATA_GPS_ALT1;
njewin 10:d96e068f3595 187 int32_t MY_DATA_GPS_ALT2;
njewin 4:8dcf0bdc25c8 188 int16_t MY_DATA_GPS_COURSE;
njewin 4:8dcf0bdc25c8 189 int16_t MY_DATA_GPS_SPEED;
njewin 12:894e648638e4 190 */
njewin 4:8dcf0bdc25c8 191
njewin 4:8dcf0bdc25c8 192
njewin 4:8dcf0bdc25c8 193 static uint8_t data_counter = 0;
njewin 4:8dcf0bdc25c8 194
njewin 4:8dcf0bdc25c8 195
njewin 4:8dcf0bdc25c8 196
njewin 4:8dcf0bdc25c8 197 // The next action should depend on the USART state.
njewin 4:8dcf0bdc25c8 198 switch ( gUSART_State ) {
njewin 4:8dcf0bdc25c8 199 // USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes
njewin 4:8dcf0bdc25c8 200 // that signals a new incoming packet.
njewin 4:8dcf0bdc25c8 201 case USART_STATE_WAIT:
njewin 4:8dcf0bdc25c8 202 if ( data_counter == 0 ) { // Waiting on 's' character
njewin 4:8dcf0bdc25c8 203 if ( um6_uart.getc() == 's' ) {
njewin 4:8dcf0bdc25c8 204
njewin 4:8dcf0bdc25c8 205 data_counter++;
njewin 4:8dcf0bdc25c8 206 } else {
njewin 4:8dcf0bdc25c8 207 data_counter = 0;
njewin 4:8dcf0bdc25c8 208 }
njewin 4:8dcf0bdc25c8 209 } else if ( data_counter == 1 ) { // Waiting on 'n' character
njewin 4:8dcf0bdc25c8 210 if ( um6_uart.getc() == 'n' ) {
njewin 4:8dcf0bdc25c8 211 data_counter++;
njewin 4:8dcf0bdc25c8 212
njewin 4:8dcf0bdc25c8 213 } else {
njewin 4:8dcf0bdc25c8 214 data_counter = 0;
njewin 4:8dcf0bdc25c8 215 }
njewin 4:8dcf0bdc25c8 216 } else if ( data_counter == 2 ) { // Waiting on 'p' character
njewin 4:8dcf0bdc25c8 217 if ( um6_uart.getc() == 'p' ) {
njewin 4:8dcf0bdc25c8 218 // The full 'snp' sequence was received. Reset data_counter (it will be used again
njewin 4:8dcf0bdc25c8 219 // later) and transition to the next state.
njewin 4:8dcf0bdc25c8 220 data_counter = 0;
njewin 4:8dcf0bdc25c8 221 gUSART_State = USART_STATE_TYPE;
njewin 4:8dcf0bdc25c8 222
njewin 4:8dcf0bdc25c8 223 } else {
njewin 4:8dcf0bdc25c8 224 data_counter = 0;
njewin 4:8dcf0bdc25c8 225 }
njewin 4:8dcf0bdc25c8 226 }
njewin 4:8dcf0bdc25c8 227 break;
njewin 4:8dcf0bdc25c8 228
njewin 4:8dcf0bdc25c8 229 // USART in the TYPE state. In this state, the USART has just received the sequence of bytes that
njewin 4:8dcf0bdc25c8 230 // indicates a new packet is about to arrive. Now, the USART expects to see the packet type.
njewin 4:8dcf0bdc25c8 231 case USART_STATE_TYPE:
njewin 4:8dcf0bdc25c8 232
njewin 4:8dcf0bdc25c8 233 new_packet.PT = um6_uart.getc();
njewin 4:8dcf0bdc25c8 234
njewin 4:8dcf0bdc25c8 235 gUSART_State = USART_STATE_ADDRESS;
njewin 4:8dcf0bdc25c8 236
njewin 4:8dcf0bdc25c8 237 break;
njewin 4:8dcf0bdc25c8 238
njewin 4:8dcf0bdc25c8 239 // USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating
njewin 4:8dcf0bdc25c8 240 // the address that the packet applies to
njewin 4:8dcf0bdc25c8 241 case USART_STATE_ADDRESS:
njewin 4:8dcf0bdc25c8 242 new_packet.address = um6_uart.getc();
njewin 4:8dcf0bdc25c8 243
njewin 4:8dcf0bdc25c8 244 // For convenience, identify the type of packet this is and copy to the packet structure
njewin 4:8dcf0bdc25c8 245 // (this will be used by the packet handler later)
njewin 4:8dcf0bdc25c8 246 if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) {
njewin 4:8dcf0bdc25c8 247 new_packet.address_type = ADDRESS_TYPE_CONFIG;
njewin 4:8dcf0bdc25c8 248 } else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) {
njewin 4:8dcf0bdc25c8 249 new_packet.address_type = ADDRESS_TYPE_DATA;
njewin 4:8dcf0bdc25c8 250 } else {
njewin 4:8dcf0bdc25c8 251 new_packet.address_type = ADDRESS_TYPE_COMMAND;
njewin 4:8dcf0bdc25c8 252 }
njewin 4:8dcf0bdc25c8 253
njewin 4:8dcf0bdc25c8 254 // Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command)
njewin 4:8dcf0bdc25c8 255 // 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 256 if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) {
njewin 4:8dcf0bdc25c8 257 gUSART_State = USART_STATE_CHECKSUM;
njewin 4:8dcf0bdc25c8 258 }
njewin 4:8dcf0bdc25c8 259
njewin 4:8dcf0bdc25c8 260 // If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data
njewin 4:8dcf0bdc25c8 261 else {
njewin 4:8dcf0bdc25c8 262 gUSART_State = USART_STATE_DATA;
njewin 4:8dcf0bdc25c8 263 // Determine the expected number of bytes in this data packet based on the packet type. A write operation
njewin 4:8dcf0bdc25c8 264 // consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size,
njewin 4:8dcf0bdc25c8 265 // where the batch size is also given in the packet type byte.
njewin 4:8dcf0bdc25c8 266 if ( new_packet.PT & PACKET_IS_BATCH ) {
njewin 4:8dcf0bdc25c8 267 new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK);
njewin 4:8dcf0bdc25c8 268
njewin 4:8dcf0bdc25c8 269 } else {
njewin 4:8dcf0bdc25c8 270 new_packet.data_length = 4;
njewin 4:8dcf0bdc25c8 271 }
njewin 4:8dcf0bdc25c8 272 }
njewin 4:8dcf0bdc25c8 273 break;
njewin 4:8dcf0bdc25c8 274
njewin 4:8dcf0bdc25c8 275 // USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of
njewin 4:8dcf0bdc25c8 276 // data.
njewin 4:8dcf0bdc25c8 277 case USART_STATE_DATA:
njewin 4:8dcf0bdc25c8 278 new_packet.packet_data[data_counter] = um6_uart.getc();
njewin 4:8dcf0bdc25c8 279 data_counter++;
njewin 4:8dcf0bdc25c8 280
njewin 4:8dcf0bdc25c8 281 // If the expected number of bytes has been received, transition to the CHECKSUM state.
njewin 4:8dcf0bdc25c8 282 if ( data_counter == new_packet.data_length ) {
njewin 4:8dcf0bdc25c8 283 // Reset data_counter, since it will be used in the CHECKSUM state.
njewin 4:8dcf0bdc25c8 284 data_counter = 0;
njewin 4:8dcf0bdc25c8 285 gUSART_State = USART_STATE_CHECKSUM;
njewin 4:8dcf0bdc25c8 286 }
njewin 4:8dcf0bdc25c8 287 break;
njewin 4:8dcf0bdc25c8 288
njewin 4:8dcf0bdc25c8 289
njewin 4:8dcf0bdc25c8 290
njewin 4:8dcf0bdc25c8 291 // USART in CHECKSUM state. In this state, the entire packet has been received, with the exception
njewin 4:8dcf0bdc25c8 292 // of the 16-bit checksum.
njewin 4:8dcf0bdc25c8 293 case USART_STATE_CHECKSUM:
njewin 4:8dcf0bdc25c8 294 // Get the highest-order byte
njewin 4:8dcf0bdc25c8 295 if ( data_counter == 0 ) {
njewin 4:8dcf0bdc25c8 296 new_packet.checksum = ((uint16_t)um6_uart.getc() << 8);
njewin 4:8dcf0bdc25c8 297 data_counter++;
njewin 4:8dcf0bdc25c8 298 } else { // ( data_counter == 1 )
njewin 4:8dcf0bdc25c8 299 // Get lower-order byte
njewin 4:8dcf0bdc25c8 300 new_packet.checksum = new_packet.checksum | ((uint16_t)um6_uart.getc() & 0x0FF);
njewin 4:8dcf0bdc25c8 301
njewin 4:8dcf0bdc25c8 302 // Both checksum bytes have been received. Make sure that the checksum is valid.
njewin 4:8dcf0bdc25c8 303 uint16_t checksum = ComputeChecksum( &new_packet );
njewin 4:8dcf0bdc25c8 304
njewin 4:8dcf0bdc25c8 305
njewin 4:8dcf0bdc25c8 306
njewin 4:8dcf0bdc25c8 307 // If checksum does not match, exit function
njewin 4:8dcf0bdc25c8 308 if ( checksum != new_packet.checksum ) {
njewin 4:8dcf0bdc25c8 309 return;
njewin 4:8dcf0bdc25c8 310 } // end if(checksum check)
njewin 4:8dcf0bdc25c8 311
njewin 4:8dcf0bdc25c8 312
njewin 4:8dcf0bdc25c8 313
njewin 4:8dcf0bdc25c8 314 else
njewin 4:8dcf0bdc25c8 315
njewin 4:8dcf0bdc25c8 316 {
njewin 4:8dcf0bdc25c8 317
njewin 4:8dcf0bdc25c8 318 // Packet was received correctly.
njewin 4:8dcf0bdc25c8 319
njewin 4:8dcf0bdc25c8 320 //-----------------------------------------------------------------------------------------------
njewin 4:8dcf0bdc25c8 321 //-----------------------------------------------------------------------------------------------
njewin 4:8dcf0bdc25c8 322 //
njewin 4:8dcf0bdc25c8 323 // CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!!
njewin 4:8dcf0bdc25c8 324
njewin 4:8dcf0bdc25c8 325
njewin 4:8dcf0bdc25c8 326 // IF DATA ADDRESS
njewin 4:8dcf0bdc25c8 327 if (new_packet.address_type == ADDRESS_TYPE_DATA) {
njewin 4:8dcf0bdc25c8 328
njewin 4:8dcf0bdc25c8 329 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 330 // UM6_GYRO_PROC_XY (0x5C)
njewin 4:8dcf0bdc25c8 331 // To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees
njewin 4:8dcf0bdc25c8 332 // per second, the data should be multiplied by the scale factor 0.0610352 as shown below
njewin 4:8dcf0bdc25c8 333 // angular_rate = register_data*0.0610352
njewin 4:8dcf0bdc25c8 334
njewin 4:8dcf0bdc25c8 335 if (new_packet.address == UM6_GYRO_PROC_XY) {
njewin 4:8dcf0bdc25c8 336
njewin 4:8dcf0bdc25c8 337 // GYRO_PROC_X
njewin 4:8dcf0bdc25c8 338 MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 339 MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1];
njewin 4:8dcf0bdc25c8 340 data.Gyro_Proc_X = MY_DATA_GYRO_PROC_X*0.0610352;
njewin 4:8dcf0bdc25c8 341
njewin 4:8dcf0bdc25c8 342 MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 343 MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3];
njewin 4:8dcf0bdc25c8 344 data.Gyro_Proc_Y = MY_DATA_GYRO_PROC_Y*0.0610352;
njewin 4:8dcf0bdc25c8 345
njewin 4:8dcf0bdc25c8 346
njewin 4:8dcf0bdc25c8 347 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 348
njewin 4:8dcf0bdc25c8 349
njewin 4:8dcf0bdc25c8 350 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 351 // UM6_GYRO_PROC_Z (0x5D)
njewin 4:8dcf0bdc25c8 352 // To convert the register data from a 16-bit 2's complement integer to the actual angular rate in
njewin 4:8dcf0bdc25c8 353 // degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below.
njewin 4:8dcf0bdc25c8 354
njewin 4:8dcf0bdc25c8 355
njewin 4:8dcf0bdc25c8 356 // GYRO_PROC_Z
njewin 4:8dcf0bdc25c8 357 MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 358 MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];;
njewin 4:8dcf0bdc25c8 359 data.Gyro_Proc_Z = MY_DATA_GYRO_PROC_Z*0.0610352;
njewin 4:8dcf0bdc25c8 360
njewin 4:8dcf0bdc25c8 361
njewin 4:8dcf0bdc25c8 362 } // end if(MY_DATA_GYRO_PROC)
njewin 4:8dcf0bdc25c8 363 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 364
njewin 4:8dcf0bdc25c8 365
njewin 4:8dcf0bdc25c8 366 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 367 // UM6_ACCEL_PROC_XY (0x5E)
njewin 4:8dcf0bdc25c8 368 // To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities,
njewin 4:8dcf0bdc25c8 369 // the data should be multiplied by the scale factor 0.000183105 as shown below.
njewin 4:8dcf0bdc25c8 370 // acceleration = register_data* 0.000183105
njewin 4:8dcf0bdc25c8 371 if (new_packet.address == UM6_ACCEL_PROC_XY) {
njewin 4:8dcf0bdc25c8 372
njewin 4:8dcf0bdc25c8 373 // ACCEL_PROC_X
njewin 4:8dcf0bdc25c8 374 MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 375 MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1];
njewin 4:8dcf0bdc25c8 376 data.Accel_Proc_X = MY_DATA_ACCEL_PROC_X*0.000183105;
njewin 4:8dcf0bdc25c8 377
njewin 4:8dcf0bdc25c8 378 // ACCEL_PROC_Y
njewin 4:8dcf0bdc25c8 379 MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 380 MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3];
njewin 4:8dcf0bdc25c8 381 data.Accel_Proc_Y = MY_DATA_ACCEL_PROC_Y*0.000183105;
njewin 4:8dcf0bdc25c8 382
njewin 4:8dcf0bdc25c8 383 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 384
njewin 4:8dcf0bdc25c8 385
njewin 4:8dcf0bdc25c8 386 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 387 // UM6_ACCEL_PROC_Z (0x5F)
njewin 4:8dcf0bdc25c8 388 // To convert the register data from a 16-bit 2's complement integer to the actual acceleration in
njewin 4:8dcf0bdc25c8 389 // gravities, the data should be multiplied by the scale factor 0.000183105 as shown below.
njewin 4:8dcf0bdc25c8 390
njewin 4:8dcf0bdc25c8 391 // ACCEL_PROC_Z
njewin 4:8dcf0bdc25c8 392 MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 393 MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5];
njewin 4:8dcf0bdc25c8 394 data.Accel_Proc_Z = MY_DATA_ACCEL_PROC_Z*0.000183105;
njewin 4:8dcf0bdc25c8 395
njewin 4:8dcf0bdc25c8 396 } // end if(MY_DATA_ACCEL_PROC)
njewin 4:8dcf0bdc25c8 397
njewin 4:8dcf0bdc25c8 398 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 399
njewin 4:8dcf0bdc25c8 400
njewin 4:8dcf0bdc25c8 401 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 402 // UM6_MAG_PROC_XY (0x60)
njewin 4:8dcf0bdc25c8 403 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
njewin 4:8dcf0bdc25c8 404 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
njewin 4:8dcf0bdc25c8 405 // shown below.
njewin 4:8dcf0bdc25c8 406 // magnetic field = register_data* 0.000305176
njewin 12:894e648638e4 407 /*
njewin 4:8dcf0bdc25c8 408 if (new_packet.address == UM6_MAG_PROC_XY) {
njewin 4:8dcf0bdc25c8 409
njewin 4:8dcf0bdc25c8 410 // MAG_PROC_X
njewin 4:8dcf0bdc25c8 411 MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 412 MY_DATA_MAG_PROC_X |= new_packet.packet_data[1];
njewin 4:8dcf0bdc25c8 413 data.Mag_Proc_X = MY_DATA_MAG_PROC_X*0.000305176;
njewin 4:8dcf0bdc25c8 414
njewin 4:8dcf0bdc25c8 415
njewin 4:8dcf0bdc25c8 416 // MAG_PROC_Y
njewin 4:8dcf0bdc25c8 417 MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 418 MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3];
njewin 4:8dcf0bdc25c8 419 data.Mag_Proc_Y = MY_DATA_MAG_PROC_Y*0.000305176;
njewin 4:8dcf0bdc25c8 420
njewin 4:8dcf0bdc25c8 421 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 422
njewin 4:8dcf0bdc25c8 423
njewin 4:8dcf0bdc25c8 424 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 425 // UM6_MAG_PROC_Z (0x61)
njewin 4:8dcf0bdc25c8 426 // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
njewin 4:8dcf0bdc25c8 427 // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
njewin 4:8dcf0bdc25c8 428 // shown below.
njewin 4:8dcf0bdc25c8 429 // magnetic field = register_data*0.000305176
njewin 4:8dcf0bdc25c8 430
njewin 4:8dcf0bdc25c8 431 // MAG_PROC_Z
njewin 4:8dcf0bdc25c8 432 MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 433 MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5];
njewin 4:8dcf0bdc25c8 434 data.Mag_Proc_Z = MY_DATA_MAG_PROC_Z*0.000305176;
njewin 4:8dcf0bdc25c8 435
njewin 4:8dcf0bdc25c8 436 } // end if(UM6_MAG_PROC)
njewin 12:894e648638e4 437 */
njewin 4:8dcf0bdc25c8 438 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 439
njewin 4:8dcf0bdc25c8 440
njewin 4:8dcf0bdc25c8 441
njewin 4:8dcf0bdc25c8 442 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 443 // UM6_EULER_PHI_THETA (0x62)
njewin 4:8dcf0bdc25c8 444 // Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle
njewin 4:8dcf0bdc25c8 445 // estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in
njewin 4:8dcf0bdc25c8 446 // degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below
njewin 4:8dcf0bdc25c8 447 // angle estimate = register_data* 0.0109863
njewin 4:8dcf0bdc25c8 448 if (new_packet.address == UM6_EULER_PHI_THETA) {
njewin 4:8dcf0bdc25c8 449 // EULER_PHI (ROLL)
njewin 4:8dcf0bdc25c8 450 MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 451 MY_DATA_EULER_PHI |= new_packet.packet_data[1];
njewin 4:8dcf0bdc25c8 452 data.Roll = MY_DATA_EULER_PHI*0.0109863;
njewin 4:8dcf0bdc25c8 453
njewin 4:8dcf0bdc25c8 454
njewin 4:8dcf0bdc25c8 455
njewin 4:8dcf0bdc25c8 456
njewin 4:8dcf0bdc25c8 457
njewin 4:8dcf0bdc25c8 458 // EULER_THETA (PITCH)
njewin 4:8dcf0bdc25c8 459 MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 460 MY_DATA_EULER_THETA |= new_packet.packet_data[3];
njewin 4:8dcf0bdc25c8 461 data.Pitch = MY_DATA_EULER_THETA*0.0109863;
njewin 4:8dcf0bdc25c8 462
njewin 4:8dcf0bdc25c8 463 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 464
njewin 4:8dcf0bdc25c8 465 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 466 // UM6_EULER_PSI (0x63) (YAW)
njewin 4:8dcf0bdc25c8 467 // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
njewin 4:8dcf0bdc25c8 468 // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
njewin 4:8dcf0bdc25c8 469 // should be multiplied by the scale factor 0.0109863 as shown below
njewin 4:8dcf0bdc25c8 470
njewin 4:8dcf0bdc25c8 471 MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 472 MY_DATA_EULER_PSI |= new_packet.packet_data[5];
njewin 4:8dcf0bdc25c8 473 data.Yaw = MY_DATA_EULER_PSI*0.0109863;
njewin 4:8dcf0bdc25c8 474
njewin 4:8dcf0bdc25c8 475
njewin 4:8dcf0bdc25c8 476 }
njewin 12:894e648638e4 477 /*
njewin 4:8dcf0bdc25c8 478 //-------------------------------------------------------------------
njewin 4:8dcf0bdc25c8 479 // GPS Ground/Speed
njewin 4:8dcf0bdc25c8 480 if (new_packet.address == UM6_GPS_COURSE_SPEED) {
njewin 4:8dcf0bdc25c8 481 // Ground course
njewin 4:8dcf0bdc25c8 482 MY_DATA_GPS_COURSE = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 483 MY_DATA_GPS_COURSE |= new_packet.packet_data[1];
njewin 8:0ce247da6370 484 data.GPS_course = MY_DATA_GPS_COURSE*0.01; // scaled by 0.01 to get ground course in degrees
njewin 4:8dcf0bdc25c8 485
njewin 4:8dcf0bdc25c8 486 // Speed
njewin 4:8dcf0bdc25c8 487 MY_DATA_GPS_SPEED = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
njewin 4:8dcf0bdc25c8 488 MY_DATA_GPS_SPEED |= new_packet.packet_data[3];
njewin 8:0ce247da6370 489 data.GPS_speed = MY_DATA_GPS_SPEED*0.01; // scaled by 0.01 to get speed in m/s
njewin 4:8dcf0bdc25c8 490
njewin 4:8dcf0bdc25c8 491
njewin 4:8dcf0bdc25c8 492 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 493 }
njewin 4:8dcf0bdc25c8 494 //-------------------------------------------------------------------
njewin 4:8dcf0bdc25c8 495 // GPS long
njewin 4:8dcf0bdc25c8 496 if (new_packet.address == UM6_GPS_LONGITUDE) {
njewin 8:0ce247da6370 497 // Longitude
njewin 9:7dcfa24d5e7a 498 MY_DATA_GPS_LONG0 = (int32_t)new_packet.packet_data[0]<<24;
njewin 9:7dcfa24d5e7a 499 MY_DATA_GPS_LONG1 = (int32_t)new_packet.packet_data[1]<<16;
njewin 9:7dcfa24d5e7a 500 MY_DATA_GPS_LONG2 = (int32_t)new_packet.packet_data[2]<<8;
njewin 9:7dcfa24d5e7a 501 MY_DATA_GPS_LONG = MY_DATA_GPS_LONG0|MY_DATA_GPS_LONG1|MY_DATA_GPS_LONG2|new_packet.packet_data[3];
njewin 9:7dcfa24d5e7a 502 memcpy(&data.GPS_long,&MY_DATA_GPS_LONG,sizeof(int));
njewin 4:8dcf0bdc25c8 503 }
njewin 4:8dcf0bdc25c8 504 //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 505 //-------------------------------------------------------------------
njewin 4:8dcf0bdc25c8 506 // GPS lat
njewin 12:894e648638e4 507 if (new_packet.address == UM6_GPS_LATITUDE) {
njewin 4:8dcf0bdc25c8 508 // Latitude
njewin 10:d96e068f3595 509 //MY_DATA_GPS_LAT0 = (int32_t)new_packet.packet_data[0]<<24;
njewin 10:d96e068f3595 510 //MY_DATA_GPS_LAT1 = (int32_t)new_packet.packet_data[1]<<16;
njewin 10:d96e068f3595 511 //MY_DATA_GPS_LAT2 = (int32_t)new_packet.packet_data[2]<<8;
njewin 10:d96e068f3595 512 //MY_DATA_GPS_LAT = MY_DATA_GPS_LAT0|MY_DATA_GPS_LAT1|MY_DATA_GPS_LAT2|new_packet.packet_data[3];
njewin 10:d96e068f3595 513 //memcpy(&data.GPS_lat,&MY_DATA_GPS_LAT,sizeof(int));
njewin 10:d96e068f3595 514 // data.GPS_lat_raw = new_packet.packet_data[0];
njewin 4:8dcf0bdc25c8 515 }
njewin 4:8dcf0bdc25c8 516 //------------------------------------------------------------
njewin 10:d96e068f3595 517 //-------------------------------------------------------------------
njewin 10:d96e068f3595 518 // GPS altitude
njewin 10:d96e068f3595 519 if (new_packet.address == UM6_GPS_ALTITUDE) {
njewin 10:d96e068f3595 520 // Longitude
njewin 10:d96e068f3595 521 MY_DATA_GPS_ALT0 = (int32_t)new_packet.packet_data[0]<<24;
njewin 10:d96e068f3595 522 MY_DATA_GPS_ALT1 = (int32_t)new_packet.packet_data[1]<<16;
njewin 10:d96e068f3595 523 MY_DATA_GPS_ALT2 = (int32_t)new_packet.packet_data[2]<<8;
njewin 10:d96e068f3595 524 MY_DATA_GPS_ALT = MY_DATA_GPS_ALT0|MY_DATA_GPS_ALT1|MY_DATA_GPS_ALT2|new_packet.packet_data[3];
njewin 10:d96e068f3595 525 memcpy(&data.GPS_alt,&MY_DATA_GPS_ALT,sizeof(int));
njewin 10:d96e068f3595 526 // data.GPS_alt_raw = MY_DATA_GPS_ALT0;
njewin 10:d96e068f3595 527
njewin 10:d96e068f3595 528 }
njewin 10:d96e068f3595 529 */ //------------------------------------------------------------
njewin 4:8dcf0bdc25c8 530 } // end if(ADDRESS_TYPE_DATA)
njewin 4:8dcf0bdc25c8 531
njewin 4:8dcf0bdc25c8 532
njewin 4:8dcf0bdc25c8 533 // A full packet has been received.
njewin 4:8dcf0bdc25c8 534 // Put the USART back into the WAIT state and reset
njewin 4:8dcf0bdc25c8 535 // the data_counter variable so that it can be used to receive the next packet.
njewin 4:8dcf0bdc25c8 536 data_counter = 0;
njewin 4:8dcf0bdc25c8 537 gUSART_State = USART_STATE_WAIT;
njewin 4:8dcf0bdc25c8 538
njewin 4:8dcf0bdc25c8 539
njewin 4:8dcf0bdc25c8 540 } // end else(GET_DATA)
njewin 4:8dcf0bdc25c8 541
njewin 4:8dcf0bdc25c8 542 }
njewin 4:8dcf0bdc25c8 543 break;
njewin 4:8dcf0bdc25c8 544
njewin 4:8dcf0bdc25c8 545 } // end switch ( gUSART_State )
njewin 4:8dcf0bdc25c8 546
njewin 4:8dcf0bdc25c8 547 return;
njewin 4:8dcf0bdc25c8 548
njewin 4:8dcf0bdc25c8 549 } // end get_gyro_x()
njewin 4:8dcf0bdc25c8 550
njewin 8:0ce247da6370 551 #endif
njewin 8:0ce247da6370 552