Chris Charters / UM6LT

This is a revision of the UM6LT library written by acloitre. Revisions include changes to the getTwoBatches(...) function, additional changes carried over to its dependent functions, and an additional getPitchAngle() function.

Fork of UM6LT by Audren Cloitre

UM6LT.h@1:703ba8e8b2cd, 2015-07-21 (annotated)

Committer:
chartersauce25
Date:
Tue Jul 21 17:38:31 2015 +0000
Revision:
1:703ba8e8b2cd
Parent:
0:5651731cfb32 
Revised getTwoBatches(...) function to call the appropriate data registers. Changes carried over to its dependent functions. A new getPitchAngle() function added.; ;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acloitre 0:5651731cfb32 1 #ifndef MBED_UM6LT_H
acloitre 0:5651731cfb32 2 #define MBED_UM6LT_H
acloitre 0:5651731cfb32 3
acloitre 0:5651731cfb32 4 #include "mbed.h"
acloitre 0:5651731cfb32 5
acloitre 0:5651731cfb32 6 // UM6 Configuration Registers
acloitre 0:5651731cfb32 7
acloitre 0:5651731cfb32 8 #define UM6_COMMUNICATION 0x00
acloitre 0:5651731cfb32 9 #define UM6_MISC_CONFIG 0x01
acloitre 0:5651731cfb32 10 #define UM6_MAG_REF_X 0x02
acloitre 0:5651731cfb32 11 #define UM6_MAG_REF_Y 0x03
acloitre 0:5651731cfb32 12 #define UM6_MAG_REF_Z 0x04
acloitre 0:5651731cfb32 13 #define UM6_ACCEL_REF_X 0x05
acloitre 0:5651731cfb32 14 #define UM6_ACCEL_REF_Y 0x06
acloitre 0:5651731cfb32 15 #define UM6_ACCEL_REF_Z 0x07
acloitre 0:5651731cfb32 16 #define UM6_EKF_MAG_VARIANCE 0x08
acloitre 0:5651731cfb32 17 #define UM6_EKF_ACCEL_VARIANCE 0x09
acloitre 0:5651731cfb32 18 #define UM6_EKF_PROCESS_VARIANCE 0x0A
acloitre 0:5651731cfb32 19 #define UM6_GYRO_BIAS_XY 0x0B
acloitre 0:5651731cfb32 20 #define UM6_GYRO_BIAS_Z 0x0C
acloitre 0:5651731cfb32 21 #define UM6_ACCEL_BIAS_XY 0x0D
acloitre 0:5651731cfb32 22 #define UM6_ACCEL_BIAS_Z 0x0E
acloitre 0:5651731cfb32 23 #define UM6_MAG_BIAS_XY 0x0F
acloitre 0:5651731cfb32 24 #define UM6_MAG_BIAS_Z 0x10
acloitre 0:5651731cfb32 25 #define UM6_ACCEL_CAL_00 0x11
acloitre 0:5651731cfb32 26 #define UM6_ACCEL_CAL_01 0x12
acloitre 0:5651731cfb32 27 #define UM6_ACCEL_CAL_02 0x13
acloitre 0:5651731cfb32 28 #define UM6_ACCEL_CAL_10 0x14
acloitre 0:5651731cfb32 29 #define UM6_ACCEL_CAL_11 0x15
acloitre 0:5651731cfb32 30 #define UM6_ACCEL_CAL_12 0x16
acloitre 0:5651731cfb32 31 #define UM6_ACCEL_CAL_20 0x17
acloitre 0:5651731cfb32 32 #define UM6_ACCEL_CAL_21 0x18
acloitre 0:5651731cfb32 33 #define UM6_ACCEL_CAL_22 0x19
acloitre 0:5651731cfb32 34 #define UM6_GYRO_CAL_00 0x1A
acloitre 0:5651731cfb32 35 #define UM6_GYRO_CAL_01 0x1B
acloitre 0:5651731cfb32 36 #define UM6_GYRO_CAL_02 0x1C
acloitre 0:5651731cfb32 37 #define UM6_GYRO_CAL_10 0x1D
acloitre 0:5651731cfb32 38 #define UM6_GYRO_CAL_11 0x1E
acloitre 0:5651731cfb32 39 #define UM6_GYRO_CAL_12 0x1F
acloitre 0:5651731cfb32 40 #define UM6_GYRO_CAL_20 0x20
acloitre 0:5651731cfb32 41 #define UM6_GYRO_CAL_21 0x21
acloitre 0:5651731cfb32 42 #define UM6_GYRO_CAL_22 0x22
acloitre 0:5651731cfb32 43 #define UM6_MAG_CAL_00 0x23
acloitre 0:5651731cfb32 44 #define UM6_MAG_CAL_01 0x24
acloitre 0:5651731cfb32 45 #define UM6_MAG_CAL_02 0x25
acloitre 0:5651731cfb32 46 #define UM6_MAG_CAL_10 0x26
acloitre 0:5651731cfb32 47 #define UM6_MAG_CAL_11 0x27
acloitre 0:5651731cfb32 48 #define UM6_MAG_CAL_12 0x28
acloitre 0:5651731cfb32 49 #define UM6_MAG_CAL_20 0x29
acloitre 0:5651731cfb32 50 #define UM6_MAG_CAL_21 0x2A
acloitre 0:5651731cfb32 51 #define UM6_MAG_CAL_22 0x2B
acloitre 0:5651731cfb32 52
acloitre 0:5651731cfb32 53 // UM6 Data Registers
acloitre 0:5651731cfb32 54
acloitre 0:5651731cfb32 55 #define UM6_STATUS 0x55
acloitre 0:5651731cfb32 56 #define UM6_GYRO_RAW_XY 0x56
acloitre 0:5651731cfb32 57 #define UM6_GYRO_RAW_Z 0x57
acloitre 0:5651731cfb32 58 #define UM6_ACCEL_RAW_XY 0x58
acloitre 0:5651731cfb32 59 #define UM6_ACCEL_RAW_Z 0x59
acloitre 0:5651731cfb32 60 #define UM6_MAG_RAW_XY 0x5A
acloitre 0:5651731cfb32 61 #define UM6_MAG_RAW_Z 0x5B
acloitre 0:5651731cfb32 62 #define UM6_GYRO_PROC_XY 0x5C
acloitre 0:5651731cfb32 63 #define UM6_GYRO_PROC_Z 0x5D
acloitre 0:5651731cfb32 64 #define UM6_ACCEL_PROC_XY 0x5E
acloitre 0:5651731cfb32 65 #define UM6_ACCEL_PROC_Z 0x5F
acloitre 0:5651731cfb32 66 #define UM6_MAG_PROC_XY 0x60
acloitre 0:5651731cfb32 67 #define UM6_MAG_PROC_Z 0x61
acloitre 0:5651731cfb32 68 #define UM6_EULER_PHI_THETA 0x62
acloitre 0:5651731cfb32 69 #define UM6_EULER_PSI 0x63
acloitre 0:5651731cfb32 70 #define UM6_QUAT_AB 0x64
acloitre 0:5651731cfb32 71 #define UM6_QUAT_CD 0x65
acloitre 0:5651731cfb32 72 #define UM6_ERROR_COV_00 0x66
acloitre 0:5651731cfb32 73 #define UM6_ERROR_COV_01 0x67
acloitre 0:5651731cfb32 74 #define UM6_ERROR_COV_02 0x68
acloitre 0:5651731cfb32 75 #define UM6_ERROR_COV_03 0x69
acloitre 0:5651731cfb32 76 #define UM6_ERROR_COV_10 0x6A
acloitre 0:5651731cfb32 77 #define UM6_ERROR_COV_11 0x6B
acloitre 0:5651731cfb32 78 #define UM6_ERROR_COV_12 0x6C
acloitre 0:5651731cfb32 79 #define UM6_ERROR_COV_13 0x6D
acloitre 0:5651731cfb32 80 #define UM6_ERROR_COV_20 0x6E
acloitre 0:5651731cfb32 81 #define UM6_ERROR_COV_21 0x6F
acloitre 0:5651731cfb32 82 #define UM6_ERROR_COV_22 0x70
acloitre 0:5651731cfb32 83 #define UM6_ERROR_COV_23 0x71
acloitre 0:5651731cfb32 84 #define UM6_ERROR_COV_30 0x72
acloitre 0:5651731cfb32 85 #define UM6_ERROR_COV_31 0x73
acloitre 0:5651731cfb32 86 #define UM6_ERROR_COV_32 0x74
acloitre 0:5651731cfb32 87 #define UM6_ERROR_COV_33 0x75
acloitre 0:5651731cfb32 88
acloitre 0:5651731cfb32 89 // UM6 Command Registers
acloitre 0:5651731cfb32 90
acloitre 0:5651731cfb32 91 #define UM6_GET_FW_VERSION 0xAA
acloitre 0:5651731cfb32 92 #define UM6_FLASH_COMMIT 0xAB
acloitre 0:5651731cfb32 93 #define UM6_ZERO_GYROS 0xAC
acloitre 0:5651731cfb32 94 #define UM6_RESET_EKF 0xAD
acloitre 0:5651731cfb32 95 #define UM6_GET_DATA 0xAE
acloitre 0:5651731cfb32 96 #define UM6_SET_ACCEL_REF 0xAF
acloitre 0:5651731cfb32 97 #define UM6_SET_MAG_REF 0xB0
acloitre 0:5651731cfb32 98 #define UM6_RESET_TO_FACTORY 0xB1
acloitre 0:5651731cfb32 99
acloitre 0:5651731cfb32 100 #define UM6_BAD_CHECKSUM 0xFD
acloitre 0:5651731cfb32 101 #define UM6_UNKNOWN_ADDRESS 0xFE
acloitre 0:5651731cfb32 102 #define UM6_INVALID_BATCH_SIZE 0xFF
acloitre 0:5651731cfb32 103
acloitre 0:5651731cfb32 104 // UM6 scale factors
acloitre 0:5651731cfb32 105
acloitre 0:5651731cfb32 106 #define UM6_ANGLE_FACTOR 0.0109863
acloitre 0:5651731cfb32 107 #define UM6_ACCEL_FACTOR 0.000183105
acloitre 0:5651731cfb32 108 #define UM6_MAG_FACTOR 0.000305176
acloitre 0:5651731cfb32 109 #define UM6_ANGLE_RATE_FACTOR 0.0610352
acloitre 0:5651731cfb32 110 #define UM6_QUATERNION_FACTOR 0.0000335693
acloitre 0:5651731cfb32 111
acloitre 0:5651731cfb32 112 class UM6LT {
acloitre 0:5651731cfb32 113
acloitre 0:5651731cfb32 114 public:
acloitre 0:5651731cfb32 115
acloitre 0:5651731cfb32 116 UM6LT (PinName tx, PinName rx);
acloitre 0:5651731cfb32 117
acloitre 0:5651731cfb32 118 void baud(int baudrate);
acloitre 0:5651731cfb32 119 int readable(void);
acloitre 0:5651731cfb32 120 char getc(void);
acloitre 0:5651731cfb32 121 void putc(char byte);
acloitre 0:5651731cfb32 122
acloitre 0:5651731cfb32 123 void setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled);
acloitre 0:5651731cfb32 124 void setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate);
acloitre 0:5651731cfb32 125 int getStatus(void);
acloitre 0:5651731cfb32 126
acloitre 0:5651731cfb32 127 int zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ);
acloitre 0:5651731cfb32 128 int autoSetAccelRef(void);
acloitre 0:5651731cfb32 129 int autoSetMagRef(void);
acloitre 0:5651731cfb32 130 int resetEKF(void);
acloitre 0:5651731cfb32 131 int writeToFlash(void);
acloitre 0:5651731cfb32 132 int resetToFactory(void);
acloitre 0:5651731cfb32 133
acloitre 0:5651731cfb32 134 int getAngles(int& roll, int& pitch, int& yaw); // in degrees
chartersauce25 1:703ba8e8b2cd 135 int getPitchAngle(); // in degrees
acloitre 0:5651731cfb32 136 int getAccel(int& accelX, int& accelY, int& accelZ); // in milli-gravity
acloitre 0:5651731cfb32 137 int getMag(int& magX, int& magY, int& magZ); // in milli-unit. the norm of the vector should be one
acloitre 0:5651731cfb32 138 int getAngleRates(int& rollRate, int& pitchRate, int& yawRate); // in degree/sec
acloitre 0:5651731cfb32 139 int getQuaternion(int& a, int& b, int& c, int& d); // in milli-unit
acloitre 0:5651731cfb32 140
acloitre 0:5651731cfb32 141 private:
acloitre 0:5651731cfb32 142
acloitre 0:5651731cfb32 143 Serial serial_;
acloitre 0:5651731cfb32 144 int stdDelayMs;
acloitre 0:5651731cfb32 145
acloitre 0:5651731cfb32 146 int verifyChecksum(int byte1, int byte2);
acloitre 0:5651731cfb32 147 void createChecksum(int checksum_dec, int& byte1, int& byte2);
acloitre 0:5651731cfb32 148 void createByte(int* bitsList, int& byte);
acloitre 0:5651731cfb32 149 void decomposeByte(int* bitsList, int byte);
acloitre 0:5651731cfb32 150 int twosComplement(int byte1, int byte2);
acloitre 0:5651731cfb32 151
acloitre 0:5651731cfb32 152 void oneWordWrite(int hasData, int isBatch, int BL, int address, int* data);
acloitre 0:5651731cfb32 153 void oneWordRead(int& PT, int& N, int& address, int* data);
acloitre 0:5651731cfb32 154 void requestData(int address, int expectedBL);
acloitre 0:5651731cfb32 155 int sendOneCommand(int address);
acloitre 0:5651731cfb32 156 int getTwoBatches(int address, int* data);
acloitre 0:5651731cfb32 157
acloitre 0:5651731cfb32 158 int noCommError(int address, int addressExpected, int PT);
acloitre 0:5651731cfb32 159 void WhatCommError(int addressRead, int addressExpected, int PT);
acloitre 0:5651731cfb32 160
acloitre 0:5651731cfb32 161 };
acloitre 0:5651731cfb32 162
acloitre 0:5651731cfb32 163 #endif /* MBED_UM6LT_H */