File content as of revision 0:b703833f6795:

#include "mbed.h"
#include "SpaceSensor.h"
SpaceSensor::SpaceSensor(PinName p_tx, PinName p_rx): _SpaceSensor(p_tx, p_rx) {
    begin(115200);
    st = new char[128];
    vecf= new float[15];
}
/**********************************************************************************************************************/    
/*4.3.1 Commands for Reading Filtered Sensor Data*/
/**********************************************************************************************************************/    
void SpaceSensor::ReadfilteredtaredQuaternion(float *Quaternion){
    _SpaceSensor.puts(":0\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Quaternion,st);
    st[0]='\n';
}
void SpaceSensor::ReadfilteredtaredEulerAngles(float *EulerAngles){
    _SpaceSensor.puts(":1\n");
    _SpaceSensor.scanf("%s", st);
    read_string(EulerAngles,st);
    st[0]='\n';
}
void SpaceSensor::ReadfilteredtaredRotationMatrix(float *RotationMatrix){
    _SpaceSensor.puts(":2\n");
    _SpaceSensor.scanf("%s", st);
    read_string(RotationMatrix,st);
    st[0]='\n';

}
void SpaceSensor::ReadfilteredtaredAxisAngle(float *Axis,float *Angle){
    _SpaceSensor.puts(":3\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Axis[i]=vecf[i];}
    Angle[0]=vecf[3];
    st[0]='\n';
    
}
void SpaceSensor::ReadfilteredtaredTwoVector(float *Forward,float *Down){
    _SpaceSensor.puts(":4\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Forward[i]=vecf[i];}
    for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
    st[0]='\n';
}
void SpaceSensor::Readfilteredgyrorates(float *Quaternion){
    _SpaceSensor.puts(":5\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Quaternion,st);
    st[0]='\n';
}
void SpaceSensor::ReadfiltereduntaredQuaternion(float *Quaternion){
    _SpaceSensor.puts(":6\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Quaternion,st);
    st[0]='\n';
}
void SpaceSensor::ReadfiltereduntaredEulerAngles(float *EulerAngles){
    _SpaceSensor.puts(":7\n");
    _SpaceSensor.scanf("%s", st);
    read_string(EulerAngles,st);
    st[0]='\n';
}
void SpaceSensor::ReadfiltereduntaredRotationMatrix(float *RotationMatrix){
    _SpaceSensor.puts(":8\n");
    _SpaceSensor.scanf("%s", st);
    read_string(RotationMatrix,st);
    st[0]='\n';
}
void SpaceSensor::ReadfiltereduntaredAxisAngle(float *Axis,float *Angle){
    _SpaceSensor.puts(":9\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Axis[i]=vecf[i];}
    Angle[0]=vecf[3];
    st[0]='\n';
}
void SpaceSensor::ReadfiltereduntaredTwoVector(float *Forward,float *Down){
    _SpaceSensor.puts(":10\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Forward[i]=vecf[i];}
    for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
    st[0]='\n';    
}
void SpaceSensor::ReadfilteredtaredForwardandDownVectors(float *Forward,float *Down){
    _SpaceSensor.puts(":11\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Forward[i]=vecf[i];}
    for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
    st[0]='\n';    
}
void SpaceSensor::ReadfilteredNorthEarthVectors(float *North,float *Earth){
    _SpaceSensor.puts(":12\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){North[i]=vecf[i];}
    for(int i=0;i<3;i++){Earth[i]=vecf[i+3];}
    st[0]='\n';    
}

/**********************************************************************************************************************/    
    /*4.3.2 Commands for Interfacing with Electronic Systems*/
/**********************************************************************************************************************/    
void SpaceSensor::Setinterrupttype(int mode){
    sprintf( st ,":29,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::Readinterrupttype(int *interrupttype){
    _SpaceSensor.puts(":30\n");
    read_string(vecf,st);
    interrupttype[0]=(int)vecf[0];
    interrupttype[1]=(int)vecf[1];
}
void SpaceSensor::Readinterruptstatus(int *interruptstatus){
    _SpaceSensor.puts(":31\n");
    read_string(vecf,st);
    interruptstatus[0]=(int)vecf[0];
}
/**********************************************************************************************************************/    
    /*4.3.3 Commands for Reading Normalized Sensor Data*/
/**********************************************************************************************************************/    
void SpaceSensor::Readall(float *gyro,float *accelerometer,float *compass){
    _SpaceSensor.puts(":32\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){gyro[i]=vecf[i];}
    for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
    for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
    st[0]='\n';    
}
void SpaceSensor::Readgyros(float *gyro){
    _SpaceSensor.puts(":33\n");
    _SpaceSensor.scanf("%s", st);
    read_string(gyro,st);
    st[0]='\n';    
}
void SpaceSensor::Readaccelerometer(float *accelerometer){
    _SpaceSensor.puts(":34\n");
    _SpaceSensor.scanf("%s", st);
    read_string(accelerometer,st);
    st[0]='\n';    
}
void SpaceSensor::Readcompass(float *compass){
    _SpaceSensor.puts(":35\n");
    _SpaceSensor.scanf("%s", st);
    read_string(compass,st);
    st[0]='\n';    
}
void SpaceSensor::ReadtemperatureC(float *temp){
    _SpaceSensor.puts(":36\n");
    _SpaceSensor.scanf("%s", st);
    read_string(temp,st);
    st[0]='\n';    
}
void SpaceSensor::ReadtemperatureF(float *temp){
    _SpaceSensor.puts(":37\n");
    _SpaceSensor.scanf("%s", st);
    read_string(temp,st);
    st[0]='\n';    
}
void SpaceSensor::Readconfidencefactor(float *confidencefactor){
    _SpaceSensor.puts(":38\n");
    _SpaceSensor.scanf("%s", st);
    read_string(confidencefactor,st);
    st[0]='\n';    
}
void SpaceSensor::Readaccelerometerunnormalized(float *accelunnormalized){
    _SpaceSensor.puts(":39\n");
    _SpaceSensor.scanf("%s", st);
    read_string(accelunnormalized,st);
    st[0]='\n';    
}
void SpaceSensor::Readcompassunnormalized(float *compassunnormalized){
    _SpaceSensor.puts(":30\n");
    _SpaceSensor.scanf("%s", st);
    read_string(compassunnormalized,st);
    st[0]='\n';    
}
/**********************************************************************************************************************/    
    /*4.3.4 Commands for Reading Raw Sensor Data*/
/**********************************************************************************************************************/    
void SpaceSensor::Readallraw(float *gyro,float *accelerometer,float *compass){
    _SpaceSensor.puts(":64\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){gyro[i]=vecf[i];}
    for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
    for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
    st[0]='\n';    

}
void SpaceSensor::Readgyroraw(float *gyro){
    _SpaceSensor.puts(":65\n");
    _SpaceSensor.scanf("%s", st);
    read_string(gyro,st);
    st[0]='\n';    
}
void SpaceSensor::Readaccelerometerraw(float *accelerometer){
    _SpaceSensor.puts(":66\n");
    _SpaceSensor.scanf("%s", st);
    read_string(accelerometer,st);
    st[0]='\n';    
}
void SpaceSensor::Readcompassraw(float *compass){
    _SpaceSensor.puts(":67\n");
    _SpaceSensor.scanf("%s", st);
    read_string(compass,st);
    st[0]='\n';    
}
/**********************************************************************************************************************/    
    /*4.3.5 Commands for Setting Filter Parameters*/
/**********************************************************************************************************************/    
void SpaceSensor::Tarewithcurrentorientation(){
    _SpaceSensor.puts(":96\n");
}
void SpaceSensor::Tarewithquaternion(float Quaternion[4]){
    sprintf( st ,":97,%f,%f,%f,%f\n",Quaternion[0],Quaternion[1],Quaternion[2],Quaternion[3]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::Tarewithrotationmatrix(float RotationMatrix[9]){
    sprintf( st ,":98,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",RotationMatrix[0],RotationMatrix[1],RotationMatrix[2],RotationMatrix[3],RotationMatrix[4],RotationMatrix[5],RotationMatrix[6],RotationMatrix[7],RotationMatrix[8]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetStaticRhoModeAccelerometer(float rho){
    sprintf( st ,":99,%f\n",rho);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetConfidenceRhoModeAccelerometer(float min,float max){
    sprintf( st ,":100,%f,%f\n",min,max);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetStaticRhoModeCompass(float rho){
    sprintf( st ,":101,%f\n",rho);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetConfidenceRhoModeCompass(float min, float max){
    sprintf( st ,":102,%f,%f\n",min,max);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetDesiredUpdateRate(int t){
    sprintf( st ,":103,%d\n",t);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetMultiReferenceVectorsWithCurrentOrientation(){
    _SpaceSensor.puts(":l04\n");
}
void SpaceSensor::SetReferenceVectorMode(int mode){
    sprintf( st ,":105,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetOversampleRate(int rate){
    sprintf( st ,":106,%d\n",rate);
    _SpaceSensor.puts(st);
}
void SpaceSensor::EnableDisablegyros(int mode){
    sprintf( st ,":107,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::EnableDisableAccelerometer(int mode){
    sprintf( st ,":108,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::EnableDisableCompass(int mode){
    sprintf( st ,":109,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::ResetMultiReferenceVectorsToZero(){
    _SpaceSensor.puts(":110\n");
}
void SpaceSensor::SetMultiReferenceResolution(int mode){
    sprintf( st ,":111,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetCompassMultiReferenceVector(int Index,float Vector[3]){
    sprintf( st ,":112,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetCompassMultiReferenceCheckVector(int Index,float Vector[3]){
    sprintf( st ,":113,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetAccelMultiReferenceVector(int Index,float Vector[3]){
    sprintf( st ,":114,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetAccelMultiReferenceCheckVector(int Index,float Vector[3]){
    sprintf( st ,":115,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetAxisDirections(int direction){
    sprintf( st ,":116,%d\n",direction);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetRunningAveragePercent(float Percent){
    sprintf( st ,":117,%f\n",Percent);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetCompassReferenceVector(float Vector[3]){
    sprintf( st ,":118,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetAccelerometerReferenceVector(float Vector[3]){
    sprintf( st ,":119,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::ResetKalmanFilter(){
    _SpaceSensor.puts(":120\n");
}
void SpaceSensor::SetAccelerometerRange(int Accelrange){
    sprintf( st ,":121,%d\n",Accelrange);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetMultiReferenceWeightPower(float WeightPower){
    sprintf( st ,":122,%f\n",WeightPower);
    _SpaceSensor.puts(st);
}
void SpaceSensor::EnableDisableFilter(int Mode){
    sprintf( st ,":123,%d\n",Mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetRunningAverageMode(int Mode){
    sprintf( st ,":124,%d\n",Mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetGyroscopeRange(int mode){
    sprintf( st ,":125,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetCompassRange(int mode){
    sprintf( st ,":126,%d\n",mode);
    _SpaceSensor.puts(st);
}
/**********************************************************************************************************************/    
    /*4.3.6 Commands for Reading Filter Parameters*/
/**********************************************************************************************************************/    
void SpaceSensor::ReadTareOrientationQuaternion(float *Quaternion){
    _SpaceSensor.puts(":128\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Quaternion,st);
    st[0]='\n';
}
void SpaceSensor::ReadTareOrientationRotationMatrix(float *RotationMatrix){
    _SpaceSensor.puts(":129\n");
    _SpaceSensor.scanf("%s", st);
    read_string(RotationMatrix,st);
    st[0]='\n';
}
void SpaceSensor::ReadRhoDataAccelerometer(int *Rhomode,float *minroh,float *maxroh){
    _SpaceSensor.puts(":130\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Rhomode[0]=(int)vecf[0];
    minroh[0]=vecf[1];
    maxroh[0]=vecf[2];
    st[0]='\n';
}
void SpaceSensor::ReadRhoDataCompass(int *Rhomode,float *minroh,float *maxroh){
    _SpaceSensor.puts(":131\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Rhomode[0]=(int)vecf[0];
    minroh[0]=vecf[1];
    maxroh[0]=vecf[2];
    st[0]='\n';
}
void SpaceSensor::ReadCurrentUpdateRate(int *rate){
    _SpaceSensor.puts(":132\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    rate[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadCompassReferenceVector(float *Vector){
    _SpaceSensor.puts(":133\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Vector,st);
    st[0]='\n';
}
void SpaceSensor::ReadAccelerometerReferenceVector(float *Vector){
    _SpaceSensor.puts(":134\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Vector,st);
    st[0]='\n';
}
void SpaceSensor::ReadReferenceVectorMode(int *Mode){
    _SpaceSensor.puts(":135\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadCompassMultiReferenceVector(int Index,float *Vector){
    sprintf( st ,":136,%d\n",Index);
    _SpaceSensor.puts(st);
    _SpaceSensor.scanf("%s", st);
    read_string(Vector,st);
    st[0]='\n';
}
void SpaceSensor::ReadCompassMultiReferenceCheckVector(int Index,float *Vector){
    sprintf( st ,":137,%d\n",Index);
    _SpaceSensor.puts(st);
    _SpaceSensor.scanf("%s", st);
    read_string(Vector,st);
    st[0]='\n';
}
void SpaceSensor::ReadAccelMultiReferenceVector(int Index,float *Vector){
    sprintf( st ,":138,%d\n",Index);
    _SpaceSensor.puts(st);
    _SpaceSensor.scanf("%s", st);
    read_string(Vector,st);
    st[0]='\n';
}
void SpaceSensor::ReadAccelMultiReferenceCheckVector(int Index,float *Vector){
    sprintf( st ,":139,%d\n",Index);
    _SpaceSensor.puts(st);
    _SpaceSensor.scanf("%s", st);
    read_string(Vector,st);
    st[0]='\n';
}
void SpaceSensor::ReadGyroEnabledState(int *Mode){
    _SpaceSensor.puts(":140\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadAccelerometerEnabledState(int *Mode){
    _SpaceSensor.puts(":141\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadCompassEnabledState(int *Mode){
    _SpaceSensor.puts(":142\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadAxisDirections(int *direction){
    _SpaceSensor.puts(":143\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    direction[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadOversampleRate(int *Rate){
    _SpaceSensor.puts(":144\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Rate[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadRunningAveragePercent(float *Percent){
    _SpaceSensor.puts(":145\n");
    _SpaceSensor.scanf("%s", st);
    read_string(Percent,st);
    st[0]='\n';
}
void SpaceSensor::ReadDesiredUpdateRate(float *rate){
    _SpaceSensor.puts(":146\n");
    _SpaceSensor.scanf("%s", st);
    read_string(rate,st);
    st[0]='\n';
}
void SpaceSensor::ReadKalmanFilterCovarianceMatrix(float *CovarianceMatrix){
    _SpaceSensor.puts(":147\n");
    _SpaceSensor.scanf("%s", st);
    read_string(CovarianceMatrix,st);
    st[0]='\n';
}
void SpaceSensor::ReadAccelerometerRange(int *Accelrange){
    _SpaceSensor.puts(":148\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Accelrange[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadMultiReferenceWeightPower(float *WeightPower){
    _SpaceSensor.puts(":149\n");
    _SpaceSensor.scanf("%s", st);
    read_string(WeightPower,st);
    st[0]='\n';
}
void SpaceSensor::ReadMultiReferenceResolution(int *Resolution){
    _SpaceSensor.puts(":150\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Resolution[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadNumberOfmultiReferenceCells(int *NumberOfCells){
    _SpaceSensor.puts(":151\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    NumberOfCells[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadFilterEnableState(int *Mode){
    _SpaceSensor.puts(":152\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadRunningAverageMode(int *Mode){
    _SpaceSensor.puts(":153\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadGyroscopeRange(int *mode){
    _SpaceSensor.puts(":154\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    mode[0]=(int)vecf[0];
    st[0]='\n';
}
void SpaceSensor::ReadCompassRange(int *mode){
    _SpaceSensor.puts(":155\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    mode[0]=(int)vecf[0];
    st[0]='\n';
}
/**********************************************************************************************************************/    
    /*4.3.7 Commands for Calibration*/
/**********************************************************************************************************************/    
void SpaceSensor::SetCompassCalibrationParameters(float Bias[3],float Matrix[9]){
    sprintf( st ,":160,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],Matrix[0],Matrix[1],Matrix[2],Matrix[3],Matrix[4],Matrix[5],Matrix[6],Matrix[7],Matrix[8]);
    _SpaceSensor.puts(st);
    st[0]='\n';
}
void SpaceSensor::SetAccelerometerCalibrationParameters(float Bias[3],float Matrix[9]){
    sprintf( st ,":161,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],Matrix[0],Matrix[1],Matrix[2],Matrix[3],Matrix[4],Matrix[5],Matrix[6],Matrix[7],Matrix[8]);
    _SpaceSensor.puts(st);
    st[0]='\n';
}
void SpaceSensor::ReadCompassCalibrationParameters(float *Bias,float *Matrix){
    _SpaceSensor.puts(":162\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Bias[i]=vecf[i];}
    for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
    st[0]='\n';
}
void SpaceSensor::ReadAccelerometerCalibrationParameters(float *Bias,float *Matrix){
    _SpaceSensor.puts(":163\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Bias[i]=vecf[i];}
    for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
    st[0]='\n';
}
void SpaceSensor::ReadGyroCalibrationParameters(float *Bias, float *range){
    _SpaceSensor.puts(":164\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    for(int i=0;i<3;i++){Bias[i]=vecf[i];}
    for(int i=0;i<3;i++){range[i]=vecf[i+3];}
    st[0]='\n';
}
void SpaceSensor::BeginGyroAutocalibration(){
    _SpaceSensor.puts(":165\n");
}
void SpaceSensor::SetGyroCalibrationParameters(float Bias[3], float range[3]){
    sprintf( st ,":166,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],range[0],range[1],range[2]);
    _SpaceSensor.puts(st);
    st[0]='\n';
}
/**********************************************************************************************************************/    
    /*4.3.8 General Commands*/
/**********************************************************************************************************************/    
void SpaceSensor::ReadSoftwareVersion(char *version){
    _SpaceSensor.puts(":223\n");
    _SpaceSensor.scanf("%s", version);
}
void SpaceSensor::RestoreFactorySettings(){
    _SpaceSensor.puts(":224\n");
}
void SpaceSensor::CommitSettings(){
    _SpaceSensor.puts(":225\n");
}
void SpaceSensor::SoftwareReset(){
    _SpaceSensor.puts(":226\n");
}
void SpaceSensor::EnableWatchdogTimer(int timer){
    sprintf( st ,":227,%d\n",timer);
    _SpaceSensor.puts(st);
}
void SpaceSensor::DisableWatchdogTimer(){
    _SpaceSensor.puts(":228\n");
}
void SpaceSensor::EnterFirmwareUpdateMode(){
    _SpaceSensor.puts(":229\n");
}
void SpaceSensor::ReadHardwareVersion(char *version){
    _SpaceSensor.puts(":230\n");
    _SpaceSensor.scanf("%s", version);
}
void SpaceSensor::SetUARTBaudRate(int Baudrate){
    sprintf( st ,":231,%d\n",Baudrate);
    _SpaceSensor.puts(st);
}
void SpaceSensor::GetUARTBaudRate(int *Baudrate){
    _SpaceSensor.puts(":232\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Baudrate[0]=(int)vecf[0];
}
void SpaceSensor::SetUSBmode(int mode){
    sprintf( st ,":233,%d\n",mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::GetUSBmode(int *mode){
    _SpaceSensor.puts(":234\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    mode[0]=(int)vecf[0];
}
void SpaceSensor::SetClockSpeed(int clock){
    sprintf( st ,":235,%d\n",clock);
    _SpaceSensor.puts(st);
}
void SpaceSensor::GetClockSpeed(int *clock){
    _SpaceSensor.puts(":236\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    clock[0]=(int)vecf[0];
}
void SpaceSensor::GetSerialnumber(int *serialnum){
    _SpaceSensor.puts(":237\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    serialnum[0]=(int)vecf[0];
}
void SpaceSensor::SetLEDColor(float color[3]){
    sprintf( st ,":238,%f,%f,%f\n",color[0],color[1],color[2]);
    _SpaceSensor.puts(st);
}
void SpaceSensor::GetLEDColor(float *color){
    _SpaceSensor.puts(":239\n");
    _SpaceSensor.scanf("%s", st);
    read_string(color,st);
}
void SpaceSensor::EnableDisableJoystick(int Mode){
    sprintf( st ,":240,%d\n",Mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::EnableDisablemouse(int Mode){
    sprintf( st ,":241,%d\n",Mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::ReadJoystickEnabledState(int *Mode){
    _SpaceSensor.puts(":242\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
}
void SpaceSensor::ReadMouseEnabledState(int *Mode){
    _SpaceSensor.puts(":243\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
}
void SpaceSensor::SetControlMode(float Controlclass,float controlindex,float handlerindex){
    sprintf( st ,":244,%f,%f,%f\n",Controlclass,controlindex,handlerindex);
    _SpaceSensor.puts(st);
}
void SpaceSensor::SetControlData(float Controlclass,float controlindex,float datapointindex,float datapoint ){
    sprintf( st ,":245,%f,%f,%f,%f\n",Controlclass,controlindex,datapointindex,datapoint);
    _SpaceSensor.puts(st);
}  
void SpaceSensor::ReadControlMode(float *Handlerindex, float Controlclass,float controlindex){
    sprintf( st ,":246,%f,%f\n",Controlclass,controlindex);
    _SpaceSensor.puts(st);
    _SpaceSensor.scanf("%s", st);
    read_string(Handlerindex,st);
}
void SpaceSensor::ReadControlData(float *Datapoint,float ControlClass,float controlIndex,float dataPointIndex){
    sprintf( st ,":247,%f,%f,%f\n",ControlClass,controlIndex,dataPointIndex);
    _SpaceSensor.puts(st);
    _SpaceSensor.scanf("%s", st);
    read_string(Datapoint,st);
}
void SpaceSensor::SetMouseAbsoluteRelative(int Mode){
    sprintf( st ,":251,%d\n",Mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::ReadMouseAbsoluteRelative(int *Mode){
    _SpaceSensor.puts(":252\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
}
void SpaceSensor::SetjoystickAndMousePresentRemoved(int Mode){
    sprintf( st ,":253,%d\n",Mode);
    _SpaceSensor.puts(st);
}
void SpaceSensor::ReadjoystickAndMousePresentRemoved(int *Mode){
    _SpaceSensor.puts(":254\n");
    _SpaceSensor.scanf("%s", st);
    read_string(vecf,st);
    Mode[0]=(int)vecf[0];
}

/**********************************************************************************************************************/    
/*Private functions*/    
/**********************************************************************************************************************/    
void SpaceSensor::read_string(float *vector,char *c){
                    char var[10];                    
                    int var_size=0,j=0,i=0;
                    while(1){
                             
                             if(j==strlen(c)){
                                             vector[i]=atof(var);
                                             break;
                                            
                                            }
                             if(c[j]==','){
                                            vector[i]=atof(var);
                                            i++;
                                            j++;
                                            var_size=0;
                                            }
                             var[var_size]=c[j];
                             j++;
                             var_size++;
    
                             }                               
                    }
    
void SpaceSensor::begin(long baud) {
    _SpaceSensor.baud(baud);
}