Nathan Ewin / Mbed 2 deprecated UM6withGPS

Prints signals from UM6 orientation sensor (CH Robotics) and GPS receiver, using MODSerial and MODgps. GPS causes UM6 signals to freeze on a fixed value, which timer and gps continue to print out.

Dependencies:   MODGPS MODSERIAL mbed

UM6_config/UM6_config.h@0:b9c0180d446f, 2013-07-08 (annotated)

Committer:
njewin
Date:
Mon Jul 08 18:22:43 2013 +0000
Revision:
0:b9c0180d446f
prints um6 and gps signals to pc

Who changed what in which revision?

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