Gijs Bruining / Epson_IMU

A library to read from an Epson IMU

Dependents:   Test2

USBIMU.cpp@1:a2b5b17c949d, 2013-12-13 (annotated)

Committer:
GijsB
Date:
Fri Dec 13 10:58:02 2013 +0000
Revision:
1:a2b5b17c949d
rsse

Who changed what in which revision?

UserRevisionLine numberNew contents of line
GijsB 1:a2b5b17c949d 1 #include "USBIMU.h"
GijsB 1:a2b5b17c949d 2
GijsB 1:a2b5b17c949d 3 USBIMU::USBIMU(){
GijsB 1:a2b5b17c949d 4 // TODO
GijsB 1:a2b5b17c949d 5 USBHostSerial *s = new USBHostSerial();
GijsB 1:a2b5b17c949d 6 init(s);
GijsB 1:a2b5b17c949d 7 }
GijsB 1:a2b5b17c949d 8
GijsB 1:a2b5b17c949d 9 void USBIMU::init(USBHostSerial *s){
GijsB 1:a2b5b17c949d 10 // TODO
GijsB 1:a2b5b17c949d 11
GijsB 1:a2b5b17c949d 12 while (!s->connect()){
GijsB 1:a2b5b17c949d 13 wait(50);
GijsB 1:a2b5b17c949d 14 }
GijsB 1:a2b5b17c949d 15
GijsB 1:a2b5b17c949d 16 faulty = false;
GijsB 1:a2b5b17c949d 17 s_ = s;
GijsB 1:a2b5b17c949d 18 wait(0.8);
GijsB 1:a2b5b17c949d 19 while(!isReady()){
GijsB 1:a2b5b17c949d 20 wait(0.1);
GijsB 1:a2b5b17c949d 21 }
GijsB 1:a2b5b17c949d 22 faulty = isFaulty();
GijsB 1:a2b5b17c949d 23 }
GijsB 1:a2b5b17c949d 24
GijsB 1:a2b5b17c949d 25 USBIMU::~USBIMU(){
GijsB 1:a2b5b17c949d 26 delete s_;
GijsB 1:a2b5b17c949d 27 }
GijsB 1:a2b5b17c949d 28
GijsB 1:a2b5b17c949d 29 signed int USBIMU::getParam(unsigned char par){
GijsB 1:a2b5b17c949d 30 // TODO
GijsB 1:a2b5b17c949d 31 if(faulty){
GijsB 1:a2b5b17c949d 32 return 0;
GijsB 1:a2b5b17c949d 33 }
GijsB 1:a2b5b17c949d 34
GijsB 1:a2b5b17c949d 35 goToStartOfSentence(par);
GijsB 1:a2b5b17c949d 36
GijsB 1:a2b5b17c949d 37 unsigned char MSByte = s_->getc(); /*The most significant byte */
GijsB 1:a2b5b17c949d 38 unsigned char LSByte = s_->getc(); /*The least significant byte */
GijsB 1:a2b5b17c949d 39 s_->getc(); /*The CR */
GijsB 1:a2b5b17c949d 40
GijsB 1:a2b5b17c949d 41 signed int res = (MSByte<<8) & LSByte;
GijsB 1:a2b5b17c949d 42 return res;
GijsB 1:a2b5b17c949d 43 }
GijsB 1:a2b5b17c949d 44
GijsB 1:a2b5b17c949d 45
GijsB 1:a2b5b17c949d 46
GijsB 1:a2b5b17c949d 47 float USBIMU::getScaledParam(unsigned char par){
GijsB 1:a2b5b17c949d 48 // TODO
GijsB 1:a2b5b17c949d 49 float val =(float) getParam(par);
GijsB 1:a2b5b17c949d 50
GijsB 1:a2b5b17c949d 51 if(val>=0){
GijsB 1:a2b5b17c949d 52 return val/2147483647;
GijsB 1:a2b5b17c949d 53 } else{
GijsB 1:a2b5b17c949d 54 return val/2147483648;
GijsB 1:a2b5b17c949d 55 }
GijsB 1:a2b5b17c949d 56
GijsB 1:a2b5b17c949d 57 }
GijsB 1:a2b5b17c949d 58
GijsB 1:a2b5b17c949d 59 void USBIMU::goToStartOfSentence(unsigned char par){
GijsB 1:a2b5b17c949d 60 // TODO
GijsB 1:a2b5b17c949d 61 s_->putc(par);
GijsB 1:a2b5b17c949d 62 s_->putc(0x00);
GijsB 1:a2b5b17c949d 63 s_->putc(CR);
GijsB 1:a2b5b17c949d 64
GijsB 1:a2b5b17c949d 65 while(s_->getc()!=par);
GijsB 1:a2b5b17c949d 66 }
GijsB 1:a2b5b17c949d 67
GijsB 1:a2b5b17c949d 68 bool USBIMU::isReady(){
GijsB 1:a2b5b17c949d 69 // TODO
GijsB 1:a2b5b17c949d 70 unsigned int param = getParam(0x3E);
GijsB 1:a2b5b17c949d 71 return (param & 0x0400) == 0;
GijsB 1:a2b5b17c949d 72 }
GijsB 1:a2b5b17c949d 73
GijsB 1:a2b5b17c949d 74 bool USBIMU::isFaulty(){
GijsB 1:a2b5b17c949d 75 // TODO
GijsB 1:a2b5b17c949d 76 goToStartOfSentence(0x3C);
GijsB 1:a2b5b17c949d 77
GijsB 1:a2b5b17c949d 78 s_->getc(); /*The most significant byte is nog interesting here*/
GijsB 1:a2b5b17c949d 79 unsigned char LSByte = s_->getc(); /*The least significant byte */
GijsB 1:a2b5b17c949d 80 s_->getc(); /*The CR */
GijsB 1:a2b5b17c949d 81
GijsB 1:a2b5b17c949d 82 return (LSByte & 0x60) != 0;
GijsB 1:a2b5b17c949d 83
GijsB 1:a2b5b17c949d 84
GijsB 1:a2b5b17c949d 85 }
GijsB 1:a2b5b17c949d 86
GijsB 1:a2b5b17c949d 87 bool USBIMU::getFaulty(){
GijsB 1:a2b5b17c949d 88 return faulty;
GijsB 1:a2b5b17c949d 89 }