IMU.h

00001 #include "mbed.h"
00002 #include "MODSERIAL.h"
00003 
00004 // for Microstrain's MIPS protocol, try this link, or search on microstrain.com
00005 // http://www.microstrain.com/sites/default/files/3dm-gx5-45_dcp_manual_8500-0064_0.pdf
00006 
00007 #define _PI ((float) 3.14159265359)
00008 
00009 // state machine states
00010 #define SYNC0 0 // first sync byte state
00011 #define SYNC1 1 // second sync byte state
00012 #define SET   2 // aka descriptor set
00013 #define LEN   3 // payload length
00014 #define PAY   4 // getting the payload data
00015 #define CRC0  5 // crc high byte
00016 #define CRC1  6 // crc low byte
00017 
00018 // data set descriptors
00019 #define IMU_DATA_SET                0x80
00020 #define GNSS_DATA_SET               0x81
00021 
00022 // enumerations for the Euler angle packet we care about
00023 #define EULER_CF_LENGTH             14
00024 #define EULER_CF_DESCRIPTOR         0x0C
00025 #define ROLL_OFFSET                 0
00026 #define PITCH_OFFSET                4
00027 #define YAW_OFFSET                  8
00028 
00029 // enumerations for the lat-lon-alt packet we care about
00030 #define LLH_POSITION_LENGTH         44
00031 #define LLH_POSITION_DESCRIPTOR     0x03
00032 #define LATITUDE_OFFSET             0
00033 #define LONGITUDE_OFFSET            8
00034 #define HEIGHT_MSL_OFFSET           16
00035 #define VALID_FLAG_OFFSET           40
00036 
00037 // enumerations for the lat-lon-alt packet we care about
00038 #define GNSS_FIX_INFO_LENGTH        8
00039 #define GNSS_FIX_INFO_DESCRIPTOR    0x0B
00040 #define FIX_TYPE_OFFSET             0
00041 #define NUM_SV_OFFSET               1
00042 #define VALID_FLAGS_OFFSET          4
00043 
00044 class IMU {
00045 public:
00046     IMU(PinName Tx, PinName Rx);
00047     void initialize();
00048     void update();
00049     void start();
00050     void stop();
00051     
00052     void runIMU();
00053     
00054     float getRoll();
00055     float getPitch();
00056     float getHeading();
00057    
00058     bool getIsValid2dFix();
00059     bool getIsValid3dFix();
00060     char getNumSV();
00061     double getLatitude();
00062     double getLongitude();
00063     double getAltitudeMSL();
00064     
00065     protected:
00066     Ticker interval;
00067     MODSERIAL _rs232;
00068     
00069     char byte;
00070     
00071     unsigned char state, len, descriptor, i, packet[256];
00072     unsigned int checksum, crc0, crc1;
00073     
00074 /*    unsigned char state, len, descriptor, i, crc0, crc1, payload[30];
00075     unsigned int checksum;*/
00076     
00077     float euler[3];
00078     double latLonAlt[3];
00079     bool is2dFixValid;
00080     bool is3dFixValid;
00081     char numSV;
00082 
00083     void processPayload(char type, char length, unsigned char * payload);
00084     void processEulerCfPacket(char length, unsigned char * payload);
00085     void processLatLonAltPacket(char length, unsigned char * payload);
00086     void processGnssFixInformation(char length, unsigned char * payload);
00087 
00088     unsigned int calcChecksum(unsigned char * mip_packet, char checksum_range);
00089 
00090     float floatFromChar(unsigned char * value);
00091     double doubleFromChar(unsigned char * value);
00092 };