Dimitri Gruebel / Mbed 2 deprecated Reaction Wheel Actuated Satellite Dynamics Test Platform Featured

Reaction Wheel Actuated Satellite Dynamics Test Platform

Dependencies:   mbed

Diploma Thesis in Aerospace Engineering, January 2014

University of Applied Sciences Munich, Faculty 03

Electronics:

  • 1x mbed NXP LPC 1768 Microcontroller
  • 2x XBee S1 Radios + Sparkfun USB Adapter
  • 1x CHR UM6-lt IMU
  • 4x Graupner BEC 8 Motor Controllers
  • 4x ROXXY 2826/09 Brushless Motors
  • 1x Hacker TopFuel LiPo 1300mAh Battery
  • 1x big Selfmade BreakOutBoard to connect all components
  • 1x small BreakOutBoard to connect IMU

Hardware developed with Catia V5R20

Manufactoring Technology: Rapid Prototyping - EOS Formiga P110

Controlled via text based menu with DockLight

__________________

UM6_config/UM6_config.h@0:1447d2f773db, 2014-07-09 (annotated)

Committer:
DimitriGruebel
Date:
Wed Jul 09 07:35:50 2014 +0000
Revision:
0:1447d2f773db
Dynamics Test Platform

Who changed what in which revision?

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