quadcopter cufe
/
FollowWall2
Go to goal robot with follow wall algo
Diff: SpaceSensor.cpp
- Revision:
- 0:efef62b55c86
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/SpaceSensor.cpp Tue Apr 08 13:21:17 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); +}