UM6LT.h

00001 #ifndef MBED_UM6LT_H
00002 #define MBED_UM6LT_H
00003 
00004 #include "mbed.h"
00005 
00006 // UM6 Configuration Registers
00007 
00008     #define UM6_COMMUNICATION           0x00
00009     #define UM6_MISC_CONFIG             0x01
00010     #define UM6_MAG_REF_X               0x02
00011     #define UM6_MAG_REF_Y               0x03
00012     #define UM6_MAG_REF_Z               0x04
00013     #define UM6_ACCEL_REF_X             0x05
00014     #define UM6_ACCEL_REF_Y             0x06
00015     #define UM6_ACCEL_REF_Z             0x07
00016     #define UM6_EKF_MAG_VARIANCE        0x08
00017     #define UM6_EKF_ACCEL_VARIANCE      0x09
00018     #define UM6_EKF_PROCESS_VARIANCE    0x0A
00019     #define UM6_GYRO_BIAS_XY            0x0B
00020     #define UM6_GYRO_BIAS_Z             0x0C
00021     #define UM6_ACCEL_BIAS_XY           0x0D
00022     #define UM6_ACCEL_BIAS_Z            0x0E
00023     #define UM6_MAG_BIAS_XY             0x0F
00024     #define UM6_MAG_BIAS_Z              0x10
00025     #define UM6_ACCEL_CAL_00            0x11
00026     #define UM6_ACCEL_CAL_01            0x12
00027     #define UM6_ACCEL_CAL_02            0x13
00028     #define UM6_ACCEL_CAL_10            0x14
00029     #define UM6_ACCEL_CAL_11            0x15
00030     #define UM6_ACCEL_CAL_12            0x16
00031     #define UM6_ACCEL_CAL_20            0x17
00032     #define UM6_ACCEL_CAL_21            0x18
00033     #define UM6_ACCEL_CAL_22            0x19
00034     #define UM6_GYRO_CAL_00             0x1A
00035     #define UM6_GYRO_CAL_01             0x1B
00036     #define UM6_GYRO_CAL_02             0x1C
00037     #define UM6_GYRO_CAL_10             0x1D
00038     #define UM6_GYRO_CAL_11             0x1E
00039     #define UM6_GYRO_CAL_12             0x1F
00040     #define UM6_GYRO_CAL_20             0x20
00041     #define UM6_GYRO_CAL_21             0x21
00042     #define UM6_GYRO_CAL_22             0x22
00043     #define UM6_MAG_CAL_00              0x23
00044     #define UM6_MAG_CAL_01              0x24
00045     #define UM6_MAG_CAL_02              0x25
00046     #define UM6_MAG_CAL_10              0x26
00047     #define UM6_MAG_CAL_11              0x27
00048     #define UM6_MAG_CAL_12              0x28
00049     #define UM6_MAG_CAL_20              0x29
00050     #define UM6_MAG_CAL_21              0x2A
00051     #define UM6_MAG_CAL_22              0x2B
00052             
00053 // UM6 Data Registers
00054 
00055     #define UM6_STATUS                  0x55
00056     #define UM6_GYRO_RAW_XY             0x56
00057     #define UM6_GYRO_RAW_Z              0x57
00058     #define UM6_ACCEL_RAW_XY            0x58
00059     #define UM6_ACCEL_RAW_Z             0x59
00060     #define UM6_MAG_RAW_XY              0x5A
00061     #define UM6_MAG_RAW_Z               0x5B
00062     #define UM6_GYRO_PROC_XY            0x5C
00063     #define UM6_GYRO_PROC_Z             0x5D
00064     #define UM6_ACCEL_PROC_XY           0x5E
00065     #define UM6_ACCEL_PROC_Z            0x5F
00066     #define UM6_MAG_PROC_XY             0x60
00067     #define UM6_MAG_PROC_Z              0x61
00068     #define UM6_EULER_PHI_THETA         0x62
00069     #define UM6_EULER_PSI               0x63
00070     #define UM6_QUAT_AB                 0x64
00071     #define UM6_QUAT_CD                 0x65
00072     #define UM6_ERROR_COV_00            0x66
00073     #define UM6_ERROR_COV_01            0x67
00074     #define UM6_ERROR_COV_02            0x68
00075     #define UM6_ERROR_COV_03            0x69
00076     #define UM6_ERROR_COV_10            0x6A
00077     #define UM6_ERROR_COV_11            0x6B
00078     #define UM6_ERROR_COV_12            0x6C
00079     #define UM6_ERROR_COV_13            0x6D
00080     #define UM6_ERROR_COV_20            0x6E
00081     #define UM6_ERROR_COV_21            0x6F
00082     #define UM6_ERROR_COV_22            0x70
00083     #define UM6_ERROR_COV_23            0x71
00084     #define UM6_ERROR_COV_30            0x72
00085     #define UM6_ERROR_COV_31            0x73
00086     #define UM6_ERROR_COV_32            0x74
00087     #define UM6_ERROR_COV_33            0x75
00088 
00089 // UM6 Command Registers
00090 
00091     #define UM6_GET_FW_VERSION          0xAA
00092     #define UM6_FLASH_COMMIT            0xAB
00093     #define UM6_ZERO_GYROS              0xAC
00094     #define UM6_RESET_EKF               0xAD
00095     #define UM6_GET_DATA                0xAE
00096     #define UM6_SET_ACCEL_REF           0xAF
00097     #define UM6_SET_MAG_REF             0xB0
00098     #define UM6_RESET_TO_FACTORY        0xB1
00099     
00100     #define UM6_BAD_CHECKSUM            0xFD
00101     #define UM6_UNKNOWN_ADDRESS         0xFE
00102     #define UM6_INVALID_BATCH_SIZE      0xFF
00103     
00104 // UM6 scale factors
00105 
00106     #define UM6_ANGLE_FACTOR            0.0109863
00107     #define UM6_ACCEL_FACTOR            0.000183105
00108     #define UM6_MAG_FACTOR              0.000305176
00109     #define UM6_ANGLE_RATE_FACTOR       0.0610352
00110     #define UM6_QUATERNION_FACTOR       0.0000335693
00111 
00112 class UM6LT {
00113 
00114     public:
00115     
00116         UM6LT (PinName tx, PinName rx);
00117         
00118         void baud(int baudrate);
00119         int  readable(void);
00120         char getc(void);
00121         void putc(char byte);
00122         
00123         void setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled);
00124         void setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate);
00125         int  getStatus(void);
00126         
00127         int zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ);
00128         int autoSetAccelRef(void);
00129         int autoSetMagRef(void);
00130         int resetEKF(void);
00131         int writeToFlash(void);
00132         int resetToFactory(void);
00133         
00134         int getAngles(int& roll, int& pitch, int& yaw); // in degrees
00135         int getAccel(int& accelX, int& accelY, int& accelZ); // in milli-gravity
00136         int getMag(int& magX, int& magY, int& magZ); // in milli-unit. the norm of the vector should be one
00137         int getAngleRates(int& rollRate, int& pitchRate, int& yawRate); // in degree/sec
00138         int getQuaternion(int& a, int& b, int& c, int& d); // in milli-unit
00139         
00140     private:
00141     
00142         Serial serial_;
00143         int stdDelayMs;
00144                 
00145         int  verifyChecksum(int byte1, int byte2);
00146         void createChecksum(int checksum_dec, int& byte1, int& byte2);
00147         void createByte(int* bitsList, int& byte);
00148         void decomposeByte(int* bitsList, int byte);        
00149         int  twosComplement(int byte1, int byte2);
00150         
00151         void oneWordWrite(int hasData, int isBatch, int BL, int address, int* data);
00152         void oneWordRead(int& PT, int& N, int& address, int* data);
00153         void requestData(int address, int expectedBL);
00154         int  sendOneCommand(int address);
00155         int  getTwoBatches(int address, int* data);
00156         
00157         int  noCommError(int address, int addressExpected, int PT);
00158         void WhatCommError(int addressRead, int addressExpected, int PT);
00159         
00160 };
00161 
00162 #endif /* MBED_UM6LT_H */