File content as of revision 1:a2b5b17c949d:

#include "IMU.h"


IMU::IMU(PinName tx, PinName rx){
    Serial *s = new Serial(tx, rx);
    init(s);
}

void IMU::init(Serial *s){
    faulty = false;
    s_ = s;
    s_->baud(BAUDRATE);
    wait(0.8);
    while(!isReady()){
        wait(0.1);
    }
    faulty = isFaulty();
}

IMU::~IMU(){
    delete s_;
}

signed int IMU::getParam(unsigned char par){
    if(faulty){
        return 0;
    }
    
    goToStartOfSentence(par);
    
    unsigned char MSByte = s_->getc(); /*The most significant byte */
    unsigned char LSByte = s_->getc(); /*The least significant byte */
    s_->getc();                        /*The CR */
    
    signed int res = (MSByte<<8) & LSByte;
    return res;
}



float IMU::getScaledParam(unsigned char par){
    float val =(float) getParam(par);
    
    if(val>=0){
        return val/2147483647;
    } else{
        return val/2147483648;
    }

}

void IMU::goToStartOfSentence(unsigned char par){
    s_->putc(par);
    s_->putc(0x00);
    s_->putc(CR);
    
    while(s_->getc()!=par);
}

bool IMU::isReady(){
    unsigned int param = getParam(0x3E);
    return (param & 0x0400) == 0;
}

bool IMU::isFaulty(){
    goToStartOfSentence(0x3C);
    
    s_->getc();                         /*The most significant byte is nog interesting here*/
    unsigned char LSByte = s_->getc(); /*The least significant byte */
    s_->getc();                        /*The CR */
    
    return (LSByte & 0x60) != 0;
    
    
}