Gijs Bruining / Epson_IMU

A library to read from an Epson IMU

Dependents:   Test2

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

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

Who changed what in which revision?

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