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.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?

UserRevisionLine numberNew 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 //----------------------------------------------------------------------------------------------