My first attempt at writing a library for the UM6LT. Not complete and not well documented but it does the trick for me.
UM6LT.cpp@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?
User | Revision | Line number | New contents of line |
---|---|---|---|
acloitre | 0:5651731cfb32 | 1 | #include "UM6LT.h" |
acloitre | 0:5651731cfb32 | 2 | #include "mbed.h" |
acloitre | 0:5651731cfb32 | 3 | |
acloitre | 0:5651731cfb32 | 4 | UM6LT::UM6LT(PinName tx, PinName rx):serial_(tx,rx) {serial_.baud(115200);stdDelayMs = 2;} |
acloitre | 0:5651731cfb32 | 5 | |
acloitre | 0:5651731cfb32 | 6 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 7 | //----------------------------------- Private Functions ------------------------------------ |
acloitre | 0:5651731cfb32 | 8 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 9 | |
acloitre | 0:5651731cfb32 | 10 | int UM6LT::verifyChecksum(int byte1, int byte2){ |
acloitre | 0:5651731cfb32 | 11 | |
acloitre | 0:5651731cfb32 | 12 | int checksumDATA = 0; |
acloitre | 0:5651731cfb32 | 13 | div_t div_res1; |
acloitre | 0:5651731cfb32 | 14 | div_t div_res2; |
acloitre | 0:5651731cfb32 | 15 | int quot1 = byte1; |
acloitre | 0:5651731cfb32 | 16 | int quot2 = byte2; |
acloitre | 0:5651731cfb32 | 17 | |
acloitre | 0:5651731cfb32 | 18 | int coef[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768}; |
acloitre | 0:5651731cfb32 | 19 | |
acloitre | 0:5651731cfb32 | 20 | for(int i=0; i<8; ++i){ |
acloitre | 0:5651731cfb32 | 21 | div_res1 = div(quot1,2); |
acloitre | 0:5651731cfb32 | 22 | div_res2 = div(quot2,2); |
acloitre | 0:5651731cfb32 | 23 | checksumDATA = checksumDATA + div_res1.rem * coef[i+8] + div_res2.rem * coef[i]; |
acloitre | 0:5651731cfb32 | 24 | quot1 = div_res1.quot; |
acloitre | 0:5651731cfb32 | 25 | quot2 = div_res2.quot; |
acloitre | 0:5651731cfb32 | 26 | } |
acloitre | 0:5651731cfb32 | 27 | |
acloitre | 0:5651731cfb32 | 28 | return checksumDATA; |
acloitre | 0:5651731cfb32 | 29 | |
acloitre | 0:5651731cfb32 | 30 | } |
acloitre | 0:5651731cfb32 | 31 | |
acloitre | 0:5651731cfb32 | 32 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 33 | |
acloitre | 0:5651731cfb32 | 34 | void UM6LT::createChecksum(int checksum_dec, int& byte1, int& byte2){ |
acloitre | 0:5651731cfb32 | 35 | |
acloitre | 0:5651731cfb32 | 36 | div_t div_res; |
acloitre | 0:5651731cfb32 | 37 | int quot = checksum_dec; |
acloitre | 0:5651731cfb32 | 38 | int coef[24] = {0, 0, 0, 0, 0, 0, 0, 0, 1, 2, 4, 8, 16, 32, 64, 128, 0, 0, 0, 0, 0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 39 | |
acloitre | 0:5651731cfb32 | 40 | byte1 = 0; |
acloitre | 0:5651731cfb32 | 41 | byte2 = 0; |
acloitre | 0:5651731cfb32 | 42 | |
acloitre | 0:5651731cfb32 | 43 | for(int i=0; i<16; ++i){ |
acloitre | 0:5651731cfb32 | 44 | div_res = div(quot,2); |
acloitre | 0:5651731cfb32 | 45 | byte1 = byte1 + div_res.rem * coef[i]; |
acloitre | 0:5651731cfb32 | 46 | byte2 = byte2 + div_res.rem * coef[i + 8]; |
acloitre | 0:5651731cfb32 | 47 | quot = div_res.quot; |
acloitre | 0:5651731cfb32 | 48 | } |
acloitre | 0:5651731cfb32 | 49 | |
acloitre | 0:5651731cfb32 | 50 | } |
acloitre | 0:5651731cfb32 | 51 | |
acloitre | 0:5651731cfb32 | 52 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 53 | |
acloitre | 0:5651731cfb32 | 54 | void UM6LT::createByte(int* bitsList, int& byte) { |
acloitre | 0:5651731cfb32 | 55 | |
acloitre | 0:5651731cfb32 | 56 | int coef[] = {1, 2, 4, 8, 16, 32, 64, 128}; |
acloitre | 0:5651731cfb32 | 57 | |
acloitre | 0:5651731cfb32 | 58 | for(int i=0; i<8; i++){ |
acloitre | 0:5651731cfb32 | 59 | byte = byte + bitsList[i]*coef[i]; |
acloitre | 0:5651731cfb32 | 60 | } |
acloitre | 0:5651731cfb32 | 61 | |
acloitre | 0:5651731cfb32 | 62 | } |
acloitre | 0:5651731cfb32 | 63 | |
acloitre | 0:5651731cfb32 | 64 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 65 | |
acloitre | 0:5651731cfb32 | 66 | void UM6LT::decomposeByte(int* bitsList, int byte){ |
acloitre | 0:5651731cfb32 | 67 | |
acloitre | 0:5651731cfb32 | 68 | div_t div_res; |
acloitre | 0:5651731cfb32 | 69 | int quot = byte; |
acloitre | 0:5651731cfb32 | 70 | |
acloitre | 0:5651731cfb32 | 71 | for(int i=0; i<8; i++){ |
acloitre | 0:5651731cfb32 | 72 | div_res = div(quot, 2); |
acloitre | 0:5651731cfb32 | 73 | bitsList[i] = div_res.rem; |
acloitre | 0:5651731cfb32 | 74 | quot = div_res.quot; |
acloitre | 0:5651731cfb32 | 75 | } |
acloitre | 0:5651731cfb32 | 76 | |
acloitre | 0:5651731cfb32 | 77 | } |
acloitre | 0:5651731cfb32 | 78 | |
acloitre | 0:5651731cfb32 | 79 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 80 | |
acloitre | 0:5651731cfb32 | 81 | int UM6LT::twosComplement(int byte1, int byte2){ |
acloitre | 0:5651731cfb32 | 82 | |
acloitre | 0:5651731cfb32 | 83 | int sumData = 0; |
acloitre | 0:5651731cfb32 | 84 | div_t div_res1; |
acloitre | 0:5651731cfb32 | 85 | div_t div_res2; |
acloitre | 0:5651731cfb32 | 86 | int quot1 = byte1; |
acloitre | 0:5651731cfb32 | 87 | int quot2 = byte2; |
acloitre | 0:5651731cfb32 | 88 | |
acloitre | 0:5651731cfb32 | 89 | int coef[16] = {1, 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, 2048, 4096, 8192, 16384, 32768}; |
acloitre | 0:5651731cfb32 | 90 | |
acloitre | 0:5651731cfb32 | 91 | for(int i=0; i<7; ++i){ |
acloitre | 0:5651731cfb32 | 92 | div_res1 = div(quot1,2); |
acloitre | 0:5651731cfb32 | 93 | div_res2 = div(quot2,2); |
acloitre | 0:5651731cfb32 | 94 | sumData = sumData + div_res1.rem * coef[i+8] + div_res2.rem * coef[i]; |
acloitre | 0:5651731cfb32 | 95 | quot1 = div_res1.quot; |
acloitre | 0:5651731cfb32 | 96 | quot2 = div_res2.quot; |
acloitre | 0:5651731cfb32 | 97 | } |
acloitre | 0:5651731cfb32 | 98 | |
acloitre | 0:5651731cfb32 | 99 | div_res1 = div(quot1,2); |
acloitre | 0:5651731cfb32 | 100 | div_res2 = div(quot2,2); |
acloitre | 0:5651731cfb32 | 101 | |
acloitre | 0:5651731cfb32 | 102 | if(div_res1.rem == 1){ |
acloitre | 0:5651731cfb32 | 103 | sumData = sumData + div_res2.rem * coef[7] - coef[15]; |
acloitre | 0:5651731cfb32 | 104 | } |
acloitre | 0:5651731cfb32 | 105 | else{ |
acloitre | 0:5651731cfb32 | 106 | sumData = sumData + div_res2.rem * coef[7]; |
acloitre | 0:5651731cfb32 | 107 | } |
acloitre | 0:5651731cfb32 | 108 | |
acloitre | 0:5651731cfb32 | 109 | return sumData; |
acloitre | 0:5651731cfb32 | 110 | |
acloitre | 0:5651731cfb32 | 111 | } |
acloitre | 0:5651731cfb32 | 112 | |
acloitre | 0:5651731cfb32 | 113 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 114 | |
acloitre | 0:5651731cfb32 | 115 | void UM6LT::oneWordWrite(int hasData, int isBatch, int BL, int address, int* data) { |
acloitre | 0:5651731cfb32 | 116 | |
acloitre | 0:5651731cfb32 | 117 | int N, PT, checksumDATA; |
acloitre | 0:5651731cfb32 | 118 | |
acloitre | 0:5651731cfb32 | 119 | |
acloitre | 0:5651731cfb32 | 120 | if(hasData){ |
acloitre | 0:5651731cfb32 | 121 | N = BL*4; //Number of data bytes |
acloitre | 0:5651731cfb32 | 122 | } |
acloitre | 0:5651731cfb32 | 123 | else{ |
acloitre | 0:5651731cfb32 | 124 | N = 0; |
acloitre | 0:5651731cfb32 | 125 | } |
acloitre | 0:5651731cfb32 | 126 | |
acloitre | 0:5651731cfb32 | 127 | int word [N+7]; |
acloitre | 0:5651731cfb32 | 128 | int coef [] = {4, 8, 16, 32}; |
acloitre | 0:5651731cfb32 | 129 | div_t div_res; |
acloitre | 0:5651731cfb32 | 130 | int quot = BL; |
acloitre | 0:5651731cfb32 | 131 | |
acloitre | 0:5651731cfb32 | 132 | PT = 128*hasData + 64*isBatch; |
acloitre | 0:5651731cfb32 | 133 | for(int i=0; i<4; ++i){ |
acloitre | 0:5651731cfb32 | 134 | div_res = div(quot,2); |
acloitre | 0:5651731cfb32 | 135 | PT = PT + div_res.rem * coef[i]; |
acloitre | 0:5651731cfb32 | 136 | quot = div_res.quot; |
acloitre | 0:5651731cfb32 | 137 | } |
acloitre | 0:5651731cfb32 | 138 | |
acloitre | 0:5651731cfb32 | 139 | checksumDATA = 115 + 110 + 112 + PT + address; |
acloitre | 0:5651731cfb32 | 140 | |
acloitre | 0:5651731cfb32 | 141 | if(hasData){ |
acloitre | 0:5651731cfb32 | 142 | for(int i=0; i<N; i++){ |
acloitre | 0:5651731cfb32 | 143 | checksumDATA = checksumDATA + data[i]; |
acloitre | 0:5651731cfb32 | 144 | word[i+5] = data[i]; |
acloitre | 0:5651731cfb32 | 145 | } |
acloitre | 0:5651731cfb32 | 146 | } |
acloitre | 0:5651731cfb32 | 147 | |
acloitre | 0:5651731cfb32 | 148 | int byte1 = 0; |
acloitre | 0:5651731cfb32 | 149 | int byte2 = 0; |
acloitre | 0:5651731cfb32 | 150 | createChecksum(checksumDATA, byte1, byte2); |
acloitre | 0:5651731cfb32 | 151 | |
acloitre | 0:5651731cfb32 | 152 | word[0] = 115; //'s' |
acloitre | 0:5651731cfb32 | 153 | word[1] = 110; //'n' |
acloitre | 0:5651731cfb32 | 154 | word[2] = 112; //'p' |
acloitre | 0:5651731cfb32 | 155 | word[3] = PT; |
acloitre | 0:5651731cfb32 | 156 | word[4] = address; |
acloitre | 0:5651731cfb32 | 157 | word[N+5] = byte1; |
acloitre | 0:5651731cfb32 | 158 | word[N+6] = byte2; |
acloitre | 0:5651731cfb32 | 159 | |
acloitre | 0:5651731cfb32 | 160 | if(serial_.writeable()) { |
acloitre | 0:5651731cfb32 | 161 | for(int i=0; i<N+7; i++){ |
acloitre | 0:5651731cfb32 | 162 | serial_.putc(word[i]); |
acloitre | 0:5651731cfb32 | 163 | } |
acloitre | 0:5651731cfb32 | 164 | } |
acloitre | 0:5651731cfb32 | 165 | else{ |
acloitre | 0:5651731cfb32 | 166 | printf("IMU not writeable.\r\n"); |
acloitre | 0:5651731cfb32 | 167 | } |
acloitre | 0:5651731cfb32 | 168 | |
acloitre | 0:5651731cfb32 | 169 | } |
acloitre | 0:5651731cfb32 | 170 | |
acloitre | 0:5651731cfb32 | 171 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 172 | |
acloitre | 0:5651731cfb32 | 173 | void UM6LT::oneWordRead(int& PT, int& N, int& address, int* data){ |
acloitre | 0:5651731cfb32 | 174 | |
acloitre | 0:5651731cfb32 | 175 | int sumData = 0; |
acloitre | 0:5651731cfb32 | 176 | int PT_byte [8]; |
acloitre | 0:5651731cfb32 | 177 | char snp [] = {'0', '0', '0'}; |
acloitre | 0:5651731cfb32 | 178 | |
acloitre | 0:5651731cfb32 | 179 | if(serial_.readable()){ |
acloitre | 0:5651731cfb32 | 180 | |
acloitre | 0:5651731cfb32 | 181 | while((snp[0] != 's' || snp[1] != 'n' || snp[2] != 'p') && serial_.readable()){ |
acloitre | 0:5651731cfb32 | 182 | snp[0] = snp[1]; |
acloitre | 0:5651731cfb32 | 183 | snp[1] = snp[2]; |
acloitre | 0:5651731cfb32 | 184 | snp[2] = serial_.getc(); |
acloitre | 0:5651731cfb32 | 185 | } |
acloitre | 0:5651731cfb32 | 186 | |
acloitre | 0:5651731cfb32 | 187 | if(snp[0] == 's' && snp[1]=='n' && snp[2] == 'p'){ |
acloitre | 0:5651731cfb32 | 188 | PT = serial_.getc(); |
acloitre | 0:5651731cfb32 | 189 | address = serial_.getc(); |
acloitre | 0:5651731cfb32 | 190 | |
acloitre | 0:5651731cfb32 | 191 | decomposeByte(PT_byte, PT); |
acloitre | 0:5651731cfb32 | 192 | |
acloitre | 0:5651731cfb32 | 193 | int hasData = PT_byte[7]; |
acloitre | 0:5651731cfb32 | 194 | int CF = PT_byte[0]; |
acloitre | 0:5651731cfb32 | 195 | |
acloitre | 0:5651731cfb32 | 196 | if(CF){ |
acloitre | 0:5651731cfb32 | 197 | // find something to do if command failed |
acloitre | 0:5651731cfb32 | 198 | printf("Last command sent to UM6 failed.\r\n"); |
acloitre | 0:5651731cfb32 | 199 | } |
acloitre | 0:5651731cfb32 | 200 | else if(hasData){ |
acloitre | 0:5651731cfb32 | 201 | int BL = PT_byte[5]*8 + PT_byte[4]*4 + PT_byte[3]*2 + PT_byte[2]; |
acloitre | 0:5651731cfb32 | 202 | N = 4*BL; |
acloitre | 0:5651731cfb32 | 203 | for(int i=0; i<N; i++){ |
acloitre | 0:5651731cfb32 | 204 | data[i] = serial_.getc(); |
acloitre | 0:5651731cfb32 | 205 | sumData = sumData + data[i]; |
acloitre | 0:5651731cfb32 | 206 | } |
acloitre | 0:5651731cfb32 | 207 | } |
acloitre | 0:5651731cfb32 | 208 | else{ |
acloitre | 0:5651731cfb32 | 209 | N = 0; |
acloitre | 0:5651731cfb32 | 210 | } |
acloitre | 0:5651731cfb32 | 211 | |
acloitre | 0:5651731cfb32 | 212 | int byte1 = serial_.getc(); |
acloitre | 0:5651731cfb32 | 213 | int byte2 = serial_.getc(); |
acloitre | 0:5651731cfb32 | 214 | |
acloitre | 0:5651731cfb32 | 215 | int checksumUM6LT = verifyChecksum(byte1, byte2); |
acloitre | 0:5651731cfb32 | 216 | int checksumDATA = 115 + 110 + 112 + PT + address + sumData; |
acloitre | 0:5651731cfb32 | 217 | |
acloitre | 0:5651731cfb32 | 218 | if(checksumDATA != checksumUM6LT){ |
acloitre | 0:5651731cfb32 | 219 | // Find something to do if checksum not good |
acloitre | 0:5651731cfb32 | 220 | printf("Calculated checksum does not match the one from the UM6.\r\n"); |
acloitre | 0:5651731cfb32 | 221 | data[0] = -1; |
acloitre | 0:5651731cfb32 | 222 | } |
acloitre | 0:5651731cfb32 | 223 | } |
acloitre | 0:5651731cfb32 | 224 | else{ |
acloitre | 0:5651731cfb32 | 225 | printf("Could not read an expected word from UM6. Its buffer is now empty\r\n"); |
acloitre | 0:5651731cfb32 | 226 | } |
acloitre | 0:5651731cfb32 | 227 | } |
acloitre | 0:5651731cfb32 | 228 | else{ |
acloitre | 0:5651731cfb32 | 229 | // Find something to do if IMU not readable |
acloitre | 0:5651731cfb32 | 230 | printf("Minor synchronization issue with UM6.\r\n"); |
acloitre | 0:5651731cfb32 | 231 | data[0] = -1; |
acloitre | 0:5651731cfb32 | 232 | } |
acloitre | 0:5651731cfb32 | 233 | |
acloitre | 0:5651731cfb32 | 234 | } |
acloitre | 0:5651731cfb32 | 235 | |
acloitre | 0:5651731cfb32 | 236 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 237 | |
acloitre | 0:5651731cfb32 | 238 | void UM6LT::requestData(int address, int expectedBL){ |
acloitre | 0:5651731cfb32 | 239 | |
acloitre | 0:5651731cfb32 | 240 | int hasData = 0; |
acloitre | 0:5651731cfb32 | 241 | int isBatch = 1; |
acloitre | 0:5651731cfb32 | 242 | int data[1]; |
acloitre | 0:5651731cfb32 | 243 | |
acloitre | 0:5651731cfb32 | 244 | if(serial_.readable()){ |
acloitre | 0:5651731cfb32 | 245 | while(serial_.readable()){ |
acloitre | 0:5651731cfb32 | 246 | data[0] = serial_.getc(); |
acloitre | 0:5651731cfb32 | 247 | } |
acloitre | 0:5651731cfb32 | 248 | } |
acloitre | 0:5651731cfb32 | 249 | if(serial_.writeable()){ |
acloitre | 0:5651731cfb32 | 250 | oneWordWrite(hasData, isBatch, expectedBL, address, data); |
acloitre | 0:5651731cfb32 | 251 | } |
acloitre | 0:5651731cfb32 | 252 | else{ |
acloitre | 0:5651731cfb32 | 253 | printf("IMU not writeable. Data request not sent.\r\n"); |
acloitre | 0:5651731cfb32 | 254 | } |
acloitre | 0:5651731cfb32 | 255 | |
acloitre | 0:5651731cfb32 | 256 | } |
acloitre | 0:5651731cfb32 | 257 | |
acloitre | 0:5651731cfb32 | 258 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 259 | |
acloitre | 0:5651731cfb32 | 260 | int UM6LT::sendOneCommand(int address){ |
acloitre | 0:5651731cfb32 | 261 | |
acloitre | 0:5651731cfb32 | 262 | int PT = -1; |
acloitre | 0:5651731cfb32 | 263 | int N = -1; |
acloitre | 0:5651731cfb32 | 264 | int addressRead = -1; |
acloitre | 0:5651731cfb32 | 265 | int data [1] = {0}; |
acloitre | 0:5651731cfb32 | 266 | |
acloitre | 0:5651731cfb32 | 267 | int okToMoveOn = 0; |
acloitre | 0:5651731cfb32 | 268 | int count = 0; |
acloitre | 0:5651731cfb32 | 269 | |
acloitre | 0:5651731cfb32 | 270 | while(!okToMoveOn && count<5){ |
acloitre | 0:5651731cfb32 | 271 | requestData(address, 0); |
acloitre | 0:5651731cfb32 | 272 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 273 | oneWordRead(PT, N, addressRead, data); |
acloitre | 0:5651731cfb32 | 274 | okToMoveOn = noCommError(addressRead, address, PT); |
acloitre | 0:5651731cfb32 | 275 | count++; |
acloitre | 0:5651731cfb32 | 276 | } |
acloitre | 0:5651731cfb32 | 277 | |
acloitre | 0:5651731cfb32 | 278 | if(okToMoveOn){ |
acloitre | 0:5651731cfb32 | 279 | switch(address){ |
acloitre | 0:5651731cfb32 | 280 | case UM6_FLASH_COMMIT: |
acloitre | 0:5651731cfb32 | 281 | printf("All current configurations were written to flash.\r\n"); |
acloitre | 0:5651731cfb32 | 282 | break; |
acloitre | 0:5651731cfb32 | 283 | case UM6_RESET_EKF: |
acloitre | 0:5651731cfb32 | 284 | printf("EKF algorithm successfully reset.\r\n"); |
acloitre | 0:5651731cfb32 | 285 | break; |
acloitre | 0:5651731cfb32 | 286 | case UM6_SET_ACCEL_REF: |
acloitre | 0:5651731cfb32 | 287 | printf("Acceleration reference vector updated.\r\n"); |
acloitre | 0:5651731cfb32 | 288 | break; |
acloitre | 0:5651731cfb32 | 289 | case UM6_SET_MAG_REF: |
acloitre | 0:5651731cfb32 | 290 | printf("Magnetic reference vector updated.\r\n"); |
acloitre | 0:5651731cfb32 | 291 | break; |
acloitre | 0:5651731cfb32 | 292 | case UM6_RESET_TO_FACTORY: |
acloitre | 0:5651731cfb32 | 293 | printf("UM6 reset to factory settings.\r\n"); |
acloitre | 0:5651731cfb32 | 294 | break; |
acloitre | 0:5651731cfb32 | 295 | default: |
acloitre | 0:5651731cfb32 | 296 | printf("This command is not coded. Can't say what happened. address: 0x%x\r\n", address); |
acloitre | 0:5651731cfb32 | 297 | break; |
acloitre | 0:5651731cfb32 | 298 | } |
acloitre | 0:5651731cfb32 | 299 | return 1; |
acloitre | 0:5651731cfb32 | 300 | } |
acloitre | 0:5651731cfb32 | 301 | else{ |
acloitre | 0:5651731cfb32 | 302 | WhatCommError(addressRead, address, PT); |
acloitre | 0:5651731cfb32 | 303 | switch(address){ |
acloitre | 0:5651731cfb32 | 304 | case UM6_FLASH_COMMIT: |
acloitre | 0:5651731cfb32 | 305 | printf("Current configurations could not be written to flash.\r\n"); |
acloitre | 0:5651731cfb32 | 306 | break; |
acloitre | 0:5651731cfb32 | 307 | case UM6_RESET_EKF: |
acloitre | 0:5651731cfb32 | 308 | printf("EKF algorithm could not be reset.\r\n"); |
acloitre | 0:5651731cfb32 | 309 | break; |
acloitre | 0:5651731cfb32 | 310 | case UM6_SET_ACCEL_REF: |
acloitre | 0:5651731cfb32 | 311 | printf("Acceleration reference vector failed to update.\r\n"); |
acloitre | 0:5651731cfb32 | 312 | break; |
acloitre | 0:5651731cfb32 | 313 | case UM6_SET_MAG_REF: |
acloitre | 0:5651731cfb32 | 314 | printf("Magnetic reference vector failed to update.\r\n"); |
acloitre | 0:5651731cfb32 | 315 | break; |
acloitre | 0:5651731cfb32 | 316 | case UM6_RESET_TO_FACTORY: |
acloitre | 0:5651731cfb32 | 317 | printf("UM6 could not be reset to factory settings.\r\n"); |
acloitre | 0:5651731cfb32 | 318 | break; |
acloitre | 0:5651731cfb32 | 319 | default: |
acloitre | 0:5651731cfb32 | 320 | printf("This command is not coded. Can't say what happened. address: 0x%x\r\n", address); |
acloitre | 0:5651731cfb32 | 321 | break; |
acloitre | 0:5651731cfb32 | 322 | } |
acloitre | 0:5651731cfb32 | 323 | return 0; |
acloitre | 0:5651731cfb32 | 324 | } |
acloitre | 0:5651731cfb32 | 325 | |
acloitre | 0:5651731cfb32 | 326 | } |
acloitre | 0:5651731cfb32 | 327 | |
acloitre | 0:5651731cfb32 | 328 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 329 | |
acloitre | 0:5651731cfb32 | 330 | int UM6LT::getTwoBatches(int address, int* dataToPost){ |
acloitre | 0:5651731cfb32 | 331 | |
acloitre | 0:5651731cfb32 | 332 | dataToPost[0] = 0; |
acloitre | 0:5651731cfb32 | 333 | dataToPost[1] = 0; |
acloitre | 0:5651731cfb32 | 334 | dataToPost[2] = 0; |
acloitre | 0:5651731cfb32 | 335 | dataToPost[3] = 0; |
acloitre | 0:5651731cfb32 | 336 | |
acloitre | 0:5651731cfb32 | 337 | int PT = -1; |
acloitre | 0:5651731cfb32 | 338 | int N = -1; |
acloitre | 0:5651731cfb32 | 339 | int addressRead = -1; |
acloitre | 0:5651731cfb32 | 340 | int dataToRead [8] = {0,0,0,0,0,0,0,0}; |
acloitre | 0:5651731cfb32 | 341 | |
acloitre | 0:5651731cfb32 | 342 | int okToMoveOn = 0; |
acloitre | 0:5651731cfb32 | 343 | int count = 0; |
acloitre | 0:5651731cfb32 | 344 | |
acloitre | 0:5651731cfb32 | 345 | while(!okToMoveOn && count<5){ |
acloitre | 0:5651731cfb32 | 346 | requestData(address, 2); |
acloitre | 0:5651731cfb32 | 347 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 348 | oneWordRead(PT, N, addressRead, dataToRead); |
acloitre | 0:5651731cfb32 | 349 | okToMoveOn = noCommError(addressRead, address, PT); |
acloitre | 0:5651731cfb32 | 350 | count++; |
acloitre | 0:5651731cfb32 | 351 | } |
acloitre | 0:5651731cfb32 | 352 | |
acloitre | 0:5651731cfb32 | 353 | if(okToMoveOn){ |
acloitre | 0:5651731cfb32 | 354 | switch(address){ |
acloitre | 0:5651731cfb32 | 355 | case UM6_EULER_PHI_THETA: |
acloitre | 0:5651731cfb32 | 356 | dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR); |
acloitre | 0:5651731cfb32 | 357 | dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_FACTOR); |
acloitre | 0:5651731cfb32 | 358 | dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_FACTOR); |
acloitre | 0:5651731cfb32 | 359 | break; |
acloitre | 0:5651731cfb32 | 360 | case UM6_ACCEL_PROC_XY: |
acloitre | 0:5651731cfb32 | 361 | dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 362 | dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ACCEL_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 363 | dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ACCEL_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 364 | break; |
acloitre | 0:5651731cfb32 | 365 | case UM6_MAG_PROC_XY: |
acloitre | 0:5651731cfb32 | 366 | dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 367 | dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_MAG_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 368 | dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_MAG_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 369 | break; |
acloitre | 0:5651731cfb32 | 370 | case UM6_GYRO_PROC_XY: |
acloitre | 0:5651731cfb32 | 371 | dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR); |
acloitre | 0:5651731cfb32 | 372 | dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_RATE_FACTOR); |
acloitre | 0:5651731cfb32 | 373 | dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_ANGLE_RATE_FACTOR); |
acloitre | 0:5651731cfb32 | 374 | break; |
acloitre | 0:5651731cfb32 | 375 | case UM6_QUAT_AB: |
acloitre | 0:5651731cfb32 | 376 | dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 377 | dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 378 | dataToPost[2] = (int)(twosComplement(dataToRead[4], dataToRead[5])*UM6_QUATERNION_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 379 | dataToPost[3] = (int)(twosComplement(dataToRead[6], dataToRead[7])*UM6_QUATERNION_FACTOR*1000); |
acloitre | 0:5651731cfb32 | 380 | break; |
acloitre | 0:5651731cfb32 | 381 | default: |
acloitre | 0:5651731cfb32 | 382 | dataToPost[0] = -1; |
acloitre | 0:5651731cfb32 | 383 | dataToPost[1] = -1; |
acloitre | 0:5651731cfb32 | 384 | dataToPost[2] = -1; |
acloitre | 0:5651731cfb32 | 385 | dataToPost[3] = -1; |
acloitre | 0:5651731cfb32 | 386 | printf("Data acquisition not programmed for this address: %d\r\n", address); |
acloitre | 0:5651731cfb32 | 387 | break; |
acloitre | 0:5651731cfb32 | 388 | } |
acloitre | 0:5651731cfb32 | 389 | return 1; |
acloitre | 0:5651731cfb32 | 390 | } |
acloitre | 0:5651731cfb32 | 391 | else{ |
acloitre | 0:5651731cfb32 | 392 | WhatCommError(address, UM6_ACCEL_PROC_XY, PT); |
acloitre | 0:5651731cfb32 | 393 | switch(address){ |
acloitre | 0:5651731cfb32 | 394 | case UM6_EULER_PHI_THETA: |
acloitre | 0:5651731cfb32 | 395 | printf("Could not acquire the values of the Euler angles.\r\n"); |
acloitre | 0:5651731cfb32 | 396 | break; |
acloitre | 0:5651731cfb32 | 397 | case UM6_ACCEL_PROC_XY: |
acloitre | 0:5651731cfb32 | 398 | printf("Could not acquire the components of the acceleration vector.\r\n"); |
acloitre | 0:5651731cfb32 | 399 | break; |
acloitre | 0:5651731cfb32 | 400 | case UM6_MAG_PROC_XY: |
acloitre | 0:5651731cfb32 | 401 | printf("Could not acquire the components of the magnetic vector.\r\n"); |
acloitre | 0:5651731cfb32 | 402 | break; |
acloitre | 0:5651731cfb32 | 403 | case UM6_GYRO_PROC_XY: |
acloitre | 0:5651731cfb32 | 404 | printf("Could not acquire the values of angle rates.\r\n"); |
acloitre | 0:5651731cfb32 | 405 | break; |
acloitre | 0:5651731cfb32 | 406 | case UM6_QUAT_AB: |
acloitre | 0:5651731cfb32 | 407 | printf("Could not acquire the components of the quaternion.\r\n"); |
acloitre | 0:5651731cfb32 | 408 | break; |
acloitre | 0:5651731cfb32 | 409 | default: |
acloitre | 0:5651731cfb32 | 410 | printf("Data acquisition not programmed for this address: %d\r\n", address); |
acloitre | 0:5651731cfb32 | 411 | break; |
acloitre | 0:5651731cfb32 | 412 | } |
acloitre | 0:5651731cfb32 | 413 | return 0; |
acloitre | 0:5651731cfb32 | 414 | } |
acloitre | 0:5651731cfb32 | 415 | |
acloitre | 0:5651731cfb32 | 416 | } |
acloitre | 0:5651731cfb32 | 417 | |
acloitre | 0:5651731cfb32 | 418 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 419 | |
acloitre | 0:5651731cfb32 | 420 | int UM6LT::noCommError(int addressRead, int addressExpected, int PT){ |
acloitre | 0:5651731cfb32 | 421 | |
acloitre | 0:5651731cfb32 | 422 | if(addressRead == 0xFD){ |
acloitre | 0:5651731cfb32 | 423 | return 0; |
acloitre | 0:5651731cfb32 | 424 | } |
acloitre | 0:5651731cfb32 | 425 | else if(addressRead == 0xFE){ |
acloitre | 0:5651731cfb32 | 426 | return 0; |
acloitre | 0:5651731cfb32 | 427 | } |
acloitre | 0:5651731cfb32 | 428 | else if(addressRead == 0xFF){ |
acloitre | 0:5651731cfb32 | 429 | return 0; |
acloitre | 0:5651731cfb32 | 430 | } |
acloitre | 0:5651731cfb32 | 431 | else if(addressRead == addressExpected){ |
acloitre | 0:5651731cfb32 | 432 | return 1; |
acloitre | 0:5651731cfb32 | 433 | } |
acloitre | 0:5651731cfb32 | 434 | else{ |
acloitre | 0:5651731cfb32 | 435 | return 0; |
acloitre | 0:5651731cfb32 | 436 | } |
acloitre | 0:5651731cfb32 | 437 | |
acloitre | 0:5651731cfb32 | 438 | } |
acloitre | 0:5651731cfb32 | 439 | |
acloitre | 0:5651731cfb32 | 440 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 441 | |
acloitre | 0:5651731cfb32 | 442 | void UM6LT::WhatCommError(int addressRead, int addressExpected, int PT){ |
acloitre | 0:5651731cfb32 | 443 | |
acloitre | 0:5651731cfb32 | 444 | if(addressRead == 0xFD){ |
acloitre | 0:5651731cfb32 | 445 | printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad checksum.\r\n !!!!!! \r\n"); |
acloitre | 0:5651731cfb32 | 446 | } |
acloitre | 0:5651731cfb32 | 447 | else if(addressRead == 0xFE){ |
acloitre | 0:5651731cfb32 | 448 | printf("\r\n !!!!!! \r\nLast word was sent to UM6 at an unknown address.\r\n !!!!!! \r\n"); |
acloitre | 0:5651731cfb32 | 449 | printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected); |
acloitre | 0:5651731cfb32 | 450 | } |
acloitre | 0:5651731cfb32 | 451 | else if(addressRead == 0xFF){ |
acloitre | 0:5651731cfb32 | 452 | printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad batch size.\r\n !!!!!! \r\n"); |
acloitre | 0:5651731cfb32 | 453 | printf("PT: %d\r\n", PT); |
acloitre | 0:5651731cfb32 | 454 | } |
acloitre | 0:5651731cfb32 | 455 | else if(addressRead == addressExpected){ |
acloitre | 0:5651731cfb32 | 456 | printf("No communication error detected...\r\n"); |
acloitre | 0:5651731cfb32 | 457 | printf("PT: %d\r\n", PT); |
acloitre | 0:5651731cfb32 | 458 | printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected); |
acloitre | 0:5651731cfb32 | 459 | } |
acloitre | 0:5651731cfb32 | 460 | else{ |
acloitre | 0:5651731cfb32 | 461 | printf("\r\n !!!!!! \r\nLast word read was not the one expected.\r\n !!!!!! \r\n"); |
acloitre | 0:5651731cfb32 | 462 | printf("PT: %d\r\n", PT); |
acloitre | 0:5651731cfb32 | 463 | printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected); |
acloitre | 0:5651731cfb32 | 464 | } |
acloitre | 0:5651731cfb32 | 465 | |
acloitre | 0:5651731cfb32 | 466 | } |
acloitre | 0:5651731cfb32 | 467 | |
acloitre | 0:5651731cfb32 | 468 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 469 | //---------------------------------- Public Functions -------------------------------------- |
acloitre | 0:5651731cfb32 | 470 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 471 | |
acloitre | 0:5651731cfb32 | 472 | void UM6LT::baud(int baudrate){serial_.baud(baudrate);} |
acloitre | 0:5651731cfb32 | 473 | |
acloitre | 0:5651731cfb32 | 474 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 475 | |
acloitre | 0:5651731cfb32 | 476 | int UM6LT::readable(void){return serial_.readable();} |
acloitre | 0:5651731cfb32 | 477 | |
acloitre | 0:5651731cfb32 | 478 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 479 | |
acloitre | 0:5651731cfb32 | 480 | char UM6LT::getc(void){return serial_.getc();} |
acloitre | 0:5651731cfb32 | 481 | |
acloitre | 0:5651731cfb32 | 482 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 483 | |
acloitre | 0:5651731cfb32 | 484 | void UM6LT::putc(char byte){serial_.putc(byte);} |
acloitre | 0:5651731cfb32 | 485 | |
acloitre | 0:5651731cfb32 | 486 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 487 | |
acloitre | 0:5651731cfb32 | 488 | void UM6LT::setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled){ |
acloitre | 0:5651731cfb32 | 489 | |
acloitre | 0:5651731cfb32 | 490 | // Broadcast rate should be an integer between 20 and 300 (in Hertz) |
acloitre | 0:5651731cfb32 | 491 | // Baudrate should be either 9600, 14400, 19200, 38400, 57600 or 115200 |
acloitre | 0:5651731cfb32 | 492 | // DataToTransmit defines what data the UM6LT will provide. It's a list of 0 and 1 for a total length of N=9. See UM6LT documentation |
acloitre | 0:5651731cfb32 | 493 | |
acloitre | 0:5651731cfb32 | 494 | int data [4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 495 | int bitsList2 [8]; |
acloitre | 0:5651731cfb32 | 496 | int bitsList3 [8]; |
acloitre | 0:5651731cfb32 | 497 | int bitsList4 [8]; |
acloitre | 0:5651731cfb32 | 498 | |
acloitre | 0:5651731cfb32 | 499 | int broadcastRateByte = ((broadcastRate - 20)*255)/280; |
acloitre | 0:5651731cfb32 | 500 | |
acloitre | 0:5651731cfb32 | 501 | int baudrateByte [3]; |
acloitre | 0:5651731cfb32 | 502 | switch(baudrate){ |
acloitre | 0:5651731cfb32 | 503 | case 9600: |
acloitre | 0:5651731cfb32 | 504 | baudrateByte[0] = 0; |
acloitre | 0:5651731cfb32 | 505 | baudrateByte[1] = 0; |
acloitre | 0:5651731cfb32 | 506 | baudrateByte[2] = 0; |
acloitre | 0:5651731cfb32 | 507 | break; |
acloitre | 0:5651731cfb32 | 508 | case 14400: |
acloitre | 0:5651731cfb32 | 509 | baudrateByte[0] = 1; |
acloitre | 0:5651731cfb32 | 510 | baudrateByte[1] = 0; |
acloitre | 0:5651731cfb32 | 511 | baudrateByte[2] = 0; |
acloitre | 0:5651731cfb32 | 512 | break; |
acloitre | 0:5651731cfb32 | 513 | case 19200: |
acloitre | 0:5651731cfb32 | 514 | baudrateByte[0] = 0; |
acloitre | 0:5651731cfb32 | 515 | baudrateByte[1] = 1; |
acloitre | 0:5651731cfb32 | 516 | baudrateByte[2] = 0; |
acloitre | 0:5651731cfb32 | 517 | break; |
acloitre | 0:5651731cfb32 | 518 | case 38400: |
acloitre | 0:5651731cfb32 | 519 | baudrateByte[0] = 1; |
acloitre | 0:5651731cfb32 | 520 | baudrateByte[1] = 1; |
acloitre | 0:5651731cfb32 | 521 | baudrateByte[2] = 0; |
acloitre | 0:5651731cfb32 | 522 | break; |
acloitre | 0:5651731cfb32 | 523 | case 57600: |
acloitre | 0:5651731cfb32 | 524 | baudrateByte[0] = 0; |
acloitre | 0:5651731cfb32 | 525 | baudrateByte[1] = 0; |
acloitre | 0:5651731cfb32 | 526 | baudrateByte[2] = 1; |
acloitre | 0:5651731cfb32 | 527 | break; |
acloitre | 0:5651731cfb32 | 528 | case 115200: |
acloitre | 0:5651731cfb32 | 529 | baudrateByte[0] = 1; |
acloitre | 0:5651731cfb32 | 530 | baudrateByte[1] = 0; |
acloitre | 0:5651731cfb32 | 531 | baudrateByte[2] = 1; |
acloitre | 0:5651731cfb32 | 532 | break; |
acloitre | 0:5651731cfb32 | 533 | default: |
acloitre | 0:5651731cfb32 | 534 | baudrateByte[0] = 1; |
acloitre | 0:5651731cfb32 | 535 | baudrateByte[1] = 0; |
acloitre | 0:5651731cfb32 | 536 | baudrateByte[2] = 1; |
acloitre | 0:5651731cfb32 | 537 | printf("Baudrate not listed for UM6LT. Default = 115200.\r\n"); |
acloitre | 0:5651731cfb32 | 538 | break; |
acloitre | 0:5651731cfb32 | 539 | } |
acloitre | 0:5651731cfb32 | 540 | |
acloitre | 0:5651731cfb32 | 541 | bitsList2[0] = baudrateByte[0]; |
acloitre | 0:5651731cfb32 | 542 | bitsList2[1] = baudrateByte[1]; |
acloitre | 0:5651731cfb32 | 543 | bitsList2[2] = baudrateByte[2]; |
acloitre | 0:5651731cfb32 | 544 | for(int i=0; i<5; i++){ |
acloitre | 0:5651731cfb32 | 545 | bitsList2[i+3] = 0; |
acloitre | 0:5651731cfb32 | 546 | bitsList3[i] = 0; |
acloitre | 0:5651731cfb32 | 547 | } |
acloitre | 0:5651731cfb32 | 548 | |
acloitre | 0:5651731cfb32 | 549 | for(int i=0; i<3; i++){ |
acloitre | 0:5651731cfb32 | 550 | bitsList3[i+5] = dataToTransmit[i]; |
acloitre | 0:5651731cfb32 | 551 | } |
acloitre | 0:5651731cfb32 | 552 | |
acloitre | 0:5651731cfb32 | 553 | for(int i=0; i<6; i++){ |
acloitre | 0:5651731cfb32 | 554 | bitsList4[i] = dataToTransmit[i+3]; |
acloitre | 0:5651731cfb32 | 555 | } |
acloitre | 0:5651731cfb32 | 556 | |
acloitre | 0:5651731cfb32 | 557 | bitsList4[6] = broadcastEnabled; |
acloitre | 0:5651731cfb32 | 558 | bitsList4[7] = 0; |
acloitre | 0:5651731cfb32 | 559 | |
acloitre | 0:5651731cfb32 | 560 | data[3] = broadcastRateByte; |
acloitre | 0:5651731cfb32 | 561 | createByte(bitsList2, data[2]); |
acloitre | 0:5651731cfb32 | 562 | createByte(bitsList3, data[1]); |
acloitre | 0:5651731cfb32 | 563 | createByte(bitsList4, data[0]); |
acloitre | 0:5651731cfb32 | 564 | |
acloitre | 0:5651731cfb32 | 565 | int hasData = 1; |
acloitre | 0:5651731cfb32 | 566 | int isBatch = 0; |
acloitre | 0:5651731cfb32 | 567 | int address = UM6_COMMUNICATION; |
acloitre | 0:5651731cfb32 | 568 | int BL = 1; |
acloitre | 0:5651731cfb32 | 569 | |
acloitre | 0:5651731cfb32 | 570 | int PTread = -1; |
acloitre | 0:5651731cfb32 | 571 | int Nread = -1; |
acloitre | 0:5651731cfb32 | 572 | int addressRead = -1; |
acloitre | 0:5651731cfb32 | 573 | int dataRead[1]; |
acloitre | 0:5651731cfb32 | 574 | |
acloitre | 0:5651731cfb32 | 575 | while(addressRead != address){ |
acloitre | 0:5651731cfb32 | 576 | oneWordWrite(hasData, isBatch, BL, address, data); |
acloitre | 0:5651731cfb32 | 577 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 578 | oneWordRead(PTread, Nread, addressRead, dataRead); |
acloitre | 0:5651731cfb32 | 579 | } |
acloitre | 0:5651731cfb32 | 580 | |
acloitre | 0:5651731cfb32 | 581 | if(PTread%2 == 1){ |
acloitre | 0:5651731cfb32 | 582 | printf("Communication configuration failed\r\n"); |
acloitre | 0:5651731cfb32 | 583 | } |
acloitre | 0:5651731cfb32 | 584 | else{ |
acloitre | 0:5651731cfb32 | 585 | printf("UM6 communication configuration completed\r\n"); |
acloitre | 0:5651731cfb32 | 586 | } |
acloitre | 0:5651731cfb32 | 587 | |
acloitre | 0:5651731cfb32 | 588 | } |
acloitre | 0:5651731cfb32 | 589 | |
acloitre | 0:5651731cfb32 | 590 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 591 | |
acloitre | 0:5651731cfb32 | 592 | void UM6LT::setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate){ |
acloitre | 0:5651731cfb32 | 593 | |
acloitre | 0:5651731cfb32 | 594 | int data [4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 595 | int bitsList [8] = {0, 0, 0, 0, 0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 596 | |
acloitre | 0:5651731cfb32 | 597 | bitsList[3] = wantPPS; |
acloitre | 0:5651731cfb32 | 598 | bitsList[4] = wantQuatUpdate; |
acloitre | 0:5651731cfb32 | 599 | bitsList[5] = wantCal; |
acloitre | 0:5651731cfb32 | 600 | bitsList[6] = wantAccelUpdate; |
acloitre | 0:5651731cfb32 | 601 | bitsList[7] = wantMagUpdate; |
acloitre | 0:5651731cfb32 | 602 | |
acloitre | 0:5651731cfb32 | 603 | createByte(bitsList, data[0]); |
acloitre | 0:5651731cfb32 | 604 | |
acloitre | 0:5651731cfb32 | 605 | int hasData = 1; |
acloitre | 0:5651731cfb32 | 606 | int isBatch = 0; |
acloitre | 0:5651731cfb32 | 607 | int address = UM6_MISC_CONFIG; |
acloitre | 0:5651731cfb32 | 608 | int BL = 1; |
acloitre | 0:5651731cfb32 | 609 | |
acloitre | 0:5651731cfb32 | 610 | int PTread = -1; |
acloitre | 0:5651731cfb32 | 611 | int Nread = -1; |
acloitre | 0:5651731cfb32 | 612 | int addressRead = -1; |
acloitre | 0:5651731cfb32 | 613 | int dataRead[1]; |
acloitre | 0:5651731cfb32 | 614 | |
acloitre | 0:5651731cfb32 | 615 | while(addressRead != address){ |
acloitre | 0:5651731cfb32 | 616 | oneWordWrite(hasData, isBatch, BL, address, data); |
acloitre | 0:5651731cfb32 | 617 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 618 | oneWordRead(PTread, Nread, addressRead, dataRead); |
acloitre | 0:5651731cfb32 | 619 | } |
acloitre | 0:5651731cfb32 | 620 | |
acloitre | 0:5651731cfb32 | 621 | if(PTread%2 == 1){ |
acloitre | 0:5651731cfb32 | 622 | printf("Miscellaneous configuration failed\r\n"); |
acloitre | 0:5651731cfb32 | 623 | } |
acloitre | 0:5651731cfb32 | 624 | else{ |
acloitre | 0:5651731cfb32 | 625 | printf("UM6 miscellaneous configuration completed\r\n"); |
acloitre | 0:5651731cfb32 | 626 | } |
acloitre | 0:5651731cfb32 | 627 | |
acloitre | 0:5651731cfb32 | 628 | } |
acloitre | 0:5651731cfb32 | 629 | |
acloitre | 0:5651731cfb32 | 630 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 631 | |
acloitre | 0:5651731cfb32 | 632 | int UM6LT::getStatus(void){ |
acloitre | 0:5651731cfb32 | 633 | |
acloitre | 0:5651731cfb32 | 634 | int PT, N, address; |
acloitre | 0:5651731cfb32 | 635 | PT = -1; |
acloitre | 0:5651731cfb32 | 636 | N = -1; |
acloitre | 0:5651731cfb32 | 637 | address = -1; |
acloitre | 0:5651731cfb32 | 638 | |
acloitre | 0:5651731cfb32 | 639 | int data[4] = {1, 1, 1, 1}; |
acloitre | 0:5651731cfb32 | 640 | int bitsList1[8]; |
acloitre | 0:5651731cfb32 | 641 | int bitsList2[8]; |
acloitre | 0:5651731cfb32 | 642 | int bitsList3[8]; |
acloitre | 0:5651731cfb32 | 643 | int bitsList4[8]; |
acloitre | 0:5651731cfb32 | 644 | |
acloitre | 0:5651731cfb32 | 645 | int okToMoveOn = 0; |
acloitre | 0:5651731cfb32 | 646 | int count = 0; |
acloitre | 0:5651731cfb32 | 647 | |
acloitre | 0:5651731cfb32 | 648 | while(!okToMoveOn && count<5){ |
acloitre | 0:5651731cfb32 | 649 | requestData(UM6_STATUS, 1); |
acloitre | 0:5651731cfb32 | 650 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 651 | oneWordRead(PT, N, address, data); |
acloitre | 0:5651731cfb32 | 652 | okToMoveOn = noCommError(address, UM6_STATUS, PT); |
acloitre | 0:5651731cfb32 | 653 | count++; |
acloitre | 0:5651731cfb32 | 654 | } |
acloitre | 0:5651731cfb32 | 655 | |
acloitre | 0:5651731cfb32 | 656 | if(okToMoveOn){ |
acloitre | 0:5651731cfb32 | 657 | decomposeByte(bitsList1, data[3]); |
acloitre | 0:5651731cfb32 | 658 | decomposeByte(bitsList2, data[2]); |
acloitre | 0:5651731cfb32 | 659 | decomposeByte(bitsList3, data[1]); |
acloitre | 0:5651731cfb32 | 660 | decomposeByte(bitsList4, data[0]); |
acloitre | 0:5651731cfb32 | 661 | |
acloitre | 0:5651731cfb32 | 662 | if(bitsList1[0] == 1){ |
acloitre | 0:5651731cfb32 | 663 | printf("Self-test completed\r\n"); |
acloitre | 0:5651731cfb32 | 664 | return 0; |
acloitre | 0:5651731cfb32 | 665 | } |
acloitre | 0:5651731cfb32 | 666 | else if(bitsList2[5] == 1){ |
acloitre | 0:5651731cfb32 | 667 | printf("Mag data timed out\r\n"); |
acloitre | 0:5651731cfb32 | 668 | return 0; |
acloitre | 0:5651731cfb32 | 669 | } |
acloitre | 0:5651731cfb32 | 670 | else if(bitsList2[6] == 1){ |
acloitre | 0:5651731cfb32 | 671 | printf("Accel data timed out\r\n"); |
acloitre | 0:5651731cfb32 | 672 | return 0; |
acloitre | 0:5651731cfb32 | 673 | } |
acloitre | 0:5651731cfb32 | 674 | else if(bitsList2[7] == 1){ |
acloitre | 0:5651731cfb32 | 675 | printf("Gyro data timed out\r\n"); |
acloitre | 0:5651731cfb32 | 676 | return 0; |
acloitre | 0:5651731cfb32 | 677 | } |
acloitre | 0:5651731cfb32 | 678 | else if(bitsList3[0] == 1){ |
acloitre | 0:5651731cfb32 | 679 | printf("EKF rebooted b/c of state estimate divergence\r\n"); |
acloitre | 0:5651731cfb32 | 680 | return 0; |
acloitre | 0:5651731cfb32 | 681 | } |
acloitre | 0:5651731cfb32 | 682 | else if(bitsList3[1] == 1){ |
acloitre | 0:5651731cfb32 | 683 | printf("Bus error with Mag sensor\r\n"); |
acloitre | 0:5651731cfb32 | 684 | return 0; |
acloitre | 0:5651731cfb32 | 685 | } |
acloitre | 0:5651731cfb32 | 686 | else if(bitsList3[2] == 1){ |
acloitre | 0:5651731cfb32 | 687 | printf("Bus error with Accel sensor\r\n"); |
acloitre | 0:5651731cfb32 | 688 | return 0; |
acloitre | 0:5651731cfb32 | 689 | } |
acloitre | 0:5651731cfb32 | 690 | else if(bitsList3[3] == 1){ |
acloitre | 0:5651731cfb32 | 691 | printf("Bus error with Gyro sensor\r\n"); |
acloitre | 0:5651731cfb32 | 692 | return 0; |
acloitre | 0:5651731cfb32 | 693 | } |
acloitre | 0:5651731cfb32 | 694 | else if(bitsList3[4] == 1){ |
acloitre | 0:5651731cfb32 | 695 | printf("Self-test operation failed on Mag z-axis\r\n"); |
acloitre | 0:5651731cfb32 | 696 | return 0; |
acloitre | 0:5651731cfb32 | 697 | } |
acloitre | 0:5651731cfb32 | 698 | else if(bitsList3[5] == 1){ |
acloitre | 0:5651731cfb32 | 699 | printf("Self-test operation failed on Mag y-axis\r\n"); |
acloitre | 0:5651731cfb32 | 700 | return 0; |
acloitre | 0:5651731cfb32 | 701 | } |
acloitre | 0:5651731cfb32 | 702 | else if(bitsList3[6] == 1){ |
acloitre | 0:5651731cfb32 | 703 | printf("Self-test operation failed on Mag x-axis\r\n"); |
acloitre | 0:5651731cfb32 | 704 | return 0; |
acloitre | 0:5651731cfb32 | 705 | } |
acloitre | 0:5651731cfb32 | 706 | else if(bitsList3[7] == 1){ |
acloitre | 0:5651731cfb32 | 707 | printf("Self-test operation failed on Accel z-axis\r\n"); |
acloitre | 0:5651731cfb32 | 708 | return 0; |
acloitre | 0:5651731cfb32 | 709 | } |
acloitre | 0:5651731cfb32 | 710 | else if(bitsList4[0] == 1){ |
acloitre | 0:5651731cfb32 | 711 | printf("Self-test operation failed on Accel y-axis\r\n"); |
acloitre | 0:5651731cfb32 | 712 | return 0; |
acloitre | 0:5651731cfb32 | 713 | } |
acloitre | 0:5651731cfb32 | 714 | else if(bitsList4[1] == 1){ |
acloitre | 0:5651731cfb32 | 715 | printf("Self-test operation failed on Accel x-axis\r\n"); |
acloitre | 0:5651731cfb32 | 716 | return 0; |
acloitre | 0:5651731cfb32 | 717 | } |
acloitre | 0:5651731cfb32 | 718 | else if(bitsList4[2] == 1){ |
acloitre | 0:5651731cfb32 | 719 | printf("Self-test operation failed on Gyro z-axis\r\n"); |
acloitre | 0:5651731cfb32 | 720 | return 0; |
acloitre | 0:5651731cfb32 | 721 | } |
acloitre | 0:5651731cfb32 | 722 | else if(bitsList4[3] == 1){ |
acloitre | 0:5651731cfb32 | 723 | printf("Self-test operation failed on Gyro y-axis\r\n"); |
acloitre | 0:5651731cfb32 | 724 | return 0; |
acloitre | 0:5651731cfb32 | 725 | } |
acloitre | 0:5651731cfb32 | 726 | else if(bitsList4[4] == 1){ |
acloitre | 0:5651731cfb32 | 727 | printf("Self-test operation failed on Gyro x-axis\r\n"); |
acloitre | 0:5651731cfb32 | 728 | return 0; |
acloitre | 0:5651731cfb32 | 729 | } |
acloitre | 0:5651731cfb32 | 730 | else if(bitsList4[5] == 1){ |
acloitre | 0:5651731cfb32 | 731 | printf("Gyro start-up initialization failed -> Gyro is dead\r\n"); |
acloitre | 0:5651731cfb32 | 732 | return 0; |
acloitre | 0:5651731cfb32 | 733 | } |
acloitre | 0:5651731cfb32 | 734 | else if(bitsList4[6] == 1){ |
acloitre | 0:5651731cfb32 | 735 | printf("Accel start-up initialization failed -> Accel is dead\r\n"); |
acloitre | 0:5651731cfb32 | 736 | return 0; |
acloitre | 0:5651731cfb32 | 737 | } |
acloitre | 0:5651731cfb32 | 738 | else if(bitsList4[7] == 1){ |
acloitre | 0:5651731cfb32 | 739 | printf("Mag start-up initialization failed -> Mag is dead\r\n"); |
acloitre | 0:5651731cfb32 | 740 | return 0; |
acloitre | 0:5651731cfb32 | 741 | } |
acloitre | 0:5651731cfb32 | 742 | else{ |
acloitre | 0:5651731cfb32 | 743 | printf("No problem to report.\r\n"); |
acloitre | 0:5651731cfb32 | 744 | return 1; |
acloitre | 0:5651731cfb32 | 745 | } |
acloitre | 0:5651731cfb32 | 746 | } |
acloitre | 0:5651731cfb32 | 747 | else{ |
acloitre | 0:5651731cfb32 | 748 | WhatCommError(address, UM6_STATUS, PT); |
acloitre | 0:5651731cfb32 | 749 | return 0; |
acloitre | 0:5651731cfb32 | 750 | } |
acloitre | 0:5651731cfb32 | 751 | |
acloitre | 0:5651731cfb32 | 752 | } |
acloitre | 0:5651731cfb32 | 753 | |
acloitre | 0:5651731cfb32 | 754 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 755 | |
acloitre | 0:5651731cfb32 | 756 | int UM6LT::zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ){ |
acloitre | 0:5651731cfb32 | 757 | |
acloitre | 0:5651731cfb32 | 758 | int PT = -1; |
acloitre | 0:5651731cfb32 | 759 | int N = -1; |
acloitre | 0:5651731cfb32 | 760 | int address = -1; |
acloitre | 0:5651731cfb32 | 761 | int data [8] = {0, 0, 0, 0, 0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 762 | |
acloitre | 0:5651731cfb32 | 763 | int okToMoveOn = 0; |
acloitre | 0:5651731cfb32 | 764 | int count = 0; |
acloitre | 0:5651731cfb32 | 765 | |
acloitre | 0:5651731cfb32 | 766 | printf("Do not move the imu: gyro zeroing in progress.\r\n"); |
acloitre | 0:5651731cfb32 | 767 | |
acloitre | 0:5651731cfb32 | 768 | while(!okToMoveOn && count<5){ |
acloitre | 0:5651731cfb32 | 769 | requestData(UM6_ZERO_GYROS, 0); |
acloitre | 0:5651731cfb32 | 770 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 771 | oneWordRead(PT, N, address, data); |
acloitre | 0:5651731cfb32 | 772 | okToMoveOn = noCommError(address, UM6_ZERO_GYROS, PT); |
acloitre | 0:5651731cfb32 | 773 | count++; |
acloitre | 0:5651731cfb32 | 774 | } |
acloitre | 0:5651731cfb32 | 775 | if(okToMoveOn){ |
acloitre | 0:5651731cfb32 | 776 | okToMoveOn = 0; |
acloitre | 0:5651731cfb32 | 777 | count = 0; |
acloitre | 0:5651731cfb32 | 778 | PT = -1; |
acloitre | 0:5651731cfb32 | 779 | N = -1; |
acloitre | 0:5651731cfb32 | 780 | address = -1; |
acloitre | 0:5651731cfb32 | 781 | wait(1); |
acloitre | 0:5651731cfb32 | 782 | while(!okToMoveOn && count<5){ |
acloitre | 0:5651731cfb32 | 783 | wait_ms(stdDelayMs); |
acloitre | 0:5651731cfb32 | 784 | oneWordRead(PT, N, address, data); |
acloitre | 0:5651731cfb32 | 785 | okToMoveOn = noCommError(address, UM6_GYRO_BIAS_XY, PT); |
acloitre | 0:5651731cfb32 | 786 | count++; |
acloitre | 0:5651731cfb32 | 787 | } |
acloitre | 0:5651731cfb32 | 788 | |
acloitre | 0:5651731cfb32 | 789 | if(okToMoveOn){ |
acloitre | 0:5651731cfb32 | 790 | gyroBiasX = twosComplement(data[0], data[1]); |
acloitre | 0:5651731cfb32 | 791 | gyroBiasY = twosComplement(data[2], data[3]); |
acloitre | 0:5651731cfb32 | 792 | gyroBiasZ = twosComplement(data[4], data[5]); |
acloitre | 0:5651731cfb32 | 793 | printf("Gyro zeroing completed.\r\n"); |
acloitre | 0:5651731cfb32 | 794 | return 1; |
acloitre | 0:5651731cfb32 | 795 | } |
acloitre | 0:5651731cfb32 | 796 | else{ |
acloitre | 0:5651731cfb32 | 797 | WhatCommError(address, UM6_GYRO_BIAS_XY, PT); |
acloitre | 0:5651731cfb32 | 798 | printf("Could not acquire the values of gyro biases.\r\n"); |
acloitre | 0:5651731cfb32 | 799 | return 0; |
acloitre | 0:5651731cfb32 | 800 | } |
acloitre | 0:5651731cfb32 | 801 | } |
acloitre | 0:5651731cfb32 | 802 | else{ |
acloitre | 0:5651731cfb32 | 803 | WhatCommError(address, UM6_ZERO_GYROS, PT); |
acloitre | 0:5651731cfb32 | 804 | printf("Could not trigger gyro zeroing script.\r\n"); |
acloitre | 0:5651731cfb32 | 805 | return 0; |
acloitre | 0:5651731cfb32 | 806 | } |
acloitre | 0:5651731cfb32 | 807 | |
acloitre | 0:5651731cfb32 | 808 | } |
acloitre | 0:5651731cfb32 | 809 | |
acloitre | 0:5651731cfb32 | 810 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 811 | |
acloitre | 0:5651731cfb32 | 812 | int UM6LT::autoSetAccelRef(void){ |
acloitre | 0:5651731cfb32 | 813 | |
acloitre | 0:5651731cfb32 | 814 | int address = UM6_SET_ACCEL_REF; |
acloitre | 0:5651731cfb32 | 815 | |
acloitre | 0:5651731cfb32 | 816 | if(sendOneCommand(address)){ |
acloitre | 0:5651731cfb32 | 817 | return 1; |
acloitre | 0:5651731cfb32 | 818 | } |
acloitre | 0:5651731cfb32 | 819 | else{ |
acloitre | 0:5651731cfb32 | 820 | return 0; |
acloitre | 0:5651731cfb32 | 821 | } |
acloitre | 0:5651731cfb32 | 822 | |
acloitre | 0:5651731cfb32 | 823 | } |
acloitre | 0:5651731cfb32 | 824 | |
acloitre | 0:5651731cfb32 | 825 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 826 | |
acloitre | 0:5651731cfb32 | 827 | int UM6LT::autoSetMagRef(void){ |
acloitre | 0:5651731cfb32 | 828 | |
acloitre | 0:5651731cfb32 | 829 | int address = UM6_SET_MAG_REF; |
acloitre | 0:5651731cfb32 | 830 | |
acloitre | 0:5651731cfb32 | 831 | if(sendOneCommand(address)){ |
acloitre | 0:5651731cfb32 | 832 | return 1; |
acloitre | 0:5651731cfb32 | 833 | } |
acloitre | 0:5651731cfb32 | 834 | else{ |
acloitre | 0:5651731cfb32 | 835 | return 0; |
acloitre | 0:5651731cfb32 | 836 | } |
acloitre | 0:5651731cfb32 | 837 | } |
acloitre | 0:5651731cfb32 | 838 | |
acloitre | 0:5651731cfb32 | 839 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 840 | |
acloitre | 0:5651731cfb32 | 841 | int UM6LT::resetEKF(void){ |
acloitre | 0:5651731cfb32 | 842 | |
acloitre | 0:5651731cfb32 | 843 | int address = UM6_RESET_EKF; |
acloitre | 0:5651731cfb32 | 844 | |
acloitre | 0:5651731cfb32 | 845 | if(sendOneCommand(address)){ |
acloitre | 0:5651731cfb32 | 846 | return 1; |
acloitre | 0:5651731cfb32 | 847 | } |
acloitre | 0:5651731cfb32 | 848 | else{ |
acloitre | 0:5651731cfb32 | 849 | return 0; |
acloitre | 0:5651731cfb32 | 850 | } |
acloitre | 0:5651731cfb32 | 851 | } |
acloitre | 0:5651731cfb32 | 852 | |
acloitre | 0:5651731cfb32 | 853 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 854 | |
acloitre | 0:5651731cfb32 | 855 | int UM6LT::writeToFlash(void){ |
acloitre | 0:5651731cfb32 | 856 | |
acloitre | 0:5651731cfb32 | 857 | int address = UM6_FLASH_COMMIT; |
acloitre | 0:5651731cfb32 | 858 | |
acloitre | 0:5651731cfb32 | 859 | if(sendOneCommand(address)){ |
acloitre | 0:5651731cfb32 | 860 | return 1; |
acloitre | 0:5651731cfb32 | 861 | } |
acloitre | 0:5651731cfb32 | 862 | else{ |
acloitre | 0:5651731cfb32 | 863 | return 0; |
acloitre | 0:5651731cfb32 | 864 | } |
acloitre | 0:5651731cfb32 | 865 | } |
acloitre | 0:5651731cfb32 | 866 | |
acloitre | 0:5651731cfb32 | 867 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 868 | |
acloitre | 0:5651731cfb32 | 869 | int UM6LT::resetToFactory(void){ |
acloitre | 0:5651731cfb32 | 870 | |
acloitre | 0:5651731cfb32 | 871 | int address = UM6_RESET_TO_FACTORY; |
acloitre | 0:5651731cfb32 | 872 | |
acloitre | 0:5651731cfb32 | 873 | if(sendOneCommand(address)){ |
acloitre | 0:5651731cfb32 | 874 | return 1; |
acloitre | 0:5651731cfb32 | 875 | } |
acloitre | 0:5651731cfb32 | 876 | else{ |
acloitre | 0:5651731cfb32 | 877 | return 0; |
acloitre | 0:5651731cfb32 | 878 | } |
acloitre | 0:5651731cfb32 | 879 | |
acloitre | 0:5651731cfb32 | 880 | } |
acloitre | 0:5651731cfb32 | 881 | |
acloitre | 0:5651731cfb32 | 882 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 883 | |
acloitre | 0:5651731cfb32 | 884 | int UM6LT::getAngles(int& roll, int& pitch, int& yaw){ |
acloitre | 0:5651731cfb32 | 885 | |
acloitre | 0:5651731cfb32 | 886 | roll = 0; |
acloitre | 0:5651731cfb32 | 887 | pitch= 0; |
acloitre | 0:5651731cfb32 | 888 | yaw= 0; |
acloitre | 0:5651731cfb32 | 889 | int data[4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 890 | int address = UM6_EULER_PHI_THETA; |
acloitre | 0:5651731cfb32 | 891 | |
acloitre | 0:5651731cfb32 | 892 | if(getTwoBatches(address, data)){ |
acloitre | 0:5651731cfb32 | 893 | roll = data[0]; |
acloitre | 0:5651731cfb32 | 894 | pitch = data[1]; |
acloitre | 0:5651731cfb32 | 895 | yaw = data[2]; |
acloitre | 0:5651731cfb32 | 896 | return 1; |
acloitre | 0:5651731cfb32 | 897 | } |
acloitre | 0:5651731cfb32 | 898 | else{ |
acloitre | 0:5651731cfb32 | 899 | return 0; |
acloitre | 0:5651731cfb32 | 900 | } |
acloitre | 0:5651731cfb32 | 901 | |
acloitre | 0:5651731cfb32 | 902 | } |
acloitre | 0:5651731cfb32 | 903 | |
acloitre | 0:5651731cfb32 | 904 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 905 | |
acloitre | 0:5651731cfb32 | 906 | |
acloitre | 0:5651731cfb32 | 907 | int UM6LT::getAccel(int& accelX, int& accelY, int& accelZ){ |
acloitre | 0:5651731cfb32 | 908 | |
acloitre | 0:5651731cfb32 | 909 | accelX = 0; |
acloitre | 0:5651731cfb32 | 910 | accelY = 0; |
acloitre | 0:5651731cfb32 | 911 | accelZ = 0; |
acloitre | 0:5651731cfb32 | 912 | int data[4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 913 | int address = UM6_ACCEL_PROC_XY; |
acloitre | 0:5651731cfb32 | 914 | |
acloitre | 0:5651731cfb32 | 915 | if(getTwoBatches(address, data)){ |
acloitre | 0:5651731cfb32 | 916 | accelX = data[0]; |
acloitre | 0:5651731cfb32 | 917 | accelY = data[1]; |
acloitre | 0:5651731cfb32 | 918 | accelZ = data[2]; |
acloitre | 0:5651731cfb32 | 919 | return 1; |
acloitre | 0:5651731cfb32 | 920 | } |
acloitre | 0:5651731cfb32 | 921 | else{ |
acloitre | 0:5651731cfb32 | 922 | return 0; |
acloitre | 0:5651731cfb32 | 923 | } |
acloitre | 0:5651731cfb32 | 924 | |
acloitre | 0:5651731cfb32 | 925 | } |
acloitre | 0:5651731cfb32 | 926 | |
acloitre | 0:5651731cfb32 | 927 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 928 | |
acloitre | 0:5651731cfb32 | 929 | |
acloitre | 0:5651731cfb32 | 930 | int UM6LT::getMag(int& magX, int& magY, int& magZ){ |
acloitre | 0:5651731cfb32 | 931 | |
acloitre | 0:5651731cfb32 | 932 | magX = 0; |
acloitre | 0:5651731cfb32 | 933 | magY = 0; |
acloitre | 0:5651731cfb32 | 934 | magZ = 0; |
acloitre | 0:5651731cfb32 | 935 | int data[4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 936 | int address = UM6_MAG_PROC_XY; |
acloitre | 0:5651731cfb32 | 937 | |
acloitre | 0:5651731cfb32 | 938 | if(getTwoBatches(address, data)){ |
acloitre | 0:5651731cfb32 | 939 | magX = data[0]; |
acloitre | 0:5651731cfb32 | 940 | magY = data[1]; |
acloitre | 0:5651731cfb32 | 941 | magZ = data[2]; |
acloitre | 0:5651731cfb32 | 942 | return 1; |
acloitre | 0:5651731cfb32 | 943 | } |
acloitre | 0:5651731cfb32 | 944 | else{ |
acloitre | 0:5651731cfb32 | 945 | return 0; |
acloitre | 0:5651731cfb32 | 946 | } |
acloitre | 0:5651731cfb32 | 947 | |
acloitre | 0:5651731cfb32 | 948 | } |
acloitre | 0:5651731cfb32 | 949 | |
acloitre | 0:5651731cfb32 | 950 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 951 | |
acloitre | 0:5651731cfb32 | 952 | |
acloitre | 0:5651731cfb32 | 953 | int UM6LT::getAngleRates(int& rollRate, int& pitchRate, int& yawRate){ |
acloitre | 0:5651731cfb32 | 954 | |
acloitre | 0:5651731cfb32 | 955 | rollRate = 0; |
acloitre | 0:5651731cfb32 | 956 | pitchRate = 0; |
acloitre | 0:5651731cfb32 | 957 | yawRate = 0; |
acloitre | 0:5651731cfb32 | 958 | int data[4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 959 | int address = UM6_GYRO_PROC_XY; |
acloitre | 0:5651731cfb32 | 960 | |
acloitre | 0:5651731cfb32 | 961 | if(getTwoBatches(address, data)){ |
acloitre | 0:5651731cfb32 | 962 | rollRate = data[0]; |
acloitre | 0:5651731cfb32 | 963 | pitchRate = data[1]; |
acloitre | 0:5651731cfb32 | 964 | yawRate = data[2]; |
acloitre | 0:5651731cfb32 | 965 | return 1; |
acloitre | 0:5651731cfb32 | 966 | } |
acloitre | 0:5651731cfb32 | 967 | else{ |
acloitre | 0:5651731cfb32 | 968 | return 0; |
acloitre | 0:5651731cfb32 | 969 | } |
acloitre | 0:5651731cfb32 | 970 | |
acloitre | 0:5651731cfb32 | 971 | } |
acloitre | 0:5651731cfb32 | 972 | |
acloitre | 0:5651731cfb32 | 973 | //---------------------------------------------------------------------------------------------- |
acloitre | 0:5651731cfb32 | 974 | |
acloitre | 0:5651731cfb32 | 975 | int UM6LT::getQuaternion(int& a, int& b, int& c, int& d){ |
acloitre | 0:5651731cfb32 | 976 | |
acloitre | 0:5651731cfb32 | 977 | a = 0; |
acloitre | 0:5651731cfb32 | 978 | b = 0; |
acloitre | 0:5651731cfb32 | 979 | c = 0; |
acloitre | 0:5651731cfb32 | 980 | d = 0; |
acloitre | 0:5651731cfb32 | 981 | int data[4] = {0, 0, 0, 0}; |
acloitre | 0:5651731cfb32 | 982 | int address = UM6_QUAT_AB; |
acloitre | 0:5651731cfb32 | 983 | |
acloitre | 0:5651731cfb32 | 984 | if(getTwoBatches(address, data)){ |
acloitre | 0:5651731cfb32 | 985 | a = data[0]; |
acloitre | 0:5651731cfb32 | 986 | b = data[1]; |
acloitre | 0:5651731cfb32 | 987 | c = data[2]; |
acloitre | 0:5651731cfb32 | 988 | d = data[3]; |
acloitre | 0:5651731cfb32 | 989 | return 1; |
acloitre | 0:5651731cfb32 | 990 | } |
acloitre | 0:5651731cfb32 | 991 | else{ |
acloitre | 0:5651731cfb32 | 992 | return 0; |
acloitre | 0:5651731cfb32 | 993 | } |
acloitre | 0:5651731cfb32 | 994 | |
acloitre | 0:5651731cfb32 | 995 | } |
acloitre | 0:5651731cfb32 | 996 | |
acloitre | 0:5651731cfb32 | 997 | //---------------------------------------------------------------------------------------------- |