quadcopter cufe
follow wall 1
Diff: SpaceSensor.cpp
- Revision:
- 0:b703833f6795
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/SpaceSensor.cpp Tue Apr 08 13:23:06 2014 +0000
@@ -0,0 +1,741 @@
+
+#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);
+}