SpaceSensor.cpp

00001 
00002 #include "mbed.h"
00003 #include "SpaceSensor.h"
00004 SpaceSensor::SpaceSensor(PinName p_tx, PinName p_rx): _SpaceSensor(p_tx, p_rx) {
00005     begin(115200);
00006     st = new char[128];
00007     vecf= new float[15];
00008 }
00009 /**********************************************************************************************************************/    
00010 /*4.3.1 Commands for Reading Filtered Sensor Data*/
00011 /**********************************************************************************************************************/    
00012 void SpaceSensor::ReadfilteredtaredQuaternion(float *Quaternion){
00013     _SpaceSensor.puts(":0\n");
00014     _SpaceSensor.scanf("%s", st);
00015     read_string(Quaternion,st);
00016     st[0]='\n';
00017 }
00018 void SpaceSensor::ReadfilteredtaredEulerAngles(float *EulerAngles){
00019     _SpaceSensor.puts(":1\n");
00020     _SpaceSensor.scanf("%s", st);
00021     read_string(EulerAngles,st);
00022     st[0]='\n';
00023 }
00024 void SpaceSensor::ReadfilteredtaredRotationMatrix(float *RotationMatrix){
00025     _SpaceSensor.puts(":2\n");
00026     _SpaceSensor.scanf("%s", st);
00027     read_string(RotationMatrix,st);
00028     st[0]='\n';
00029 
00030 }
00031 void SpaceSensor::ReadfilteredtaredAxisAngle(float *Axis,float *Angle){
00032     _SpaceSensor.puts(":3\n");
00033     _SpaceSensor.scanf("%s", st);
00034     read_string(vecf,st);
00035     for(int i=0;i<3;i++){Axis[i]=vecf[i];}
00036     Angle[0]=vecf[3];
00037     st[0]='\n';
00038     
00039 }
00040 void SpaceSensor::ReadfilteredtaredTwoVector(float *Forward,float *Down){
00041     _SpaceSensor.puts(":4\n");
00042     _SpaceSensor.scanf("%s", st);
00043     read_string(vecf,st);
00044     for(int i=0;i<3;i++){Forward[i]=vecf[i];}
00045     for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
00046     st[0]='\n';
00047 }
00048 void SpaceSensor::Readfilteredgyrorates(float *Quaternion){
00049     _SpaceSensor.puts(":5\n");
00050     _SpaceSensor.scanf("%s", st);
00051     read_string(Quaternion,st);
00052     st[0]='\n';
00053 }
00054 void SpaceSensor::ReadfiltereduntaredQuaternion(float *Quaternion){
00055     _SpaceSensor.puts(":6\n");
00056     _SpaceSensor.scanf("%s", st);
00057     read_string(Quaternion,st);
00058     st[0]='\n';
00059 }
00060 void SpaceSensor::ReadfiltereduntaredEulerAngles(float *EulerAngles){
00061     _SpaceSensor.puts(":7\n");
00062     _SpaceSensor.scanf("%s", st);
00063     read_string(EulerAngles,st);
00064     st[0]='\n';
00065 }
00066 void SpaceSensor::ReadfiltereduntaredRotationMatrix(float *RotationMatrix){
00067     _SpaceSensor.puts(":8\n");
00068     _SpaceSensor.scanf("%s", st);
00069     read_string(RotationMatrix,st);
00070     st[0]='\n';
00071 }
00072 void SpaceSensor::ReadfiltereduntaredAxisAngle(float *Axis,float *Angle){
00073     _SpaceSensor.puts(":9\n");
00074     _SpaceSensor.scanf("%s", st);
00075     read_string(vecf,st);
00076     for(int i=0;i<3;i++){Axis[i]=vecf[i];}
00077     Angle[0]=vecf[3];
00078     st[0]='\n';
00079 }
00080 void SpaceSensor::ReadfiltereduntaredTwoVector(float *Forward,float *Down){
00081     _SpaceSensor.puts(":10\n");
00082     _SpaceSensor.scanf("%s", st);
00083     read_string(vecf,st);
00084     for(int i=0;i<3;i++){Forward[i]=vecf[i];}
00085     for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
00086     st[0]='\n';    
00087 }
00088 void SpaceSensor::ReadfilteredtaredForwardandDownVectors(float *Forward,float *Down){
00089     _SpaceSensor.puts(":11\n");
00090     _SpaceSensor.scanf("%s", st);
00091     read_string(vecf,st);
00092     for(int i=0;i<3;i++){Forward[i]=vecf[i];}
00093     for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
00094     st[0]='\n';    
00095 }
00096 void SpaceSensor::ReadfilteredNorthEarthVectors(float *North,float *Earth){
00097     _SpaceSensor.puts(":12\n");
00098     _SpaceSensor.scanf("%s", st);
00099     read_string(vecf,st);
00100     for(int i=0;i<3;i++){North[i]=vecf[i];}
00101     for(int i=0;i<3;i++){Earth[i]=vecf[i+3];}
00102     st[0]='\n';    
00103 }
00104 
00105 /**********************************************************************************************************************/    
00106     /*4.3.2 Commands for Interfacing with Electronic Systems*/
00107 /**********************************************************************************************************************/    
00108 void SpaceSensor::Setinterrupttype(int mode){
00109     sprintf( st ,":29,%d\n",mode);
00110     _SpaceSensor.puts(st);
00111 }
00112 void SpaceSensor::Readinterrupttype(int *interrupttype){
00113     _SpaceSensor.puts(":30\n");
00114     read_string(vecf,st);
00115     interrupttype[0]=(int)vecf[0];
00116     interrupttype[1]=(int)vecf[1];
00117 }
00118 void SpaceSensor::Readinterruptstatus(int *interruptstatus){
00119     _SpaceSensor.puts(":31\n");
00120     read_string(vecf,st);
00121     interruptstatus[0]=(int)vecf[0];
00122 }
00123 /**********************************************************************************************************************/    
00124     /*4.3.3 Commands for Reading Normalized Sensor Data*/
00125 /**********************************************************************************************************************/    
00126 void SpaceSensor::Readall(float *gyro,float *accelerometer,float *compass){
00127     _SpaceSensor.puts(":32\n");
00128     _SpaceSensor.scanf("%s", st);
00129     read_string(vecf,st);
00130     for(int i=0;i<3;i++){gyro[i]=vecf[i];}
00131     for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
00132     for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
00133     st[0]='\n';    
00134 }
00135 void SpaceSensor::Readgyros(float *gyro){
00136     _SpaceSensor.puts(":33\n");
00137     _SpaceSensor.scanf("%s", st);
00138     read_string(gyro,st);
00139     st[0]='\n';    
00140 }
00141 void SpaceSensor::Readaccelerometer(float *accelerometer){
00142     _SpaceSensor.puts(":34\n");
00143     _SpaceSensor.scanf("%s", st);
00144     read_string(accelerometer,st);
00145     st[0]='\n';    
00146 }
00147 void SpaceSensor::Readcompass(float *compass){
00148     _SpaceSensor.puts(":35\n");
00149     _SpaceSensor.scanf("%s", st);
00150     read_string(compass,st);
00151     st[0]='\n';    
00152 }
00153 void SpaceSensor::ReadtemperatureC(float *temp){
00154     _SpaceSensor.puts(":36\n");
00155     _SpaceSensor.scanf("%s", st);
00156     read_string(temp,st);
00157     st[0]='\n';    
00158 }
00159 void SpaceSensor::ReadtemperatureF(float *temp){
00160     _SpaceSensor.puts(":37\n");
00161     _SpaceSensor.scanf("%s", st);
00162     read_string(temp,st);
00163     st[0]='\n';    
00164 }
00165 void SpaceSensor::Readconfidencefactor(float *confidencefactor){
00166     _SpaceSensor.puts(":38\n");
00167     _SpaceSensor.scanf("%s", st);
00168     read_string(confidencefactor,st);
00169     st[0]='\n';    
00170 }
00171 void SpaceSensor::Readaccelerometerunnormalized(float *accelunnormalized){
00172     _SpaceSensor.puts(":39\n");
00173     _SpaceSensor.scanf("%s", st);
00174     read_string(accelunnormalized,st);
00175     st[0]='\n';    
00176 }
00177 void SpaceSensor::Readcompassunnormalized(float *compassunnormalized){
00178     _SpaceSensor.puts(":30\n");
00179     _SpaceSensor.scanf("%s", st);
00180     read_string(compassunnormalized,st);
00181     st[0]='\n';    
00182 }
00183 /**********************************************************************************************************************/    
00184     /*4.3.4 Commands for Reading Raw Sensor Data*/
00185 /**********************************************************************************************************************/    
00186 void SpaceSensor::Readallraw(float *gyro,float *accelerometer,float *compass){
00187     _SpaceSensor.puts(":64\n");
00188     _SpaceSensor.scanf("%s", st);
00189     read_string(vecf,st);
00190     for(int i=0;i<3;i++){gyro[i]=vecf[i];}
00191     for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
00192     for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
00193     st[0]='\n';    
00194 
00195 }
00196 void SpaceSensor::Readgyroraw(float *gyro){
00197     _SpaceSensor.puts(":65\n");
00198     _SpaceSensor.scanf("%s", st);
00199     read_string(gyro,st);
00200     st[0]='\n';    
00201 }
00202 void SpaceSensor::Readaccelerometerraw(float *accelerometer){
00203     _SpaceSensor.puts(":66\n");
00204     _SpaceSensor.scanf("%s", st);
00205     read_string(accelerometer,st);
00206     st[0]='\n';    
00207 }
00208 void SpaceSensor::Readcompassraw(float *compass){
00209     _SpaceSensor.puts(":67\n");
00210     _SpaceSensor.scanf("%s", st);
00211     read_string(compass,st);
00212     st[0]='\n';    
00213 }
00214 /**********************************************************************************************************************/    
00215     /*4.3.5 Commands for Setting Filter Parameters*/
00216 /**********************************************************************************************************************/    
00217 void SpaceSensor::Tarewithcurrentorientation(){
00218     _SpaceSensor.puts(":96\n");
00219 }
00220 void SpaceSensor::Tarewithquaternion(float Quaternion[4]){
00221     sprintf( st ,":97,%f,%f,%f,%f\n",Quaternion[0],Quaternion[1],Quaternion[2],Quaternion[3]);
00222     _SpaceSensor.puts(st);
00223 }
00224 void SpaceSensor::Tarewithrotationmatrix(float RotationMatrix[9]){
00225     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]);
00226     _SpaceSensor.puts(st);
00227 }
00228 void SpaceSensor::SetStaticRhoModeAccelerometer(float rho){
00229     sprintf( st ,":99,%f\n",rho);
00230     _SpaceSensor.puts(st);
00231 }
00232 void SpaceSensor::SetConfidenceRhoModeAccelerometer(float min,float max){
00233     sprintf( st ,":100,%f,%f\n",min,max);
00234     _SpaceSensor.puts(st);
00235 }
00236 void SpaceSensor::SetStaticRhoModeCompass(float rho){
00237     sprintf( st ,":101,%f\n",rho);
00238     _SpaceSensor.puts(st);
00239 }
00240 void SpaceSensor::SetConfidenceRhoModeCompass(float min, float max){
00241     sprintf( st ,":102,%f,%f\n",min,max);
00242     _SpaceSensor.puts(st);
00243 }
00244 void SpaceSensor::SetDesiredUpdateRate(int t){
00245     sprintf( st ,":103,%d\n",t);
00246     _SpaceSensor.puts(st);
00247 }
00248 void SpaceSensor::SetMultiReferenceVectorsWithCurrentOrientation(){
00249     _SpaceSensor.puts(":l04\n");
00250 }
00251 void SpaceSensor::SetReferenceVectorMode(int mode){
00252     sprintf( st ,":105,%d\n",mode);
00253     _SpaceSensor.puts(st);
00254 }
00255 void SpaceSensor::SetOversampleRate(int rate){
00256     sprintf( st ,":106,%d\n",rate);
00257     _SpaceSensor.puts(st);
00258 }
00259 void SpaceSensor::EnableDisablegyros(int mode){
00260     sprintf( st ,":107,%d\n",mode);
00261     _SpaceSensor.puts(st);
00262 }
00263 void SpaceSensor::EnableDisableAccelerometer(int mode){
00264     sprintf( st ,":108,%d\n",mode);
00265     _SpaceSensor.puts(st);
00266 }
00267 void SpaceSensor::EnableDisableCompass(int mode){
00268     sprintf( st ,":109,%d\n",mode);
00269     _SpaceSensor.puts(st);
00270 }
00271 void SpaceSensor::ResetMultiReferenceVectorsToZero(){
00272     _SpaceSensor.puts(":110\n");
00273 }
00274 void SpaceSensor::SetMultiReferenceResolution(int mode){
00275     sprintf( st ,":111,%d\n",mode);
00276     _SpaceSensor.puts(st);
00277 }
00278 void SpaceSensor::SetCompassMultiReferenceVector(int Index,float Vector[3]){
00279     sprintf( st ,":112,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
00280     _SpaceSensor.puts(st);
00281 }
00282 void SpaceSensor::SetCompassMultiReferenceCheckVector(int Index,float Vector[3]){
00283     sprintf( st ,":113,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
00284     _SpaceSensor.puts(st);
00285 }
00286 void SpaceSensor::SetAccelMultiReferenceVector(int Index,float Vector[3]){
00287     sprintf( st ,":114,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
00288     _SpaceSensor.puts(st);
00289 }
00290 void SpaceSensor::SetAccelMultiReferenceCheckVector(int Index,float Vector[3]){
00291     sprintf( st ,":115,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
00292     _SpaceSensor.puts(st);
00293 }
00294 void SpaceSensor::SetAxisDirections(int direction){
00295     sprintf( st ,":116,%d\n",direction);
00296     _SpaceSensor.puts(st);
00297 }
00298 void SpaceSensor::SetRunningAveragePercent(float Percent){
00299     sprintf( st ,":117,%f\n",Percent);
00300     _SpaceSensor.puts(st);
00301 }
00302 void SpaceSensor::SetCompassReferenceVector(float Vector[3]){
00303     sprintf( st ,":118,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
00304     _SpaceSensor.puts(st);
00305 }
00306 void SpaceSensor::SetAccelerometerReferenceVector(float Vector[3]){
00307     sprintf( st ,":119,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
00308     _SpaceSensor.puts(st);
00309 }
00310 void SpaceSensor::ResetKalmanFilter(){
00311     _SpaceSensor.puts(":120\n");
00312 }
00313 void SpaceSensor::SetAccelerometerRange(int Accelrange){
00314     sprintf( st ,":121,%d\n",Accelrange);
00315     _SpaceSensor.puts(st);
00316 }
00317 void SpaceSensor::SetMultiReferenceWeightPower(float WeightPower){
00318     sprintf( st ,":122,%f\n",WeightPower);
00319     _SpaceSensor.puts(st);
00320 }
00321 void SpaceSensor::EnableDisableFilter(int Mode){
00322     sprintf( st ,":123,%d\n",Mode);
00323     _SpaceSensor.puts(st);
00324 }
00325 void SpaceSensor::SetRunningAverageMode(int Mode){
00326     sprintf( st ,":124,%d\n",Mode);
00327     _SpaceSensor.puts(st);
00328 }
00329 void SpaceSensor::SetGyroscopeRange(int mode){
00330     sprintf( st ,":125,%d\n",mode);
00331     _SpaceSensor.puts(st);
00332 }
00333 void SpaceSensor::SetCompassRange(int mode){
00334     sprintf( st ,":126,%d\n",mode);
00335     _SpaceSensor.puts(st);
00336 }
00337 /**********************************************************************************************************************/    
00338     /*4.3.6 Commands for Reading Filter Parameters*/
00339 /**********************************************************************************************************************/    
00340 void SpaceSensor::ReadTareOrientationQuaternion(float *Quaternion){
00341     _SpaceSensor.puts(":128\n");
00342     _SpaceSensor.scanf("%s", st);
00343     read_string(Quaternion,st);
00344     st[0]='\n';
00345 }
00346 void SpaceSensor::ReadTareOrientationRotationMatrix(float *RotationMatrix){
00347     _SpaceSensor.puts(":129\n");
00348     _SpaceSensor.scanf("%s", st);
00349     read_string(RotationMatrix,st);
00350     st[0]='\n';
00351 }
00352 void SpaceSensor::ReadRhoDataAccelerometer(int *Rhomode,float *minroh,float *maxroh){
00353     _SpaceSensor.puts(":130\n");
00354     _SpaceSensor.scanf("%s", st);
00355     read_string(vecf,st);
00356     Rhomode[0]=(int)vecf[0];
00357     minroh[0]=vecf[1];
00358     maxroh[0]=vecf[2];
00359     st[0]='\n';
00360 }
00361 void SpaceSensor::ReadRhoDataCompass(int *Rhomode,float *minroh,float *maxroh){
00362     _SpaceSensor.puts(":131\n");
00363     _SpaceSensor.scanf("%s", st);
00364     read_string(vecf,st);
00365     Rhomode[0]=(int)vecf[0];
00366     minroh[0]=vecf[1];
00367     maxroh[0]=vecf[2];
00368     st[0]='\n';
00369 }
00370 void SpaceSensor::ReadCurrentUpdateRate(int *rate){
00371     _SpaceSensor.puts(":132\n");
00372     _SpaceSensor.scanf("%s", st);
00373     read_string(vecf,st);
00374     rate[0]=(int)vecf[0];
00375     st[0]='\n';
00376 }
00377 void SpaceSensor::ReadCompassReferenceVector(float *Vector){
00378     _SpaceSensor.puts(":133\n");
00379     _SpaceSensor.scanf("%s", st);
00380     read_string(Vector,st);
00381     st[0]='\n';
00382 }
00383 void SpaceSensor::ReadAccelerometerReferenceVector(float *Vector){
00384     _SpaceSensor.puts(":134\n");
00385     _SpaceSensor.scanf("%s", st);
00386     read_string(Vector,st);
00387     st[0]='\n';
00388 }
00389 void SpaceSensor::ReadReferenceVectorMode(int *Mode){
00390     _SpaceSensor.puts(":135\n");
00391     _SpaceSensor.scanf("%s", st);
00392     read_string(vecf,st);
00393     Mode[0]=(int)vecf[0];
00394     st[0]='\n';
00395 }
00396 void SpaceSensor::ReadCompassMultiReferenceVector(int Index,float *Vector){
00397     sprintf( st ,":136,%d\n",Index);
00398     _SpaceSensor.puts(st);
00399     _SpaceSensor.scanf("%s", st);
00400     read_string(Vector,st);
00401     st[0]='\n';
00402 }
00403 void SpaceSensor::ReadCompassMultiReferenceCheckVector(int Index,float *Vector){
00404     sprintf( st ,":137,%d\n",Index);
00405     _SpaceSensor.puts(st);
00406     _SpaceSensor.scanf("%s", st);
00407     read_string(Vector,st);
00408     st[0]='\n';
00409 }
00410 void SpaceSensor::ReadAccelMultiReferenceVector(int Index,float *Vector){
00411     sprintf( st ,":138,%d\n",Index);
00412     _SpaceSensor.puts(st);
00413     _SpaceSensor.scanf("%s", st);
00414     read_string(Vector,st);
00415     st[0]='\n';
00416 }
00417 void SpaceSensor::ReadAccelMultiReferenceCheckVector(int Index,float *Vector){
00418     sprintf( st ,":139,%d\n",Index);
00419     _SpaceSensor.puts(st);
00420     _SpaceSensor.scanf("%s", st);
00421     read_string(Vector,st);
00422     st[0]='\n';
00423 }
00424 void SpaceSensor::ReadGyroEnabledState(int *Mode){
00425     _SpaceSensor.puts(":140\n");
00426     _SpaceSensor.scanf("%s", st);
00427     read_string(vecf,st);
00428     Mode[0]=(int)vecf[0];
00429     st[0]='\n';
00430 }
00431 void SpaceSensor::ReadAccelerometerEnabledState(int *Mode){
00432     _SpaceSensor.puts(":141\n");
00433     _SpaceSensor.scanf("%s", st);
00434     read_string(vecf,st);
00435     Mode[0]=(int)vecf[0];
00436     st[0]='\n';
00437 }
00438 void SpaceSensor::ReadCompassEnabledState(int *Mode){
00439     _SpaceSensor.puts(":142\n");
00440     _SpaceSensor.scanf("%s", st);
00441     read_string(vecf,st);
00442     Mode[0]=(int)vecf[0];
00443     st[0]='\n';
00444 }
00445 void SpaceSensor::ReadAxisDirections(int *direction){
00446     _SpaceSensor.puts(":143\n");
00447     _SpaceSensor.scanf("%s", st);
00448     read_string(vecf,st);
00449     direction[0]=(int)vecf[0];
00450     st[0]='\n';
00451 }
00452 void SpaceSensor::ReadOversampleRate(int *Rate){
00453     _SpaceSensor.puts(":144\n");
00454     _SpaceSensor.scanf("%s", st);
00455     read_string(vecf,st);
00456     Rate[0]=(int)vecf[0];
00457     st[0]='\n';
00458 }
00459 void SpaceSensor::ReadRunningAveragePercent(float *Percent){
00460     _SpaceSensor.puts(":145\n");
00461     _SpaceSensor.scanf("%s", st);
00462     read_string(Percent,st);
00463     st[0]='\n';
00464 }
00465 void SpaceSensor::ReadDesiredUpdateRate(float *rate){
00466     _SpaceSensor.puts(":146\n");
00467     _SpaceSensor.scanf("%s", st);
00468     read_string(rate,st);
00469     st[0]='\n';
00470 }
00471 void SpaceSensor::ReadKalmanFilterCovarianceMatrix(float *CovarianceMatrix){
00472     _SpaceSensor.puts(":147\n");
00473     _SpaceSensor.scanf("%s", st);
00474     read_string(CovarianceMatrix,st);
00475     st[0]='\n';
00476 }
00477 void SpaceSensor::ReadAccelerometerRange(int *Accelrange){
00478     _SpaceSensor.puts(":148\n");
00479     _SpaceSensor.scanf("%s", st);
00480     read_string(vecf,st);
00481     Accelrange[0]=(int)vecf[0];
00482     st[0]='\n';
00483 }
00484 void SpaceSensor::ReadMultiReferenceWeightPower(float *WeightPower){
00485     _SpaceSensor.puts(":149\n");
00486     _SpaceSensor.scanf("%s", st);
00487     read_string(WeightPower,st);
00488     st[0]='\n';
00489 }
00490 void SpaceSensor::ReadMultiReferenceResolution(int *Resolution){
00491     _SpaceSensor.puts(":150\n");
00492     _SpaceSensor.scanf("%s", st);
00493     read_string(vecf,st);
00494     Resolution[0]=(int)vecf[0];
00495     st[0]='\n';
00496 }
00497 void SpaceSensor::ReadNumberOfmultiReferenceCells(int *NumberOfCells){
00498     _SpaceSensor.puts(":151\n");
00499     _SpaceSensor.scanf("%s", st);
00500     read_string(vecf,st);
00501     NumberOfCells[0]=(int)vecf[0];
00502     st[0]='\n';
00503 }
00504 void SpaceSensor::ReadFilterEnableState(int *Mode){
00505     _SpaceSensor.puts(":152\n");
00506     _SpaceSensor.scanf("%s", st);
00507     read_string(vecf,st);
00508     Mode[0]=(int)vecf[0];
00509     st[0]='\n';
00510 }
00511 void SpaceSensor::ReadRunningAverageMode(int *Mode){
00512     _SpaceSensor.puts(":153\n");
00513     _SpaceSensor.scanf("%s", st);
00514     read_string(vecf,st);
00515     Mode[0]=(int)vecf[0];
00516     st[0]='\n';
00517 }
00518 void SpaceSensor::ReadGyroscopeRange(int *mode){
00519     _SpaceSensor.puts(":154\n");
00520     _SpaceSensor.scanf("%s", st);
00521     read_string(vecf,st);
00522     mode[0]=(int)vecf[0];
00523     st[0]='\n';
00524 }
00525 void SpaceSensor::ReadCompassRange(int *mode){
00526     _SpaceSensor.puts(":155\n");
00527     _SpaceSensor.scanf("%s", st);
00528     read_string(vecf,st);
00529     mode[0]=(int)vecf[0];
00530     st[0]='\n';
00531 }
00532 /**********************************************************************************************************************/    
00533     /*4.3.7 Commands for Calibration*/
00534 /**********************************************************************************************************************/    
00535 void SpaceSensor::SetCompassCalibrationParameters(float Bias[3],float Matrix[9]){
00536     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]);
00537     _SpaceSensor.puts(st);
00538     st[0]='\n';
00539 }
00540 void SpaceSensor::SetAccelerometerCalibrationParameters(float Bias[3],float Matrix[9]){
00541     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]);
00542     _SpaceSensor.puts(st);
00543     st[0]='\n';
00544 }
00545 void SpaceSensor::ReadCompassCalibrationParameters(float *Bias,float *Matrix){
00546     _SpaceSensor.puts(":162\n");
00547     _SpaceSensor.scanf("%s", st);
00548     read_string(vecf,st);
00549     for(int i=0;i<3;i++){Bias[i]=vecf[i];}
00550     for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
00551     st[0]='\n';
00552 }
00553 void SpaceSensor::ReadAccelerometerCalibrationParameters(float *Bias,float *Matrix){
00554     _SpaceSensor.puts(":163\n");
00555     _SpaceSensor.scanf("%s", st);
00556     read_string(vecf,st);
00557     for(int i=0;i<3;i++){Bias[i]=vecf[i];}
00558     for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
00559     st[0]='\n';
00560 }
00561 void SpaceSensor::ReadGyroCalibrationParameters(float *Bias, float *range){
00562     _SpaceSensor.puts(":164\n");
00563     _SpaceSensor.scanf("%s", st);
00564     read_string(vecf,st);
00565     for(int i=0;i<3;i++){Bias[i]=vecf[i];}
00566     for(int i=0;i<3;i++){range[i]=vecf[i+3];}
00567     st[0]='\n';
00568 }
00569 void SpaceSensor::BeginGyroAutocalibration(){
00570     _SpaceSensor.puts(":165\n");
00571 }
00572 void SpaceSensor::SetGyroCalibrationParameters(float Bias[3], float range[3]){
00573     sprintf( st ,":166,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],range[0],range[1],range[2]);
00574     _SpaceSensor.puts(st);
00575     st[0]='\n';
00576 }
00577 /**********************************************************************************************************************/    
00578     /*4.3.8 General Commands*/
00579 /**********************************************************************************************************************/    
00580 void SpaceSensor::ReadSoftwareVersion(char *version){
00581     _SpaceSensor.puts(":223\n");
00582     _SpaceSensor.scanf("%s", version);
00583 }
00584 void SpaceSensor::RestoreFactorySettings(){
00585     _SpaceSensor.puts(":224\n");
00586 }
00587 void SpaceSensor::CommitSettings(){
00588     _SpaceSensor.puts(":225\n");
00589 }
00590 void SpaceSensor::SoftwareReset(){
00591     _SpaceSensor.puts(":226\n");
00592 }
00593 void SpaceSensor::EnableWatchdogTimer(int timer){
00594     sprintf( st ,":227,%d\n",timer);
00595     _SpaceSensor.puts(st);
00596 }
00597 void SpaceSensor::DisableWatchdogTimer(){
00598     _SpaceSensor.puts(":228\n");
00599 }
00600 void SpaceSensor::EnterFirmwareUpdateMode(){
00601     _SpaceSensor.puts(":229\n");
00602 }
00603 void SpaceSensor::ReadHardwareVersion(char *version){
00604     _SpaceSensor.puts(":230\n");
00605     _SpaceSensor.scanf("%s", version);
00606 }
00607 void SpaceSensor::SetUARTBaudRate(int Baudrate){
00608     sprintf( st ,":231,%d\n",Baudrate);
00609     _SpaceSensor.puts(st);
00610 }
00611 void SpaceSensor::GetUARTBaudRate(int *Baudrate){
00612     _SpaceSensor.puts(":232\n");
00613     _SpaceSensor.scanf("%s", st);
00614     read_string(vecf,st);
00615     Baudrate[0]=(int)vecf[0];
00616 }
00617 void SpaceSensor::SetUSBmode(int mode){
00618     sprintf( st ,":233,%d\n",mode);
00619     _SpaceSensor.puts(st);
00620 }
00621 void SpaceSensor::GetUSBmode(int *mode){
00622     _SpaceSensor.puts(":234\n");
00623     _SpaceSensor.scanf("%s", st);
00624     read_string(vecf,st);
00625     mode[0]=(int)vecf[0];
00626 }
00627 void SpaceSensor::SetClockSpeed(int clock){
00628     sprintf( st ,":235,%d\n",clock);
00629     _SpaceSensor.puts(st);
00630 }
00631 void SpaceSensor::GetClockSpeed(int *clock){
00632     _SpaceSensor.puts(":236\n");
00633     _SpaceSensor.scanf("%s", st);
00634     read_string(vecf,st);
00635     clock[0]=(int)vecf[0];
00636 }
00637 void SpaceSensor::GetSerialnumber(int *serialnum){
00638     _SpaceSensor.puts(":237\n");
00639     _SpaceSensor.scanf("%s", st);
00640     read_string(vecf,st);
00641     serialnum[0]=(int)vecf[0];
00642 }
00643 void SpaceSensor::SetLEDColor(float color[3]){
00644     sprintf( st ,":238,%f,%f,%f\n",color[0],color[1],color[2]);
00645     _SpaceSensor.puts(st);
00646 }
00647 void SpaceSensor::GetLEDColor(float *color){
00648     _SpaceSensor.puts(":239\n");
00649     _SpaceSensor.scanf("%s", st);
00650     read_string(color,st);
00651 }
00652 void SpaceSensor::EnableDisableJoystick(int Mode){
00653     sprintf( st ,":240,%d\n",Mode);
00654     _SpaceSensor.puts(st);
00655 }
00656 void SpaceSensor::EnableDisablemouse(int Mode){
00657     sprintf( st ,":241,%d\n",Mode);
00658     _SpaceSensor.puts(st);
00659 }
00660 void SpaceSensor::ReadJoystickEnabledState(int *Mode){
00661     _SpaceSensor.puts(":242\n");
00662     _SpaceSensor.scanf("%s", st);
00663     read_string(vecf,st);
00664     Mode[0]=(int)vecf[0];
00665 }
00666 void SpaceSensor::ReadMouseEnabledState(int *Mode){
00667     _SpaceSensor.puts(":243\n");
00668     _SpaceSensor.scanf("%s", st);
00669     read_string(vecf,st);
00670     Mode[0]=(int)vecf[0];
00671 }
00672 void SpaceSensor::SetControlMode(float Controlclass,float controlindex,float handlerindex){
00673     sprintf( st ,":244,%f,%f,%f\n",Controlclass,controlindex,handlerindex);
00674     _SpaceSensor.puts(st);
00675 }
00676 void SpaceSensor::SetControlData(float Controlclass,float controlindex,float datapointindex,float datapoint ){
00677     sprintf( st ,":245,%f,%f,%f,%f\n",Controlclass,controlindex,datapointindex,datapoint);
00678     _SpaceSensor.puts(st);
00679 }  
00680 void SpaceSensor::ReadControlMode(float *Handlerindex, float Controlclass,float controlindex){
00681     sprintf( st ,":246,%f,%f\n",Controlclass,controlindex);
00682     _SpaceSensor.puts(st);
00683     _SpaceSensor.scanf("%s", st);
00684     read_string(Handlerindex,st);
00685 }
00686 void SpaceSensor::ReadControlData(float *Datapoint,float ControlClass,float controlIndex,float dataPointIndex){
00687     sprintf( st ,":247,%f,%f,%f\n",ControlClass,controlIndex,dataPointIndex);
00688     _SpaceSensor.puts(st);
00689     _SpaceSensor.scanf("%s", st);
00690     read_string(Datapoint,st);
00691 }
00692 void SpaceSensor::SetMouseAbsoluteRelative(int Mode){
00693     sprintf( st ,":251,%d\n",Mode);
00694     _SpaceSensor.puts(st);
00695 }
00696 void SpaceSensor::ReadMouseAbsoluteRelative(int *Mode){
00697     _SpaceSensor.puts(":252\n");
00698     _SpaceSensor.scanf("%s", st);
00699     read_string(vecf,st);
00700     Mode[0]=(int)vecf[0];
00701 }
00702 void SpaceSensor::SetjoystickAndMousePresentRemoved(int Mode){
00703     sprintf( st ,":253,%d\n",Mode);
00704     _SpaceSensor.puts(st);
00705 }
00706 void SpaceSensor::ReadjoystickAndMousePresentRemoved(int *Mode){
00707     _SpaceSensor.puts(":254\n");
00708     _SpaceSensor.scanf("%s", st);
00709     read_string(vecf,st);
00710     Mode[0]=(int)vecf[0];
00711 }
00712 
00713 /**********************************************************************************************************************/    
00714 /*Private functions*/    
00715 /**********************************************************************************************************************/    
00716 void SpaceSensor::read_string(float *vector,char *c){
00717                     char var[10];                    
00718                     int var_size=0,j=0,i=0;
00719                     while(1){
00720                              
00721                              if(j==strlen(c)){
00722                                              vector[i]=atof(var);
00723                                              break;
00724                                             
00725                                             }
00726                              if(c[j]==','){
00727                                             vector[i]=atof(var);
00728                                             i++;
00729                                             j++;
00730                                             var_size=0;
00731                                             }
00732                              var[var_size]=c[j];
00733                              j++;
00734                              var_size++;
00735     
00736                              }                               
00737                     }
00738     
00739 void SpaceSensor::begin(long baud) {
00740     _SpaceSensor.baud(baud);
00741 }