Nathan Ewin
Prints signals from UM6 orientation sensor (CH Robotics) and GPS receiver, using MODSerial and MODgps. GPS causes UM6 signals to freeze on a fixed value, which timer and gps continue to print out.
Dependencies: MODGPS MODSERIAL mbed
Revision 0:b9c0180d446f, committed 2013-07-08
- Comitter:
- njewin
- Date:
- Mon Jul 08 18:22:43 2013 +0000
- Commit message:
- prints um6 and gps signals to pc
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODGPS.lib Mon Jul 08 18:22:43 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mprinke/code/MODGPS/#34a9030f27a4
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Mon Jul 08 18:22:43 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/AjK/code/MODSERIAL/#ae0408ebdd68
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UM6_config/UM6_config.h Mon Jul 08 18:22:43 2013 +0000
@@ -0,0 +1,553 @@
+/* ------------------------------------------------------------------------------
+ File: UM6_config.h
+ Author: CH Robotics, edited by LHiggs, & Nathan Ewin
+ Version: 1.0
+
+ Description: Preprocessor definitions and function declarations for UM6 configuration
+
+ // added configuration for GPS signals from LS20031 sensor connected to UM6
+ // GPS ground speed and heading angle setup outputs data
+ // GPS latitude and longitude not setup correctly
+------------------------------------------------------------------------------ */
+#ifndef __UM6_CONFIG_H
+#define __UM6_CONFIG_H
+
+#include "UM6_usart.h"
+
+
+
+MODSERIAL um6_uart(p13, p14); // UM6 SERIAL OVER UART PINS 26 & 25
+
+
+
+
+
+
+
+
+// CONFIG_ARRAY_SIZE and DATA_ARRAY_SIZE specify the number of 32 bit configuration and data registers used by the firmware
+// (Note: The term "register" is used loosely here. These "registers" are not actually registers in the same sense of a
+// microcontroller register. They are simply index locations into arrays stored in global memory. Data and configuration
+// parameters are stored in arrays because it allows a common communication protocol to be used to access all data and
+// configuration. The software communicating with the sensor needs only specify the register address, and the communication
+// software running on the sensor knows exactly where to find it - it needn't know what the data is. The software communicatin
+// with the sensor, on the other hand, needs to know what it is asking for (naturally...)
+// This setup makes it easy to make more data immediately available when needed - simply increase the array size, add code in
+// the firmware that writes data to the new array location, and then make updates to the firmware definition on the PC side.
+#define CONFIG_ARRAY_SIZE 44
+#define DATA_ARRAY_SIZE 13
+#define COMMAND_COUNT 9
+
+// original data array size 32
+//
+#define CONFIG_REG_START_ADDRESS 0
+#define DATA_REG_START_ADDRESS 85
+#define COMMAND_START_ADDRESS 170
+
+// hex 0x55 = dec 85
+// hex 0xAA = dec 170
+// These preprocessor definitions make it easier to access specific configuration parameters in code
+// They specify array locations associated with each register name. Note that in the comments below, many of the values are
+// said to be 32-bit IEEE floating point. Obviously this isn't directly the case, since the arrays are actually 32-bit unsigned
+// integer arrays. Bit for bit, the data does correspond to the correct floating point value. Since you can't cast ints as floats,
+// special conversion has to happen to copy the float data to and from the array.
+// Starting with configuration register locations...
+
+
+// Now for data register locations.
+// 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
+// to produce the actual array index labeled below
+#define UM6_STATUS DATA_REG_START_ADDRESS // Status register defines error codes with individual bits
+#define UM6_GYRO_RAW_XY (DATA_REG_START_ADDRESS + 1) // Raw gyro data is stored in 16-bit signed integers
+#define UM6_GYRO_RAW_Z (DATA_REG_START_ADDRESS + 2)
+#define UM6_ACCEL_RAW_XY (DATA_REG_START_ADDRESS + 3) // Raw accel data is stored in 16-bit signed integers
+#define UM6_ACCEL_RAW_Z (DATA_REG_START_ADDRESS + 4)
+//#define UM6_MAG_RAW_XY (DATA_REG_START_ADDRESS + 5) // Raw mag data is stored in 16-bit signed integers
+//#define UM6_MAG_RAW_Z (DATA_REG_START_ADDRESS + 6)
+#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.
+#define UM6_GYRO_PROC_Z (DATA_REG_START_ADDRESS + 8)
+#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.
+#define UM6_ACCEL_PROC_Z (DATA_REG_START_ADDRESS + 10)
+//#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.
+//#define UM6_MAG_PROC_Z (DATA_REG_START_ADDRESS + 12)
+#define UM6_EULER_PHI_THETA (DATA_REG_START_ADDRESS + 13) // Euler angles are 32-bit IEEE floating point
+#define UM6_EULER_PSI (DATA_REG_START_ADDRESS + 14)
+#define UM6_QUAT_AB (DATA_REG_START_ADDRESS + 15) // Quaternions are 16-bit signed integers.
+#define UM6_QUAT_CD (DATA_REG_START_ADDRESS + 16)
+/*
+#define UM6_ERROR_COV_00 (DATA_REG_START_ADDRESS + 17) // Error covariance is a 4x4 matrix of 32-bit IEEE floating point values
+#define UM6_ERROR_COV_01 (DATA_REG_START_ADDRESS + 18)
+#define UM6_ERROR_COV_02 (DATA_REG_START_ADDRESS + 19)
+#define UM6_ERROR_COV_03 (DATA_REG_START_ADDRESS + 20)
+#define UM6_ERROR_COV_10 (DATA_REG_START_ADDRESS + 21)
+#define UM6_ERROR_COV_11 (DATA_REG_START_ADDRESS + 22)
+#define UM6_ERROR_COV_12 (DATA_REG_START_ADDRESS + 23)
+#define UM6_ERROR_COV_13 (DATA_REG_START_ADDRESS + 24)
+#define UM6_ERROR_COV_20 (DATA_REG_START_ADDRESS + 25)
+#define UM6_ERROR_COV_21 (DATA_REG_START_ADDRESS + 26)
+#define UM6_ERROR_COV_22 (DATA_REG_START_ADDRESS + 27)
+#define UM6_ERROR_COV_23 (DATA_REG_START_ADDRESS + 28)
+#define UM6_ERROR_COV_30 (DATA_REG_START_ADDRESS + 29)
+#define UM6_ERROR_COV_31 (DATA_REG_START_ADDRESS + 30)
+#define UM6_ERROR_COV_32 (DATA_REG_START_ADDRESS + 31)
+#define UM6_ERROR_COV_33 (DATA_REG_START_ADDRESS + 32)
+*/
+// connected gps module directing to mbed using MODGPS
+//#define UM6_GPS_LONGITUDE (DATA_REG_START_ADDRESS + 34)
+//#define UM6_GPS_LATITUDE (DATA_REG_START_ADDRESS + 35)
+//#define UM6_GPS_ALTITUDE (DATA_REG_START_ADDRESS + 36)
+//#define UM6_GPS_COURSE_SPEED (DATA_REG_START_ADDRESS + 40)
+
+
+
+
+
+
+
+/*******************************************************************************
+* Function Name : ComputeChecksum
+* Input : USARTPacket* new_packet
+* Output : None
+* Return : uint16_t
+* Description : Returns the two byte sum of all the individual bytes in the
+ given packet.
+*******************************************************************************/
+uint16_t ComputeChecksum( USARTPacket* new_packet ) {
+ int32_t index;
+ uint16_t checksum = 0x73 + 0x6E + 0x70 + new_packet->PT + new_packet->address;
+
+ for ( index = 0; index < new_packet->data_length; index++ ) {
+ checksum += new_packet->packet_data[index];
+ }
+ return checksum;
+}
+
+
+
+
+
+static USARTPacket new_packet;
+
+// Flag for storing the current USART state
+uint8_t gUSART_State = USART_STATE_WAIT;
+
+
+struct UM6{
+float Gyro_Proc_X;
+float Gyro_Proc_Y;
+float Gyro_Proc_Z;
+float Accel_Proc_X;
+float Accel_Proc_Y;
+float Accel_Proc_Z;
+//float Mag_Proc_X;
+//float Mag_Proc_Y;
+//float Mag_Proc_Z;
+float Roll;
+float Pitch;
+float Yaw;
+//float GPS_long;
+//float GPS_lat;
+//float GPS_alt;
+//float GPS_course;
+//float GPS_speed;
+
+};
+UM6 data;
+
+
+
+
+void Process_um6_packet() {
+
+int16_t MY_DATA_GYRO_PROC_X;
+int16_t MY_DATA_GYRO_PROC_Y;
+int16_t MY_DATA_GYRO_PROC_Z;
+int16_t MY_DATA_ACCEL_PROC_X;
+int16_t MY_DATA_ACCEL_PROC_Y;
+int16_t MY_DATA_ACCEL_PROC_Z;
+//int16_t MY_DATA_MAG_PROC_X;
+//int16_t MY_DATA_MAG_PROC_Y;
+//int16_t MY_DATA_MAG_PROC_Z;
+int16_t MY_DATA_EULER_PHI;
+int16_t MY_DATA_EULER_THETA;
+int16_t MY_DATA_EULER_PSI;
+/*
+int32_t MY_DATA_GPS_LONG;
+int32_t MY_DATA_GPS_LONG0;
+int32_t MY_DATA_GPS_LONG1;
+int32_t MY_DATA_GPS_LONG2;
+int32_t MY_DATA_GPS_LAT;
+int32_t MY_DATA_GPS_LAT0;
+int32_t MY_DATA_GPS_LAT1;
+int32_t MY_DATA_GPS_LAT2;
+int32_t MY_DATA_GPS_ALT;
+int32_t MY_DATA_GPS_ALT0;
+int32_t MY_DATA_GPS_ALT1;
+int32_t MY_DATA_GPS_ALT2;
+int16_t MY_DATA_GPS_COURSE;
+int16_t MY_DATA_GPS_SPEED;
+*/
+
+
+static uint8_t data_counter = 0;
+
+
+
+ // The next action should depend on the USART state.
+ switch ( gUSART_State ) {
+ // USART in the WAIT state. In this state, the USART is waiting to see the sequence of bytes
+ // that signals a new incoming packet.
+ case USART_STATE_WAIT:
+ if ( data_counter == 0 ) { // Waiting on 's' character
+ if ( um6_uart.getc() == 's' ) {
+
+ data_counter++;
+ } else {
+ data_counter = 0;
+ }
+ } else if ( data_counter == 1 ) { // Waiting on 'n' character
+ if ( um6_uart.getc() == 'n' ) {
+ data_counter++;
+
+ } else {
+ data_counter = 0;
+ }
+ } else if ( data_counter == 2 ) { // Waiting on 'p' character
+ if ( um6_uart.getc() == 'p' ) {
+ // The full 'snp' sequence was received. Reset data_counter (it will be used again
+ // later) and transition to the next state.
+ data_counter = 0;
+ gUSART_State = USART_STATE_TYPE;
+
+ } else {
+ data_counter = 0;
+ }
+ }
+ break;
+
+ // USART in the TYPE state. In this state, the USART has just received the sequence of bytes that
+ // indicates a new packet is about to arrive. Now, the USART expects to see the packet type.
+ case USART_STATE_TYPE:
+
+ new_packet.PT = um6_uart.getc();
+
+ gUSART_State = USART_STATE_ADDRESS;
+
+ break;
+
+ // USART in the ADDRESS state. In this state, the USART expects to receive a single byte indicating
+ // the address that the packet applies to
+ case USART_STATE_ADDRESS:
+ new_packet.address = um6_uart.getc();
+
+ // For convenience, identify the type of packet this is and copy to the packet structure
+ // (this will be used by the packet handler later)
+ if ( (new_packet.address >= CONFIG_REG_START_ADDRESS) && (new_packet.address < DATA_REG_START_ADDRESS) ) {
+ new_packet.address_type = ADDRESS_TYPE_CONFIG;
+ } else if ( (new_packet.address >= DATA_REG_START_ADDRESS) && (new_packet.address < COMMAND_START_ADDRESS) ) {
+ new_packet.address_type = ADDRESS_TYPE_DATA;
+ } else {
+ new_packet.address_type = ADDRESS_TYPE_COMMAND;
+ }
+
+ // Identify the type of communication this is (whether reading or writing to a data or configuration register, or sending a command)
+ // If this is a read operation, jump directly to the USART_STATE_CHECKSUM state - there is no more data in the packet
+ if ( (new_packet.PT & PACKET_HAS_DATA) == 0 ) {
+ gUSART_State = USART_STATE_CHECKSUM;
+ }
+
+ // If this is a write operation, go to the USART_STATE_DATA state to read in the relevant data
+ else {
+ gUSART_State = USART_STATE_DATA;
+ // Determine the expected number of bytes in this data packet based on the packet type. A write operation
+ // consists of 4 bytes unless it is a batch operation, in which case the number of bytes equals 4*batch_size,
+ // where the batch size is also given in the packet type byte.
+ if ( new_packet.PT & PACKET_IS_BATCH ) {
+ new_packet.data_length = 4*((new_packet.PT >> 2) & PACKET_BATCH_LENGTH_MASK);
+
+ } else {
+ new_packet.data_length = 4;
+ }
+ }
+ break;
+
+ // USART in the DATA state. In this state, the USART expects to receive new_packet.length bytes of
+ // data.
+ case USART_STATE_DATA:
+ new_packet.packet_data[data_counter] = um6_uart.getc();
+ data_counter++;
+
+ // If the expected number of bytes has been received, transition to the CHECKSUM state.
+ if ( data_counter == new_packet.data_length ) {
+ // Reset data_counter, since it will be used in the CHECKSUM state.
+ data_counter = 0;
+ gUSART_State = USART_STATE_CHECKSUM;
+ }
+ break;
+
+
+
+ // USART in CHECKSUM state. In this state, the entire packet has been received, with the exception
+ // of the 16-bit checksum.
+ case USART_STATE_CHECKSUM:
+ // Get the highest-order byte
+ if ( data_counter == 0 ) {
+ new_packet.checksum = ((uint16_t)um6_uart.getc() << 8);
+ data_counter++;
+ } else { // ( data_counter == 1 )
+ // Get lower-order byte
+ new_packet.checksum = new_packet.checksum | ((uint16_t)um6_uart.getc() & 0x0FF);
+
+ // Both checksum bytes have been received. Make sure that the checksum is valid.
+ uint16_t checksum = ComputeChecksum( &new_packet );
+
+
+
+ // If checksum does not match, exit function
+ if ( checksum != new_packet.checksum ) {
+ return;
+ } // end if(checksum check)
+
+
+
+ else
+
+ {
+
+ // Packet was received correctly.
+
+ //-----------------------------------------------------------------------------------------------
+ //-----------------------------------------------------------------------------------------------
+ //
+ // CHECKSUM WAS GOOD SO GET ARE DATA!!!!!!!!!!!!
+
+
+ // IF DATA ADDRESS
+ if (new_packet.address_type == ADDRESS_TYPE_DATA) {
+
+ //------------------------------------------------------------
+ // UM6_GYRO_PROC_XY (0x5C)
+ // To convert the register data from 16-bit 2's complement integers to actual angular rates in degrees
+ // per second, the data should be multiplied by the scale factor 0.0610352 as shown below
+ // angular_rate = register_data*0.0610352
+
+ if (new_packet.address == UM6_GYRO_PROC_XY) {
+
+ // GYRO_PROC_X
+ MY_DATA_GYRO_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_GYRO_PROC_X |= new_packet.packet_data[1];
+ data.Gyro_Proc_X = MY_DATA_GYRO_PROC_X*0.0610352;
+
+ MY_DATA_GYRO_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_GYRO_PROC_Y |= new_packet.packet_data[3];
+ data.Gyro_Proc_Y = MY_DATA_GYRO_PROC_Y*0.0610352;
+
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_GYRO_PROC_Z (0x5D)
+ // To convert the register data from a 16-bit 2's complement integer to the actual angular rate in
+ // degrees per second, the data should be multiplied by the scale factor 0.0610352 as shown below.
+
+
+ // GYRO_PROC_Z
+ MY_DATA_GYRO_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_GYRO_PROC_Z |= new_packet.packet_data[5];;
+ data.Gyro_Proc_Z = MY_DATA_GYRO_PROC_Z*0.0610352;
+
+
+ } // end if(MY_DATA_GYRO_PROC)
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_ACCEL_PROC_XY (0x5E)
+ // To convert the register data from 16-bit 2's complement integers to actual acceleration in gravities,
+ // the data should be multiplied by the scale factor 0.000183105 as shown below.
+ // acceleration = register_data* 0.000183105
+ if (new_packet.address == UM6_ACCEL_PROC_XY) {
+
+ // ACCEL_PROC_X
+ MY_DATA_ACCEL_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_ACCEL_PROC_X |= new_packet.packet_data[1];
+ data.Accel_Proc_X = MY_DATA_ACCEL_PROC_X*0.000183105;
+
+
+ // ACCEL_PROC_Y
+ MY_DATA_ACCEL_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_ACCEL_PROC_Y |= new_packet.packet_data[3];
+ data.Accel_Proc_Y = MY_DATA_ACCEL_PROC_Y*0.000183105;
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_ACCEL_PROC_Z (0x5F)
+ // To convert the register data from a 16-bit 2's complement integer to the actual acceleration in
+ // gravities, the data should be multiplied by the scale factor 0.000183105 as shown below.
+
+ // ACCEL_PROC_Z
+ MY_DATA_ACCEL_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_ACCEL_PROC_Z |= new_packet.packet_data[5];
+ data.Accel_Proc_Z = MY_DATA_ACCEL_PROC_Z*0.000183105;
+
+ } // end if(MY_DATA_ACCEL_PROC)
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_MAG_PROC_XY (0x60)
+ // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
+ // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
+ // shown below.
+ // magnetic field = register_data* 0.000305176
+ /*
+ if (new_packet.address == UM6_MAG_PROC_XY) {
+
+ // MAG_PROC_X
+ MY_DATA_MAG_PROC_X = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_MAG_PROC_X |= new_packet.packet_data[1];
+ data.Mag_Proc_X = MY_DATA_MAG_PROC_X*0.000305176;
+
+
+ // MAG_PROC_Y
+ MY_DATA_MAG_PROC_Y = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_MAG_PROC_Y |= new_packet.packet_data[3];
+ data.Mag_Proc_Y = MY_DATA_MAG_PROC_Y*0.000305176;
+
+ //------------------------------------------------------------
+
+
+ //------------------------------------------------------------
+ // UM6_MAG_PROC_Z (0x61)
+ // To convert the register data from 16-bit 2's complement integers to a unit-norm (assuming proper
+ // calibration) magnetic-field vector, the data should be multiplied by the scale factor 0.000305176 as
+ // shown below.
+ // magnetic field = register_data*0.000305176
+
+ // MAG_PROC_Z
+ MY_DATA_MAG_PROC_Z = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_MAG_PROC_Z |= new_packet.packet_data[5];
+ data.Mag_Proc_Z = MY_DATA_MAG_PROC_Z*0.000305176;
+
+ } // end if(UM6_MAG_PROC)
+ */
+ //------------------------------------------------------------
+
+
+
+ //------------------------------------------------------------
+ // UM6_EULER_PHI_THETA (0x62)
+ // Stores the most recently computed roll (phi) and pitch (theta) angle estimates. The angle
+ // estimates are stored as 16-bit 2's complement integers. To obtain the actual angle estimate in
+ // degrees, the register data should be multiplied by the scale factor 0.0109863 as shown below
+ // angle estimate = register_data* 0.0109863
+ if (new_packet.address == UM6_EULER_PHI_THETA) {
+ // EULER_PHI (ROLL)
+ MY_DATA_EULER_PHI = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_EULER_PHI |= new_packet.packet_data[1];
+ data.Roll = MY_DATA_EULER_PHI*0.0109863;
+
+
+
+
+
+ // EULER_THETA (PITCH)
+ MY_DATA_EULER_THETA = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_EULER_THETA |= new_packet.packet_data[3];
+ data.Pitch = MY_DATA_EULER_THETA*0.0109863;
+
+ //------------------------------------------------------------
+
+ //------------------------------------------------------------
+ // UM6_EULER_PSI (0x63) (YAW)
+ // Stores the most recently computed yaw (psi) angle estimate. The angle estimate is stored as a 16-
+ // bit 2's complement integer. To obtain the actual angle estimate in degrees, the register data
+ // should be multiplied by the scale factor 0.0109863 as shown below
+
+ MY_DATA_EULER_PSI = (int16_t)new_packet.packet_data[4]<<8; //bitshift it
+ MY_DATA_EULER_PSI |= new_packet.packet_data[5];
+ data.Yaw = MY_DATA_EULER_PSI*0.0109863;
+
+
+ }
+ /*
+ //-------------------------------------------------------------------
+ // GPS Ground/Speed
+ if (new_packet.address == UM6_GPS_COURSE_SPEED) {
+ // Ground course
+ MY_DATA_GPS_COURSE = (int16_t)new_packet.packet_data[0]<<8; //bitshift it
+ MY_DATA_GPS_COURSE |= new_packet.packet_data[1];
+ data.GPS_course = MY_DATA_GPS_COURSE*0.01; // scaled by 0.01 to get ground course in degrees
+
+ // Speed
+ MY_DATA_GPS_SPEED = (int16_t)new_packet.packet_data[2]<<8; //bitshift it
+ MY_DATA_GPS_SPEED |= new_packet.packet_data[3];
+ data.GPS_speed = MY_DATA_GPS_SPEED*0.01; // scaled by 0.01 to get speed in m/s
+
+
+ //------------------------------------------------------------
+ }
+ //-------------------------------------------------------------------
+ // GPS long
+ if (new_packet.address == UM6_GPS_LONGITUDE) {
+ // Longitude
+ MY_DATA_GPS_LONG0 = (int32_t)new_packet.packet_data[0]<<24;
+ MY_DATA_GPS_LONG1 = (int32_t)new_packet.packet_data[1]<<16;
+ MY_DATA_GPS_LONG2 = (int32_t)new_packet.packet_data[2]<<8;
+ MY_DATA_GPS_LONG = MY_DATA_GPS_LONG0|MY_DATA_GPS_LONG1|MY_DATA_GPS_LONG2|new_packet.packet_data[3];
+ memcpy(&data.GPS_long,&MY_DATA_GPS_LONG,sizeof(int));
+ }
+ //------------------------------------------------------------
+ //-------------------------------------------------------------------
+ // GPS lat
+ if (new_packet.address == UM6_GPS_LATITUDE) {
+ // Latitude
+ //MY_DATA_GPS_LAT0 = (int32_t)new_packet.packet_data[0]<<24;
+ //MY_DATA_GPS_LAT1 = (int32_t)new_packet.packet_data[1]<<16;
+ //MY_DATA_GPS_LAT2 = (int32_t)new_packet.packet_data[2]<<8;
+ //MY_DATA_GPS_LAT = MY_DATA_GPS_LAT0|MY_DATA_GPS_LAT1|MY_DATA_GPS_LAT2|new_packet.packet_data[3];
+ //memcpy(&data.GPS_lat,&MY_DATA_GPS_LAT,sizeof(int));
+ // data.GPS_lat_raw = new_packet.packet_data[0];
+ }
+ //------------------------------------------------------------
+ //-------------------------------------------------------------------
+ // GPS altitude
+ if (new_packet.address == UM6_GPS_ALTITUDE) {
+ // Longitude
+ MY_DATA_GPS_ALT0 = (int32_t)new_packet.packet_data[0]<<24;
+ MY_DATA_GPS_ALT1 = (int32_t)new_packet.packet_data[1]<<16;
+ MY_DATA_GPS_ALT2 = (int32_t)new_packet.packet_data[2]<<8;
+ MY_DATA_GPS_ALT = MY_DATA_GPS_ALT0|MY_DATA_GPS_ALT1|MY_DATA_GPS_ALT2|new_packet.packet_data[3];
+ memcpy(&data.GPS_alt,&MY_DATA_GPS_ALT,sizeof(int));
+ // data.GPS_alt_raw = MY_DATA_GPS_ALT0;
+
+ }
+ */ //------------------------------------------------------------
+ } // end if(ADDRESS_TYPE_DATA)
+
+
+ // A full packet has been received.
+ // Put the USART back into the WAIT state and reset
+ // the data_counter variable so that it can be used to receive the next packet.
+ data_counter = 0;
+ gUSART_State = USART_STATE_WAIT;
+
+
+ } // end else(GET_DATA)
+
+ }
+ break;
+
+ } // end switch ( gUSART_State )
+
+return;
+
+ } // end get_gyro_x()
+
+#endif
+
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/UM6_usart/UM6_usart.h Mon Jul 08 18:22:43 2013 +0000
@@ -0,0 +1,60 @@
+/* ______________________________________________________________________________________
+ File: UM6_usart.h
+ Author: CH Robotics, adapted for mbed by lhiggs
+ Version: 1.0
+
+ Description: Function declarations for USART communucation
+ -------------------------------------------------------------------------------------- */
+
+ #ifndef _CHR_USART_H
+ #define _CHR_USART_H
+
+ #define MAX_PACKET_DATA 40
+
+ // Definitions of states for USART receiver state machine (for receiving packets)
+ #define USART_STATE_WAIT 1
+ #define USART_STATE_TYPE 2
+ #define USART_STATE_ADDRESS 3
+ #define USART_STATE_DATA 4
+ #define USART_STATE_CHECKSUM 5
+
+ // Flags for interpreting the packet type byte in communication packets
+ #define PACKET_HAS_DATA (1 << 7)
+ #define PACKET_IS_BATCH (1 << 6)
+ #define PACKET_BATCH_LENGTH_MASK ( 0x0F )
+
+ #define PACKET_BATCH_LENGTH_OFFSET 2
+
+ #define BATCH_SIZE_2 2
+ #define BATCH_SIZE_3 3
+
+ #define PACKET_NO_DATA 0
+ #define PACKET_COMMAND_FAILED (1 << 0)
+
+
+ // Define flags for identifying the type of packet address received
+ #define ADDRESS_TYPE_CONFIG 0
+ #define ADDRESS_TYPE_DATA 1
+ #define ADDRESS_TYPE_COMMAND 2
+
+
+extern uint8_t gUSART_State;
+
+ // Structure for storing TX and RX packet data
+ typedef struct _USARTPacket
+ {
+ uint8_t PT; // Packet type
+ uint8_t address; // Packet address
+ uint16_t checksum; // Checksum
+
+ // Data included for convenience, but that isn't stored in the packet itself
+ uint8_t data_length; // Number of bytes in data section
+ uint8_t address_type; // Specified the address type (DATA, CONFIG, OR COMMAND)
+
+ uint8_t packet_data[MAX_PACKET_DATA];
+
+ } USARTPacket;
+
+uint16_t ComputeChecksum( USARTPacket* new_packet );
+
+#endif
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Mon Jul 08 18:22:43 2013 +0000
@@ -0,0 +1,55 @@
+#include "mbed.h"
+#include "MODSERIAL.h"
+#include "UM6_usart.h" // UM6 USART HEADER
+#include "UM6_config.h" // UM6 CONFIG HEADER
+#include "GPS.h"
+
+//------------ system and interface setup ----------------------------//
+Serial pc(USBTX, USBRX); // sets up serial connection to pc terminal
+
+//------------ Hardware setup ----------------------------------------//
+DigitalOut pc_led(LED1); // LED1 = PC SERIAL
+DigitalOut uart_led(LED2); // LED2 = UM6 SERIAL
+GPS gps(NC,p27);
+
+
+// interupt function for processing uart messages --------------------//
+void rxCallback(MODSERIAL_IRQ_INFO *q) {
+ if (um6_uart.rxBufferGetCount() >= MAX_PACKET_DATA) {
+ uart_led = !uart_led; // Lights LED when uart RxBuff has > 40 bytes
+ Process_um6_packet();
+ }
+}
+
+
+//============= Main Program =========================================//
+int main() {
+ Timer t;
+ GPS_Time q1;
+ GPS_VTG v1;
+ int Roll_Counter=0;
+
+ pc.baud(115200); // baud rate to pc interface
+ um6_uart.baud(115200); // baud rate to um6 interface
+ gps.baud(115200);
+ gps.format(8, GPS::None, 1);
+
+ //---- call interrupt functions --------------------------//
+ um6_uart.attach(&rxCallback, MODSERIAL::RxIrq); // attach interupt function to uart
+ t.start(); // start logging timer
+
+ //---- main while loop -------------------------------------------//
+ while(1) {
+ Roll_Counter++;
+ if (Roll_Counter > 10000000) {
+
+ gps.vtg(&v1);
+
+ pc.printf("time %f s, Yaw %f deg, speed %f, longitude %f \n ",t.read(),data.Yaw,v1.velocity_kph(),gps.longitude());
+ pc_led = !pc_led;
+ Roll_Counter = 0;
+ }
+
+ } // end while(1) loop
+
+} // end main() program
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Mon Jul 08 18:22:43 2013 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/b3110cd2dd17 \ No newline at end of file