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