Hochschule München / Mbed 2 deprecated SatelitenSimulator

UM6 on AirBearing

Dependencies:   MODSERIAL mbed

UM6_config/UM6_config.h@0:8a2c63ece2a9, 2014-04-10 (annotated)

Committer:
HMFK03LST1
Date:
Thu Apr 10 11:17:09 2014 +0000
Revision:
0:8a2c63ece2a9
UM6 on AirBearing

Who changed what in which revision?

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