Chris Charters / UM6LT

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

Fork of UM6LT by Audren Cloitre

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

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

Who changed what in which revision?

UserRevisionLine numberNew contents of line
acloitre 0:5651731cfb32 1 #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){
chartersauce25 1:703ba8e8b2cd 331
chartersauce25 1:703ba8e8b2cd 332 /*MAJOR CHANGE:
chartersauce25 1:703ba8e8b2cd 333 - UM6_QUAT_CD ADDED
chartersauce25 1:703ba8e8b2cd 334 - UM6_EULER_PSI ADDED
chartersauce25 1:703ba8e8b2cd 335 - UM6_MAG_PROC_Z ADDED
chartersauce25 1:703ba8e8b2cd 336 - UM6_ACCEL_PROC_Z ADDED
chartersauce25 1:703ba8e8b2cd 337 -
chartersauce25 1:703ba8e8b2cd 338 */
chartersauce25 1:703ba8e8b2cd 339
acloitre 0:5651731cfb32 340 dataToPost[0] = 0;
acloitre 0:5651731cfb32 341 dataToPost[1] = 0;
acloitre 0:5651731cfb32 342 dataToPost[2] = 0;
acloitre 0:5651731cfb32 343 dataToPost[3] = 0;
acloitre 0:5651731cfb32 344
acloitre 0:5651731cfb32 345 int PT = -1;
acloitre 0:5651731cfb32 346 int N = -1;
acloitre 0:5651731cfb32 347 int addressRead = -1;
acloitre 0:5651731cfb32 348 int dataToRead [8] = {0,0,0,0,0,0,0,0};
acloitre 0:5651731cfb32 349
acloitre 0:5651731cfb32 350 int okToMoveOn = 0;
acloitre 0:5651731cfb32 351 int count = 0;
acloitre 0:5651731cfb32 352
acloitre 0:5651731cfb32 353 while(!okToMoveOn && count<5){
acloitre 0:5651731cfb32 354 requestData(address, 2);
acloitre 0:5651731cfb32 355 wait_ms(stdDelayMs);
acloitre 0:5651731cfb32 356 oneWordRead(PT, N, addressRead, dataToRead);
acloitre 0:5651731cfb32 357 okToMoveOn = noCommError(addressRead, address, PT);
acloitre 0:5651731cfb32 358 count++;
acloitre 0:5651731cfb32 359 }
acloitre 0:5651731cfb32 360
acloitre 0:5651731cfb32 361 if(okToMoveOn){
acloitre 0:5651731cfb32 362 switch(address){
acloitre 0:5651731cfb32 363 case UM6_EULER_PHI_THETA:
acloitre 0:5651731cfb32 364 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR);
acloitre 0:5651731cfb32 365 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_FACTOR);
chartersauce25 1:703ba8e8b2cd 366 break;
chartersauce25 1:703ba8e8b2cd 367 case UM6_EULER_PSI:
chartersauce25 1:703ba8e8b2cd 368 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_FACTOR);
acloitre 0:5651731cfb32 369 break;
acloitre 0:5651731cfb32 370 case UM6_ACCEL_PROC_XY:
acloitre 0:5651731cfb32 371 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000);
acloitre 0:5651731cfb32 372 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ACCEL_FACTOR*1000);
chartersauce25 1:703ba8e8b2cd 373 break;
chartersauce25 1:703ba8e8b2cd 374 case UM6_ACCEL_PROC_Z:
chartersauce25 1:703ba8e8b2cd 375 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ACCEL_FACTOR*1000);
acloitre 0:5651731cfb32 376 break;
acloitre 0:5651731cfb32 377 case UM6_MAG_PROC_XY:
acloitre 0:5651731cfb32 378 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000);
acloitre 0:5651731cfb32 379 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_MAG_FACTOR*1000);
chartersauce25 1:703ba8e8b2cd 380 break;
chartersauce25 1:703ba8e8b2cd 381 case UM6_MAG_PROC_Z:
chartersauce25 1:703ba8e8b2cd 382 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_MAG_FACTOR*1000);
acloitre 0:5651731cfb32 383 break;
acloitre 0:5651731cfb32 384 case UM6_GYRO_PROC_XY:
acloitre 0:5651731cfb32 385 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR);
acloitre 0:5651731cfb32 386 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_ANGLE_RATE_FACTOR);
chartersauce25 1:703ba8e8b2cd 387 break;
chartersauce25 1:703ba8e8b2cd 388 case UM6_GYRO_PROC_Z:
chartersauce25 1:703ba8e8b2cd 389 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_ANGLE_RATE_FACTOR);
acloitre 0:5651731cfb32 390 break;
acloitre 0:5651731cfb32 391 case UM6_QUAT_AB:
acloitre 0:5651731cfb32 392 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000);
acloitre 0:5651731cfb32 393 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000);
chartersauce25 1:703ba8e8b2cd 394 break;
chartersauce25 1:703ba8e8b2cd 395 case UM6_QUAT_CD:
chartersauce25 1:703ba8e8b2cd 396 dataToPost[0] = (int)(twosComplement(dataToRead[0], dataToRead[1])*UM6_QUATERNION_FACTOR*1000);
chartersauce25 1:703ba8e8b2cd 397 dataToPost[1] = (int)(twosComplement(dataToRead[2], dataToRead[3])*UM6_QUATERNION_FACTOR*1000);
acloitre 0:5651731cfb32 398 break;
acloitre 0:5651731cfb32 399 default:
acloitre 0:5651731cfb32 400 dataToPost[0] = -1;
acloitre 0:5651731cfb32 401 dataToPost[1] = -1;
acloitre 0:5651731cfb32 402 dataToPost[2] = -1;
acloitre 0:5651731cfb32 403 dataToPost[3] = -1;
acloitre 0:5651731cfb32 404 printf("Data acquisition not programmed for this address: %d\r\n", address);
acloitre 0:5651731cfb32 405 break;
acloitre 0:5651731cfb32 406 }
acloitre 0:5651731cfb32 407 return 1;
acloitre 0:5651731cfb32 408 }
acloitre 0:5651731cfb32 409 else{
acloitre 0:5651731cfb32 410 WhatCommError(address, UM6_ACCEL_PROC_XY, PT);
acloitre 0:5651731cfb32 411 switch(address){
acloitre 0:5651731cfb32 412 case UM6_EULER_PHI_THETA:
acloitre 0:5651731cfb32 413 printf("Could not acquire the values of the Euler angles.\r\n");
acloitre 0:5651731cfb32 414 break;
acloitre 0:5651731cfb32 415 case UM6_ACCEL_PROC_XY:
acloitre 0:5651731cfb32 416 printf("Could not acquire the components of the acceleration vector.\r\n");
acloitre 0:5651731cfb32 417 break;
acloitre 0:5651731cfb32 418 case UM6_MAG_PROC_XY:
acloitre 0:5651731cfb32 419 printf("Could not acquire the components of the magnetic vector.\r\n");
acloitre 0:5651731cfb32 420 break;
acloitre 0:5651731cfb32 421 case UM6_GYRO_PROC_XY:
acloitre 0:5651731cfb32 422 printf("Could not acquire the values of angle rates.\r\n");
acloitre 0:5651731cfb32 423 break;
acloitre 0:5651731cfb32 424 case UM6_QUAT_AB:
acloitre 0:5651731cfb32 425 printf("Could not acquire the components of the quaternion.\r\n");
acloitre 0:5651731cfb32 426 break;
acloitre 0:5651731cfb32 427 default:
acloitre 0:5651731cfb32 428 printf("Data acquisition not programmed for this address: %d\r\n", address);
acloitre 0:5651731cfb32 429 break;
acloitre 0:5651731cfb32 430 }
acloitre 0:5651731cfb32 431 return 0;
acloitre 0:5651731cfb32 432 }
acloitre 0:5651731cfb32 433
acloitre 0:5651731cfb32 434 }
acloitre 0:5651731cfb32 435
acloitre 0:5651731cfb32 436 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 437
acloitre 0:5651731cfb32 438 int UM6LT::noCommError(int addressRead, int addressExpected, int PT){
acloitre 0:5651731cfb32 439
acloitre 0:5651731cfb32 440 if(addressRead == 0xFD){
acloitre 0:5651731cfb32 441 return 0;
acloitre 0:5651731cfb32 442 }
acloitre 0:5651731cfb32 443 else if(addressRead == 0xFE){
acloitre 0:5651731cfb32 444 return 0;
acloitre 0:5651731cfb32 445 }
acloitre 0:5651731cfb32 446 else if(addressRead == 0xFF){
acloitre 0:5651731cfb32 447 return 0;
acloitre 0:5651731cfb32 448 }
acloitre 0:5651731cfb32 449 else if(addressRead == addressExpected){
acloitre 0:5651731cfb32 450 return 1;
acloitre 0:5651731cfb32 451 }
acloitre 0:5651731cfb32 452 else{
acloitre 0:5651731cfb32 453 return 0;
acloitre 0:5651731cfb32 454 }
acloitre 0:5651731cfb32 455
acloitre 0:5651731cfb32 456 }
acloitre 0:5651731cfb32 457
acloitre 0:5651731cfb32 458 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 459
acloitre 0:5651731cfb32 460 void UM6LT::WhatCommError(int addressRead, int addressExpected, int PT){
acloitre 0:5651731cfb32 461
acloitre 0:5651731cfb32 462 if(addressRead == 0xFD){
acloitre 0:5651731cfb32 463 printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad checksum.\r\n !!!!!! \r\n");
acloitre 0:5651731cfb32 464 }
acloitre 0:5651731cfb32 465 else if(addressRead == 0xFE){
acloitre 0:5651731cfb32 466 printf("\r\n !!!!!! \r\nLast word was sent to UM6 at an unknown address.\r\n !!!!!! \r\n");
acloitre 0:5651731cfb32 467 printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected);
acloitre 0:5651731cfb32 468 }
acloitre 0:5651731cfb32 469 else if(addressRead == 0xFF){
acloitre 0:5651731cfb32 470 printf("\r\n !!!!!! \r\nLast word sent to UM6 had a bad batch size.\r\n !!!!!! \r\n");
acloitre 0:5651731cfb32 471 printf("PT: %d\r\n", PT);
acloitre 0:5651731cfb32 472 }
acloitre 0:5651731cfb32 473 else if(addressRead == addressExpected){
acloitre 0:5651731cfb32 474 printf("No communication error detected...\r\n");
acloitre 0:5651731cfb32 475 printf("PT: %d\r\n", PT);
acloitre 0:5651731cfb32 476 printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected);
acloitre 0:5651731cfb32 477 }
acloitre 0:5651731cfb32 478 else{
acloitre 0:5651731cfb32 479 printf("\r\n !!!!!! \r\nLast word read was not the one expected.\r\n !!!!!! \r\n");
acloitre 0:5651731cfb32 480 printf("PT: %d\r\n", PT);
acloitre 0:5651731cfb32 481 printf("address read: 0x%x\r\naddress expected: 0x%x\r\n", addressRead, addressExpected);
acloitre 0:5651731cfb32 482 }
acloitre 0:5651731cfb32 483
acloitre 0:5651731cfb32 484 }
acloitre 0:5651731cfb32 485
acloitre 0:5651731cfb32 486 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 487 //---------------------------------- Public Functions --------------------------------------
acloitre 0:5651731cfb32 488 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 489
acloitre 0:5651731cfb32 490 void UM6LT::baud(int baudrate){serial_.baud(baudrate);}
acloitre 0:5651731cfb32 491
acloitre 0:5651731cfb32 492 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 493
acloitre 0:5651731cfb32 494 int UM6LT::readable(void){return serial_.readable();}
acloitre 0:5651731cfb32 495
acloitre 0:5651731cfb32 496 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 497
acloitre 0:5651731cfb32 498 char UM6LT::getc(void){return serial_.getc();}
acloitre 0:5651731cfb32 499
acloitre 0:5651731cfb32 500 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 501
acloitre 0:5651731cfb32 502 void UM6LT::putc(char byte){serial_.putc(byte);}
acloitre 0:5651731cfb32 503
acloitre 0:5651731cfb32 504 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 505
acloitre 0:5651731cfb32 506 void UM6LT::setCommParams(int broadcastRate, int baudrate, int* dataToTransmit, int broadcastEnabled){
acloitre 0:5651731cfb32 507
acloitre 0:5651731cfb32 508 // Broadcast rate should be an integer between 20 and 300 (in Hertz)
acloitre 0:5651731cfb32 509 // Baudrate should be either 9600, 14400, 19200, 38400, 57600 or 115200
acloitre 0:5651731cfb32 510 // 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 511
acloitre 0:5651731cfb32 512 int data [4] = {0, 0, 0, 0};
acloitre 0:5651731cfb32 513 int bitsList2 [8];
acloitre 0:5651731cfb32 514 int bitsList3 [8];
acloitre 0:5651731cfb32 515 int bitsList4 [8];
acloitre 0:5651731cfb32 516
acloitre 0:5651731cfb32 517 int broadcastRateByte = ((broadcastRate - 20)*255)/280;
acloitre 0:5651731cfb32 518
acloitre 0:5651731cfb32 519 int baudrateByte [3];
acloitre 0:5651731cfb32 520 switch(baudrate){
acloitre 0:5651731cfb32 521 case 9600:
acloitre 0:5651731cfb32 522 baudrateByte[0] = 0;
acloitre 0:5651731cfb32 523 baudrateByte[1] = 0;
acloitre 0:5651731cfb32 524 baudrateByte[2] = 0;
acloitre 0:5651731cfb32 525 break;
acloitre 0:5651731cfb32 526 case 14400:
acloitre 0:5651731cfb32 527 baudrateByte[0] = 1;
acloitre 0:5651731cfb32 528 baudrateByte[1] = 0;
acloitre 0:5651731cfb32 529 baudrateByte[2] = 0;
acloitre 0:5651731cfb32 530 break;
acloitre 0:5651731cfb32 531 case 19200:
acloitre 0:5651731cfb32 532 baudrateByte[0] = 0;
acloitre 0:5651731cfb32 533 baudrateByte[1] = 1;
acloitre 0:5651731cfb32 534 baudrateByte[2] = 0;
acloitre 0:5651731cfb32 535 break;
acloitre 0:5651731cfb32 536 case 38400:
acloitre 0:5651731cfb32 537 baudrateByte[0] = 1;
acloitre 0:5651731cfb32 538 baudrateByte[1] = 1;
acloitre 0:5651731cfb32 539 baudrateByte[2] = 0;
acloitre 0:5651731cfb32 540 break;
acloitre 0:5651731cfb32 541 case 57600:
acloitre 0:5651731cfb32 542 baudrateByte[0] = 0;
acloitre 0:5651731cfb32 543 baudrateByte[1] = 0;
acloitre 0:5651731cfb32 544 baudrateByte[2] = 1;
acloitre 0:5651731cfb32 545 break;
acloitre 0:5651731cfb32 546 case 115200:
acloitre 0:5651731cfb32 547 baudrateByte[0] = 1;
acloitre 0:5651731cfb32 548 baudrateByte[1] = 0;
acloitre 0:5651731cfb32 549 baudrateByte[2] = 1;
acloitre 0:5651731cfb32 550 break;
acloitre 0:5651731cfb32 551 default:
acloitre 0:5651731cfb32 552 baudrateByte[0] = 1;
acloitre 0:5651731cfb32 553 baudrateByte[1] = 0;
acloitre 0:5651731cfb32 554 baudrateByte[2] = 1;
acloitre 0:5651731cfb32 555 printf("Baudrate not listed for UM6LT. Default = 115200.\r\n");
acloitre 0:5651731cfb32 556 break;
acloitre 0:5651731cfb32 557 }
acloitre 0:5651731cfb32 558
acloitre 0:5651731cfb32 559 bitsList2[0] = baudrateByte[0];
acloitre 0:5651731cfb32 560 bitsList2[1] = baudrateByte[1];
acloitre 0:5651731cfb32 561 bitsList2[2] = baudrateByte[2];
acloitre 0:5651731cfb32 562 for(int i=0; i<5; i++){
acloitre 0:5651731cfb32 563 bitsList2[i+3] = 0;
acloitre 0:5651731cfb32 564 bitsList3[i] = 0;
acloitre 0:5651731cfb32 565 }
acloitre 0:5651731cfb32 566
acloitre 0:5651731cfb32 567 for(int i=0; i<3; i++){
acloitre 0:5651731cfb32 568 bitsList3[i+5] = dataToTransmit[i];
acloitre 0:5651731cfb32 569 }
acloitre 0:5651731cfb32 570
acloitre 0:5651731cfb32 571 for(int i=0; i<6; i++){
acloitre 0:5651731cfb32 572 bitsList4[i] = dataToTransmit[i+3];
acloitre 0:5651731cfb32 573 }
acloitre 0:5651731cfb32 574
acloitre 0:5651731cfb32 575 bitsList4[6] = broadcastEnabled;
acloitre 0:5651731cfb32 576 bitsList4[7] = 0;
acloitre 0:5651731cfb32 577
acloitre 0:5651731cfb32 578 data[3] = broadcastRateByte;
acloitre 0:5651731cfb32 579 createByte(bitsList2, data[2]);
acloitre 0:5651731cfb32 580 createByte(bitsList3, data[1]);
acloitre 0:5651731cfb32 581 createByte(bitsList4, data[0]);
acloitre 0:5651731cfb32 582
acloitre 0:5651731cfb32 583 int hasData = 1;
acloitre 0:5651731cfb32 584 int isBatch = 0;
acloitre 0:5651731cfb32 585 int address = UM6_COMMUNICATION;
acloitre 0:5651731cfb32 586 int BL = 1;
acloitre 0:5651731cfb32 587
acloitre 0:5651731cfb32 588 int PTread = -1;
acloitre 0:5651731cfb32 589 int Nread = -1;
acloitre 0:5651731cfb32 590 int addressRead = -1;
acloitre 0:5651731cfb32 591 int dataRead[1];
acloitre 0:5651731cfb32 592
acloitre 0:5651731cfb32 593 while(addressRead != address){
acloitre 0:5651731cfb32 594 oneWordWrite(hasData, isBatch, BL, address, data);
acloitre 0:5651731cfb32 595 wait_ms(stdDelayMs);
acloitre 0:5651731cfb32 596 oneWordRead(PTread, Nread, addressRead, dataRead);
acloitre 0:5651731cfb32 597 }
acloitre 0:5651731cfb32 598
acloitre 0:5651731cfb32 599 if(PTread%2 == 1){
acloitre 0:5651731cfb32 600 printf("Communication configuration failed\r\n");
acloitre 0:5651731cfb32 601 }
acloitre 0:5651731cfb32 602 else{
acloitre 0:5651731cfb32 603 printf("UM6 communication configuration completed\r\n");
acloitre 0:5651731cfb32 604 }
acloitre 0:5651731cfb32 605
acloitre 0:5651731cfb32 606 }
acloitre 0:5651731cfb32 607
acloitre 0:5651731cfb32 608 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 609
acloitre 0:5651731cfb32 610 void UM6LT::setConfigParams(int wantPPS, int wantQuatUpdate, int wantCal, int wantAccelUpdate, int wantMagUpdate){
acloitre 0:5651731cfb32 611
acloitre 0:5651731cfb32 612 int data [4] = {0, 0, 0, 0};
acloitre 0:5651731cfb32 613 int bitsList [8] = {0, 0, 0, 0, 0, 0, 0, 0};
acloitre 0:5651731cfb32 614
acloitre 0:5651731cfb32 615 bitsList[3] = wantPPS;
acloitre 0:5651731cfb32 616 bitsList[4] = wantQuatUpdate;
acloitre 0:5651731cfb32 617 bitsList[5] = wantCal;
acloitre 0:5651731cfb32 618 bitsList[6] = wantAccelUpdate;
acloitre 0:5651731cfb32 619 bitsList[7] = wantMagUpdate;
acloitre 0:5651731cfb32 620
acloitre 0:5651731cfb32 621 createByte(bitsList, data[0]);
acloitre 0:5651731cfb32 622
acloitre 0:5651731cfb32 623 int hasData = 1;
acloitre 0:5651731cfb32 624 int isBatch = 0;
acloitre 0:5651731cfb32 625 int address = UM6_MISC_CONFIG;
acloitre 0:5651731cfb32 626 int BL = 1;
acloitre 0:5651731cfb32 627
acloitre 0:5651731cfb32 628 int PTread = -1;
acloitre 0:5651731cfb32 629 int Nread = -1;
acloitre 0:5651731cfb32 630 int addressRead = -1;
acloitre 0:5651731cfb32 631 int dataRead[1];
acloitre 0:5651731cfb32 632
acloitre 0:5651731cfb32 633 while(addressRead != address){
acloitre 0:5651731cfb32 634 oneWordWrite(hasData, isBatch, BL, address, data);
acloitre 0:5651731cfb32 635 wait_ms(stdDelayMs);
acloitre 0:5651731cfb32 636 oneWordRead(PTread, Nread, addressRead, dataRead);
acloitre 0:5651731cfb32 637 }
acloitre 0:5651731cfb32 638
acloitre 0:5651731cfb32 639 if(PTread%2 == 1){
acloitre 0:5651731cfb32 640 printf("Miscellaneous configuration failed\r\n");
acloitre 0:5651731cfb32 641 }
acloitre 0:5651731cfb32 642 else{
acloitre 0:5651731cfb32 643 printf("UM6 miscellaneous configuration completed\r\n");
acloitre 0:5651731cfb32 644 }
acloitre 0:5651731cfb32 645
acloitre 0:5651731cfb32 646 }
acloitre 0:5651731cfb32 647
acloitre 0:5651731cfb32 648 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 649
acloitre 0:5651731cfb32 650 int UM6LT::getStatus(void){
acloitre 0:5651731cfb32 651
acloitre 0:5651731cfb32 652 int PT, N, address;
acloitre 0:5651731cfb32 653 PT = -1;
acloitre 0:5651731cfb32 654 N = -1;
acloitre 0:5651731cfb32 655 address = -1;
acloitre 0:5651731cfb32 656
acloitre 0:5651731cfb32 657 int data[4] = {1, 1, 1, 1};
acloitre 0:5651731cfb32 658 int bitsList1[8];
acloitre 0:5651731cfb32 659 int bitsList2[8];
acloitre 0:5651731cfb32 660 int bitsList3[8];
acloitre 0:5651731cfb32 661 int bitsList4[8];
acloitre 0:5651731cfb32 662
acloitre 0:5651731cfb32 663 int okToMoveOn = 0;
acloitre 0:5651731cfb32 664 int count = 0;
acloitre 0:5651731cfb32 665
acloitre 0:5651731cfb32 666 while(!okToMoveOn && count<5){
acloitre 0:5651731cfb32 667 requestData(UM6_STATUS, 1);
acloitre 0:5651731cfb32 668 wait_ms(stdDelayMs);
acloitre 0:5651731cfb32 669 oneWordRead(PT, N, address, data);
acloitre 0:5651731cfb32 670 okToMoveOn = noCommError(address, UM6_STATUS, PT);
acloitre 0:5651731cfb32 671 count++;
acloitre 0:5651731cfb32 672 }
acloitre 0:5651731cfb32 673
acloitre 0:5651731cfb32 674 if(okToMoveOn){
acloitre 0:5651731cfb32 675 decomposeByte(bitsList1, data[3]);
acloitre 0:5651731cfb32 676 decomposeByte(bitsList2, data[2]);
acloitre 0:5651731cfb32 677 decomposeByte(bitsList3, data[1]);
acloitre 0:5651731cfb32 678 decomposeByte(bitsList4, data[0]);
acloitre 0:5651731cfb32 679
acloitre 0:5651731cfb32 680 if(bitsList1[0] == 1){
acloitre 0:5651731cfb32 681 printf("Self-test completed\r\n");
acloitre 0:5651731cfb32 682 return 0;
acloitre 0:5651731cfb32 683 }
acloitre 0:5651731cfb32 684 else if(bitsList2[5] == 1){
acloitre 0:5651731cfb32 685 printf("Mag data timed out\r\n");
acloitre 0:5651731cfb32 686 return 0;
acloitre 0:5651731cfb32 687 }
acloitre 0:5651731cfb32 688 else if(bitsList2[6] == 1){
acloitre 0:5651731cfb32 689 printf("Accel data timed out\r\n");
acloitre 0:5651731cfb32 690 return 0;
acloitre 0:5651731cfb32 691 }
acloitre 0:5651731cfb32 692 else if(bitsList2[7] == 1){
acloitre 0:5651731cfb32 693 printf("Gyro data timed out\r\n");
acloitre 0:5651731cfb32 694 return 0;
acloitre 0:5651731cfb32 695 }
acloitre 0:5651731cfb32 696 else if(bitsList3[0] == 1){
acloitre 0:5651731cfb32 697 printf("EKF rebooted b/c of state estimate divergence\r\n");
acloitre 0:5651731cfb32 698 return 0;
acloitre 0:5651731cfb32 699 }
acloitre 0:5651731cfb32 700 else if(bitsList3[1] == 1){
acloitre 0:5651731cfb32 701 printf("Bus error with Mag sensor\r\n");
acloitre 0:5651731cfb32 702 return 0;
acloitre 0:5651731cfb32 703 }
acloitre 0:5651731cfb32 704 else if(bitsList3[2] == 1){
acloitre 0:5651731cfb32 705 printf("Bus error with Accel sensor\r\n");
acloitre 0:5651731cfb32 706 return 0;
acloitre 0:5651731cfb32 707 }
acloitre 0:5651731cfb32 708 else if(bitsList3[3] == 1){
acloitre 0:5651731cfb32 709 printf("Bus error with Gyro sensor\r\n");
acloitre 0:5651731cfb32 710 return 0;
acloitre 0:5651731cfb32 711 }
acloitre 0:5651731cfb32 712 else if(bitsList3[4] == 1){
acloitre 0:5651731cfb32 713 printf("Self-test operation failed on Mag z-axis\r\n");
acloitre 0:5651731cfb32 714 return 0;
acloitre 0:5651731cfb32 715 }
acloitre 0:5651731cfb32 716 else if(bitsList3[5] == 1){
acloitre 0:5651731cfb32 717 printf("Self-test operation failed on Mag y-axis\r\n");
acloitre 0:5651731cfb32 718 return 0;
acloitre 0:5651731cfb32 719 }
acloitre 0:5651731cfb32 720 else if(bitsList3[6] == 1){
acloitre 0:5651731cfb32 721 printf("Self-test operation failed on Mag x-axis\r\n");
acloitre 0:5651731cfb32 722 return 0;
acloitre 0:5651731cfb32 723 }
acloitre 0:5651731cfb32 724 else if(bitsList3[7] == 1){
acloitre 0:5651731cfb32 725 printf("Self-test operation failed on Accel z-axis\r\n");
acloitre 0:5651731cfb32 726 return 0;
acloitre 0:5651731cfb32 727 }
acloitre 0:5651731cfb32 728 else if(bitsList4[0] == 1){
acloitre 0:5651731cfb32 729 printf("Self-test operation failed on Accel y-axis\r\n");
acloitre 0:5651731cfb32 730 return 0;
acloitre 0:5651731cfb32 731 }
acloitre 0:5651731cfb32 732 else if(bitsList4[1] == 1){
acloitre 0:5651731cfb32 733 printf("Self-test operation failed on Accel x-axis\r\n");
acloitre 0:5651731cfb32 734 return 0;
acloitre 0:5651731cfb32 735 }
acloitre 0:5651731cfb32 736 else if(bitsList4[2] == 1){
acloitre 0:5651731cfb32 737 printf("Self-test operation failed on Gyro z-axis\r\n");
acloitre 0:5651731cfb32 738 return 0;
acloitre 0:5651731cfb32 739 }
acloitre 0:5651731cfb32 740 else if(bitsList4[3] == 1){
acloitre 0:5651731cfb32 741 printf("Self-test operation failed on Gyro y-axis\r\n");
acloitre 0:5651731cfb32 742 return 0;
acloitre 0:5651731cfb32 743 }
acloitre 0:5651731cfb32 744 else if(bitsList4[4] == 1){
acloitre 0:5651731cfb32 745 printf("Self-test operation failed on Gyro x-axis\r\n");
acloitre 0:5651731cfb32 746 return 0;
acloitre 0:5651731cfb32 747 }
acloitre 0:5651731cfb32 748 else if(bitsList4[5] == 1){
acloitre 0:5651731cfb32 749 printf("Gyro start-up initialization failed -> Gyro is dead\r\n");
acloitre 0:5651731cfb32 750 return 0;
acloitre 0:5651731cfb32 751 }
acloitre 0:5651731cfb32 752 else if(bitsList4[6] == 1){
acloitre 0:5651731cfb32 753 printf("Accel start-up initialization failed -> Accel is dead\r\n");
acloitre 0:5651731cfb32 754 return 0;
acloitre 0:5651731cfb32 755 }
acloitre 0:5651731cfb32 756 else if(bitsList4[7] == 1){
acloitre 0:5651731cfb32 757 printf("Mag start-up initialization failed -> Mag is dead\r\n");
acloitre 0:5651731cfb32 758 return 0;
acloitre 0:5651731cfb32 759 }
acloitre 0:5651731cfb32 760 else{
chartersauce25 1:703ba8e8b2cd 761 //printf("No problem to report.\r\n");
acloitre 0:5651731cfb32 762 return 1;
acloitre 0:5651731cfb32 763 }
acloitre 0:5651731cfb32 764 }
acloitre 0:5651731cfb32 765 else{
acloitre 0:5651731cfb32 766 WhatCommError(address, UM6_STATUS, PT);
acloitre 0:5651731cfb32 767 return 0;
acloitre 0:5651731cfb32 768 }
acloitre 0:5651731cfb32 769
acloitre 0:5651731cfb32 770 }
acloitre 0:5651731cfb32 771
acloitre 0:5651731cfb32 772 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 773
acloitre 0:5651731cfb32 774 int UM6LT::zeroGyros(int& gyroBiasX, int& gyroBiasY, int& gyroBiasZ){
acloitre 0:5651731cfb32 775
acloitre 0:5651731cfb32 776 int PT = -1;
acloitre 0:5651731cfb32 777 int N = -1;
acloitre 0:5651731cfb32 778 int address = -1;
acloitre 0:5651731cfb32 779 int data [8] = {0, 0, 0, 0, 0, 0, 0, 0};
acloitre 0:5651731cfb32 780
acloitre 0:5651731cfb32 781 int okToMoveOn = 0;
acloitre 0:5651731cfb32 782 int count = 0;
acloitre 0:5651731cfb32 783
acloitre 0:5651731cfb32 784 printf("Do not move the imu: gyro zeroing in progress.\r\n");
acloitre 0:5651731cfb32 785
acloitre 0:5651731cfb32 786 while(!okToMoveOn && count<5){
acloitre 0:5651731cfb32 787 requestData(UM6_ZERO_GYROS, 0);
acloitre 0:5651731cfb32 788 wait_ms(stdDelayMs);
acloitre 0:5651731cfb32 789 oneWordRead(PT, N, address, data);
acloitre 0:5651731cfb32 790 okToMoveOn = noCommError(address, UM6_ZERO_GYROS, PT);
acloitre 0:5651731cfb32 791 count++;
acloitre 0:5651731cfb32 792 }
acloitre 0:5651731cfb32 793 if(okToMoveOn){
acloitre 0:5651731cfb32 794 okToMoveOn = 0;
acloitre 0:5651731cfb32 795 count = 0;
acloitre 0:5651731cfb32 796 PT = -1;
acloitre 0:5651731cfb32 797 N = -1;
acloitre 0:5651731cfb32 798 address = -1;
acloitre 0:5651731cfb32 799 wait(1);
acloitre 0:5651731cfb32 800 while(!okToMoveOn && count<5){
acloitre 0:5651731cfb32 801 wait_ms(stdDelayMs);
acloitre 0:5651731cfb32 802 oneWordRead(PT, N, address, data);
acloitre 0:5651731cfb32 803 okToMoveOn = noCommError(address, UM6_GYRO_BIAS_XY, PT);
acloitre 0:5651731cfb32 804 count++;
acloitre 0:5651731cfb32 805 }
acloitre 0:5651731cfb32 806
acloitre 0:5651731cfb32 807 if(okToMoveOn){
acloitre 0:5651731cfb32 808 gyroBiasX = twosComplement(data[0], data[1]);
acloitre 0:5651731cfb32 809 gyroBiasY = twosComplement(data[2], data[3]);
acloitre 0:5651731cfb32 810 gyroBiasZ = twosComplement(data[4], data[5]);
acloitre 0:5651731cfb32 811 printf("Gyro zeroing completed.\r\n");
acloitre 0:5651731cfb32 812 return 1;
acloitre 0:5651731cfb32 813 }
acloitre 0:5651731cfb32 814 else{
acloitre 0:5651731cfb32 815 WhatCommError(address, UM6_GYRO_BIAS_XY, PT);
acloitre 0:5651731cfb32 816 printf("Could not acquire the values of gyro biases.\r\n");
acloitre 0:5651731cfb32 817 return 0;
acloitre 0:5651731cfb32 818 }
acloitre 0:5651731cfb32 819 }
acloitre 0:5651731cfb32 820 else{
acloitre 0:5651731cfb32 821 WhatCommError(address, UM6_ZERO_GYROS, PT);
acloitre 0:5651731cfb32 822 printf("Could not trigger gyro zeroing script.\r\n");
acloitre 0:5651731cfb32 823 return 0;
acloitre 0:5651731cfb32 824 }
acloitre 0:5651731cfb32 825
acloitre 0:5651731cfb32 826 }
acloitre 0:5651731cfb32 827
acloitre 0:5651731cfb32 828 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 829
acloitre 0:5651731cfb32 830 int UM6LT::autoSetAccelRef(void){
acloitre 0:5651731cfb32 831
acloitre 0:5651731cfb32 832 int address = UM6_SET_ACCEL_REF;
acloitre 0:5651731cfb32 833
acloitre 0:5651731cfb32 834 if(sendOneCommand(address)){
acloitre 0:5651731cfb32 835 return 1;
acloitre 0:5651731cfb32 836 }
acloitre 0:5651731cfb32 837 else{
acloitre 0:5651731cfb32 838 return 0;
acloitre 0:5651731cfb32 839 }
acloitre 0:5651731cfb32 840
acloitre 0:5651731cfb32 841 }
acloitre 0:5651731cfb32 842
acloitre 0:5651731cfb32 843 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 844
acloitre 0:5651731cfb32 845 int UM6LT::autoSetMagRef(void){
acloitre 0:5651731cfb32 846
acloitre 0:5651731cfb32 847 int address = UM6_SET_MAG_REF;
acloitre 0:5651731cfb32 848
acloitre 0:5651731cfb32 849 if(sendOneCommand(address)){
acloitre 0:5651731cfb32 850 return 1;
acloitre 0:5651731cfb32 851 }
acloitre 0:5651731cfb32 852 else{
acloitre 0:5651731cfb32 853 return 0;
acloitre 0:5651731cfb32 854 }
acloitre 0:5651731cfb32 855 }
acloitre 0:5651731cfb32 856
acloitre 0:5651731cfb32 857 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 858
acloitre 0:5651731cfb32 859 int UM6LT::resetEKF(void){
acloitre 0:5651731cfb32 860
acloitre 0:5651731cfb32 861 int address = UM6_RESET_EKF;
acloitre 0:5651731cfb32 862
acloitre 0:5651731cfb32 863 if(sendOneCommand(address)){
acloitre 0:5651731cfb32 864 return 1;
acloitre 0:5651731cfb32 865 }
acloitre 0:5651731cfb32 866 else{
acloitre 0:5651731cfb32 867 return 0;
acloitre 0:5651731cfb32 868 }
acloitre 0:5651731cfb32 869 }
acloitre 0:5651731cfb32 870
acloitre 0:5651731cfb32 871 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 872
acloitre 0:5651731cfb32 873 int UM6LT::writeToFlash(void){
acloitre 0:5651731cfb32 874
acloitre 0:5651731cfb32 875 int address = UM6_FLASH_COMMIT;
acloitre 0:5651731cfb32 876
acloitre 0:5651731cfb32 877 if(sendOneCommand(address)){
acloitre 0:5651731cfb32 878 return 1;
acloitre 0:5651731cfb32 879 }
acloitre 0:5651731cfb32 880 else{
acloitre 0:5651731cfb32 881 return 0;
acloitre 0:5651731cfb32 882 }
acloitre 0:5651731cfb32 883 }
acloitre 0:5651731cfb32 884
acloitre 0:5651731cfb32 885 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 886
acloitre 0:5651731cfb32 887 int UM6LT::resetToFactory(void){
acloitre 0:5651731cfb32 888
acloitre 0:5651731cfb32 889 int address = UM6_RESET_TO_FACTORY;
acloitre 0:5651731cfb32 890
acloitre 0:5651731cfb32 891 if(sendOneCommand(address)){
acloitre 0:5651731cfb32 892 return 1;
acloitre 0:5651731cfb32 893 }
acloitre 0:5651731cfb32 894 else{
acloitre 0:5651731cfb32 895 return 0;
acloitre 0:5651731cfb32 896 }
acloitre 0:5651731cfb32 897
acloitre 0:5651731cfb32 898 }
acloitre 0:5651731cfb32 899
acloitre 0:5651731cfb32 900 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 901
acloitre 0:5651731cfb32 902 int UM6LT::getAngles(int& roll, int& pitch, int& yaw){
chartersauce25 1:703ba8e8b2cd 903 /*MAJOR UPDATE:
chartersauce25 1:703ba8e8b2cd 904 - getTwoBatches(...) updated
chartersauce25 1:703ba8e8b2cd 905 */
acloitre 0:5651731cfb32 906 roll = 0;
acloitre 0:5651731cfb32 907 pitch= 0;
acloitre 0:5651731cfb32 908 yaw= 0;
chartersauce25 1:703ba8e8b2cd 909 int data_1[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 910 int data_2[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 911 int address_phi_theta = UM6_EULER_PHI_THETA;
chartersauce25 1:703ba8e8b2cd 912 int address_psi = UM6_EULER_PSI;
acloitre 0:5651731cfb32 913
chartersauce25 1:703ba8e8b2cd 914 if(getTwoBatches(address_phi_theta, data_1) && getTwoBatches(address_psi, data_2)){
chartersauce25 1:703ba8e8b2cd 915 roll = data_1[0];
chartersauce25 1:703ba8e8b2cd 916 pitch = data_1[1];
chartersauce25 1:703ba8e8b2cd 917 yaw = data_2[0];
acloitre 0:5651731cfb32 918 return 1;
acloitre 0:5651731cfb32 919 }
acloitre 0:5651731cfb32 920 else{
acloitre 0:5651731cfb32 921 return 0;
acloitre 0:5651731cfb32 922 }
acloitre 0:5651731cfb32 923
acloitre 0:5651731cfb32 924 }
acloitre 0:5651731cfb32 925
acloitre 0:5651731cfb32 926 //----------------------------------------------------------------------------------------------
chartersauce25 1:703ba8e8b2cd 927 /*
chartersauce25 1:703ba8e8b2cd 928 SUPER NEW!!!
chartersauce25 1:703ba8e8b2cd 929
chartersauce25 1:703ba8e8b2cd 930 MAJOR UPDATE:
chartersauce25 1:703ba8e8b2cd 931 - getTwoBatches(...) updated
chartersauce25 1:703ba8e8b2cd 932 */
chartersauce25 1:703ba8e8b2cd 933
chartersauce25 1:703ba8e8b2cd 934 int UM6LT::getPitchAngle(){
chartersauce25 1:703ba8e8b2cd 935
chartersauce25 1:703ba8e8b2cd 936 int pitch = 0;
chartersauce25 1:703ba8e8b2cd 937 int data[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 938 int address_phi_theta = UM6_EULER_PHI_THETA;
chartersauce25 1:703ba8e8b2cd 939
chartersauce25 1:703ba8e8b2cd 940 if(getTwoBatches(address_phi_theta, data)){
chartersauce25 1:703ba8e8b2cd 941 return pitch = data[1];
chartersauce25 1:703ba8e8b2cd 942 }
chartersauce25 1:703ba8e8b2cd 943 else{
chartersauce25 1:703ba8e8b2cd 944 return 0;
chartersauce25 1:703ba8e8b2cd 945 }
chartersauce25 1:703ba8e8b2cd 946
chartersauce25 1:703ba8e8b2cd 947 }
chartersauce25 1:703ba8e8b2cd 948 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 949
acloitre 0:5651731cfb32 950
acloitre 0:5651731cfb32 951 int UM6LT::getAccel(int& accelX, int& accelY, int& accelZ){
chartersauce25 1:703ba8e8b2cd 952 /*MAJOR UPDATE:
chartersauce25 1:703ba8e8b2cd 953 - getTwoBatches(...) updated
chartersauce25 1:703ba8e8b2cd 954 */
acloitre 0:5651731cfb32 955 accelX = 0;
acloitre 0:5651731cfb32 956 accelY = 0;
acloitre 0:5651731cfb32 957 accelZ = 0;
chartersauce25 1:703ba8e8b2cd 958 int data_1[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 959 int data_2[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 960 int address_XY = UM6_ACCEL_PROC_XY;
chartersauce25 1:703ba8e8b2cd 961 int address_Z = UM6_ACCEL_PROC_Z;
acloitre 0:5651731cfb32 962
chartersauce25 1:703ba8e8b2cd 963 if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
chartersauce25 1:703ba8e8b2cd 964 accelX = data_1[0];
chartersauce25 1:703ba8e8b2cd 965 accelY = data_1[1];
chartersauce25 1:703ba8e8b2cd 966 accelZ = data_2[0];
acloitre 0:5651731cfb32 967 return 1;
acloitre 0:5651731cfb32 968 }
acloitre 0:5651731cfb32 969 else{
acloitre 0:5651731cfb32 970 return 0;
acloitre 0:5651731cfb32 971 }
acloitre 0:5651731cfb32 972
acloitre 0:5651731cfb32 973 }
acloitre 0:5651731cfb32 974
acloitre 0:5651731cfb32 975 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 976
acloitre 0:5651731cfb32 977
acloitre 0:5651731cfb32 978 int UM6LT::getMag(int& magX, int& magY, int& magZ){
chartersauce25 1:703ba8e8b2cd 979 /*MAJOR UPDATE:
chartersauce25 1:703ba8e8b2cd 980 - getTwoBatches(...) updated
chartersauce25 1:703ba8e8b2cd 981 */
acloitre 0:5651731cfb32 982 magX = 0;
acloitre 0:5651731cfb32 983 magY = 0;
acloitre 0:5651731cfb32 984 magZ = 0;
chartersauce25 1:703ba8e8b2cd 985 int data_1[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 986 int data_2[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 987 int address_XY = UM6_MAG_PROC_XY;
chartersauce25 1:703ba8e8b2cd 988 int address_Z = UM6_MAG_PROC_Z;
acloitre 0:5651731cfb32 989
chartersauce25 1:703ba8e8b2cd 990 if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
chartersauce25 1:703ba8e8b2cd 991 magX = data_1[0];
chartersauce25 1:703ba8e8b2cd 992 magY = data_1[1];
chartersauce25 1:703ba8e8b2cd 993 magZ = data_2[0];
acloitre 0:5651731cfb32 994 return 1;
acloitre 0:5651731cfb32 995 }
acloitre 0:5651731cfb32 996 else{
acloitre 0:5651731cfb32 997 return 0;
acloitre 0:5651731cfb32 998 }
acloitre 0:5651731cfb32 999
acloitre 0:5651731cfb32 1000 }
acloitre 0:5651731cfb32 1001
acloitre 0:5651731cfb32 1002 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 1003
acloitre 0:5651731cfb32 1004
acloitre 0:5651731cfb32 1005 int UM6LT::getAngleRates(int& rollRate, int& pitchRate, int& yawRate){
chartersauce25 1:703ba8e8b2cd 1006 /*MAJOR UPDATE:
chartersauce25 1:703ba8e8b2cd 1007 - getTwoBatches(...) updated
chartersauce25 1:703ba8e8b2cd 1008 */
acloitre 0:5651731cfb32 1009 rollRate = 0;
acloitre 0:5651731cfb32 1010 pitchRate = 0;
acloitre 0:5651731cfb32 1011 yawRate = 0;
chartersauce25 1:703ba8e8b2cd 1012 int data_1[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 1013 int data_2[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 1014 int address_XY = UM6_GYRO_PROC_XY;
chartersauce25 1:703ba8e8b2cd 1015 int address_Z = UM6_GYRO_PROC_Z;
acloitre 0:5651731cfb32 1016
chartersauce25 1:703ba8e8b2cd 1017 if(getTwoBatches(address_XY, data_1) && getTwoBatches(address_Z, data_2)){
chartersauce25 1:703ba8e8b2cd 1018 rollRate = data_1[0];
chartersauce25 1:703ba8e8b2cd 1019 pitchRate = data_1[1];
chartersauce25 1:703ba8e8b2cd 1020 yawRate = data_2[0];
acloitre 0:5651731cfb32 1021 return 1;
acloitre 0:5651731cfb32 1022 }
acloitre 0:5651731cfb32 1023 else{
acloitre 0:5651731cfb32 1024 return 0;
acloitre 0:5651731cfb32 1025 }
acloitre 0:5651731cfb32 1026
acloitre 0:5651731cfb32 1027 }
acloitre 0:5651731cfb32 1028
acloitre 0:5651731cfb32 1029 //----------------------------------------------------------------------------------------------
acloitre 0:5651731cfb32 1030
acloitre 0:5651731cfb32 1031 int UM6LT::getQuaternion(int& a, int& b, int& c, int& d){
chartersauce25 1:703ba8e8b2cd 1032 /*MAJOR UPDATE:
chartersauce25 1:703ba8e8b2cd 1033 - getTwoBatches(...) updated
chartersauce25 1:703ba8e8b2cd 1034 */
acloitre 0:5651731cfb32 1035 a = 0;
acloitre 0:5651731cfb32 1036 b = 0;
acloitre 0:5651731cfb32 1037 c = 0;
acloitre 0:5651731cfb32 1038 d = 0;
chartersauce25 1:703ba8e8b2cd 1039 int data_1[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 1040 int data_2[4] = {0, 0, 0, 0};
chartersauce25 1:703ba8e8b2cd 1041 int address_AB = UM6_QUAT_AB;
chartersauce25 1:703ba8e8b2cd 1042 int address_CD = UM6_QUAT_CD;
acloitre 0:5651731cfb32 1043
chartersauce25 1:703ba8e8b2cd 1044 if(getTwoBatches(address_AB, data_1) && getTwoBatches(address_CD, data_2)){
chartersauce25 1:703ba8e8b2cd 1045 a = data_1[0];
chartersauce25 1:703ba8e8b2cd 1046 b = data_1[1];
chartersauce25 1:703ba8e8b2cd 1047 c = data_2[0];
chartersauce25 1:703ba8e8b2cd 1048 d = data_2[1];
acloitre 0:5651731cfb32 1049 return 1;
acloitre 0:5651731cfb32 1050 }
acloitre 0:5651731cfb32 1051 else{
acloitre 0:5651731cfb32 1052 return 0;
acloitre 0:5651731cfb32 1053 }
acloitre 0:5651731cfb32 1054
acloitre 0:5651731cfb32 1055 }
acloitre 0:5651731cfb32 1056
acloitre 0:5651731cfb32 1057 //----------------------------------------------------------------------------------------------