Audren Cloitre / UM6LT

My first attempt at writing a library for the UM6LT. Not complete and not well documented but it does the trick for me.

Dependents:   UM6LT_v1

UM6LT.h@0:5651731cfb32, 2012-09-30 (annotated)

Committer:
acloitre
Date:
Sun Sep 30 17:59:52 2012 +0000
Revision:
0:5651731cfb32
[mbed] converted /UM6LT_v1/UM6LT

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
acloitre 0:5651731cfb32 135 int getAccel(int& accelX, int& accelY, int& accelZ); // in milli-gravity
acloitre 0:5651731cfb32 136 int getMag(int& magX, int& magY, int& magZ); // in milli-unit. the norm of the vector should be one
acloitre 0:5651731cfb32 137 int getAngleRates(int& rollRate, int& pitchRate, int& yawRate); // in degree/sec
acloitre 0:5651731cfb32 138 int getQuaternion(int& a, int& b, int& c, int& d); // in milli-unit
acloitre 0:5651731cfb32 139
acloitre 0:5651731cfb32 140 private:
acloitre 0:5651731cfb32 141
acloitre 0:5651731cfb32 142 Serial serial_;
acloitre 0:5651731cfb32 143 int stdDelayMs;
acloitre 0:5651731cfb32 144
acloitre 0:5651731cfb32 145 int verifyChecksum(int byte1, int byte2);
acloitre 0:5651731cfb32 146 void createChecksum(int checksum_dec, int& byte1, int& byte2);
acloitre 0:5651731cfb32 147 void createByte(int* bitsList, int& byte);
acloitre 0:5651731cfb32 148 void decomposeByte(int* bitsList, int byte);
acloitre 0:5651731cfb32 149 int twosComplement(int byte1, int byte2);
acloitre 0:5651731cfb32 150
acloitre 0:5651731cfb32 151 void oneWordWrite(int hasData, int isBatch, int BL, int address, int* data);
acloitre 0:5651731cfb32 152 void oneWordRead(int& PT, int& N, int& address, int* data);
acloitre 0:5651731cfb32 153 void requestData(int address, int expectedBL);
acloitre 0:5651731cfb32 154 int sendOneCommand(int address);
acloitre 0:5651731cfb32 155 int getTwoBatches(int address, int* data);
acloitre 0:5651731cfb32 156
acloitre 0:5651731cfb32 157 int noCommError(int address, int addressExpected, int PT);
acloitre 0:5651731cfb32 158 void WhatCommError(int addressRead, int addressExpected, int PT);
acloitre 0:5651731cfb32 159
acloitre 0:5651731cfb32 160 };
acloitre 0:5651731cfb32 161
acloitre 0:5651731cfb32 162 #endif /* MBED_UM6LT_H */