Gijs Bruining / Epson_IMU

A library to read from an Epson IMU

Dependents:   Test2

IMU.cpp@0:b4479b51578c, 2013-11-28 (annotated)

Committer:
GijsB
Date:
Thu Nov 28 15:14:40 2013 +0000
Revision:
0:b4479b51578c
Child:
1:a2b5b17c949d
eerste;

Who changed what in which revision?

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