File content as of revision 1:a2b5b17c949d:

#include "USBIMU.h"

USBIMU::USBIMU(){
    // TODO
    USBHostSerial *s = new USBHostSerial();
    init(s);
}

void USBIMU::init(USBHostSerial *s){
    // TODO
    
    while (!s->connect()){
        wait(50);
    }
    
    faulty = false;
    s_ = s;
    wait(0.8);
    while(!isReady()){
        wait(0.1);
    }
    faulty = isFaulty();
}

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

signed int USBIMU::getParam(unsigned char par){
    // TODO
    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 USBIMU::getScaledParam(unsigned char par){
    // TODO
    float val =(float) getParam(par);
    
    if(val>=0){
        return val/2147483647;
    } else{
        return val/2147483648;
    }

}

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

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

bool USBIMU::isFaulty(){
    // TODO
    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;
    
    
}

bool USBIMU::getFaulty(){
    return faulty;
}