quadcopter cufe / Mbed 2 deprecated FollowWall2

Go to goal robot with follow wall algo

Dependencies:   mbed

SpaceSensor.cpp@0:efef62b55c86, 2014-04-08 (annotated)

Committer:
khaledelmadawi
Date:
Tue Apr 08 13:21:17 2014 +0000
Revision:
0:efef62b55c86
Follow wall

Who changed what in which revision?

UserRevisionLine numberNew contents of line
khaledelmadawi 0:efef62b55c86 1
khaledelmadawi 0:efef62b55c86 2 #include "mbed.h"
khaledelmadawi 0:efef62b55c86 3 #include "SpaceSensor.h"
khaledelmadawi 0:efef62b55c86 4 SpaceSensor::SpaceSensor(PinName p_tx, PinName p_rx): _SpaceSensor(p_tx, p_rx) {
khaledelmadawi 0:efef62b55c86 5 begin(115200);
khaledelmadawi 0:efef62b55c86 6 st = new char[128];
khaledelmadawi 0:efef62b55c86 7 vecf= new float[15];
khaledelmadawi 0:efef62b55c86 8 }
khaledelmadawi 0:efef62b55c86 9 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 10 /*4.3.1 Commands for Reading Filtered Sensor Data*/
khaledelmadawi 0:efef62b55c86 11 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 12 void SpaceSensor::ReadfilteredtaredQuaternion(float *Quaternion){
khaledelmadawi 0:efef62b55c86 13 _SpaceSensor.puts(":0\n");
khaledelmadawi 0:efef62b55c86 14 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 15 read_string(Quaternion,st);
khaledelmadawi 0:efef62b55c86 16 st[0]='\n';
khaledelmadawi 0:efef62b55c86 17 }
khaledelmadawi 0:efef62b55c86 18 void SpaceSensor::ReadfilteredtaredEulerAngles(float *EulerAngles){
khaledelmadawi 0:efef62b55c86 19 _SpaceSensor.puts(":1\n");
khaledelmadawi 0:efef62b55c86 20 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 21 read_string(EulerAngles,st);
khaledelmadawi 0:efef62b55c86 22 st[0]='\n';
khaledelmadawi 0:efef62b55c86 23 }
khaledelmadawi 0:efef62b55c86 24 void SpaceSensor::ReadfilteredtaredRotationMatrix(float *RotationMatrix){
khaledelmadawi 0:efef62b55c86 25 _SpaceSensor.puts(":2\n");
khaledelmadawi 0:efef62b55c86 26 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 27 read_string(RotationMatrix,st);
khaledelmadawi 0:efef62b55c86 28 st[0]='\n';
khaledelmadawi 0:efef62b55c86 29
khaledelmadawi 0:efef62b55c86 30 }
khaledelmadawi 0:efef62b55c86 31 void SpaceSensor::ReadfilteredtaredAxisAngle(float *Axis,float *Angle){
khaledelmadawi 0:efef62b55c86 32 _SpaceSensor.puts(":3\n");
khaledelmadawi 0:efef62b55c86 33 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 34 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 35 for(int i=0;i<3;i++){Axis[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 36 Angle[0]=vecf[3];
khaledelmadawi 0:efef62b55c86 37 st[0]='\n';
khaledelmadawi 0:efef62b55c86 38
khaledelmadawi 0:efef62b55c86 39 }
khaledelmadawi 0:efef62b55c86 40 void SpaceSensor::ReadfilteredtaredTwoVector(float *Forward,float *Down){
khaledelmadawi 0:efef62b55c86 41 _SpaceSensor.puts(":4\n");
khaledelmadawi 0:efef62b55c86 42 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 43 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 44 for(int i=0;i<3;i++){Forward[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 45 for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 46 st[0]='\n';
khaledelmadawi 0:efef62b55c86 47 }
khaledelmadawi 0:efef62b55c86 48 void SpaceSensor::Readfilteredgyrorates(float *Quaternion){
khaledelmadawi 0:efef62b55c86 49 _SpaceSensor.puts(":5\n");
khaledelmadawi 0:efef62b55c86 50 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 51 read_string(Quaternion,st);
khaledelmadawi 0:efef62b55c86 52 st[0]='\n';
khaledelmadawi 0:efef62b55c86 53 }
khaledelmadawi 0:efef62b55c86 54 void SpaceSensor::ReadfiltereduntaredQuaternion(float *Quaternion){
khaledelmadawi 0:efef62b55c86 55 _SpaceSensor.puts(":6\n");
khaledelmadawi 0:efef62b55c86 56 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 57 read_string(Quaternion,st);
khaledelmadawi 0:efef62b55c86 58 st[0]='\n';
khaledelmadawi 0:efef62b55c86 59 }
khaledelmadawi 0:efef62b55c86 60 void SpaceSensor::ReadfiltereduntaredEulerAngles(float *EulerAngles){
khaledelmadawi 0:efef62b55c86 61 _SpaceSensor.puts(":7\n");
khaledelmadawi 0:efef62b55c86 62 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 63 read_string(EulerAngles,st);
khaledelmadawi 0:efef62b55c86 64 st[0]='\n';
khaledelmadawi 0:efef62b55c86 65 }
khaledelmadawi 0:efef62b55c86 66 void SpaceSensor::ReadfiltereduntaredRotationMatrix(float *RotationMatrix){
khaledelmadawi 0:efef62b55c86 67 _SpaceSensor.puts(":8\n");
khaledelmadawi 0:efef62b55c86 68 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 69 read_string(RotationMatrix,st);
khaledelmadawi 0:efef62b55c86 70 st[0]='\n';
khaledelmadawi 0:efef62b55c86 71 }
khaledelmadawi 0:efef62b55c86 72 void SpaceSensor::ReadfiltereduntaredAxisAngle(float *Axis,float *Angle){
khaledelmadawi 0:efef62b55c86 73 _SpaceSensor.puts(":9\n");
khaledelmadawi 0:efef62b55c86 74 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 75 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 76 for(int i=0;i<3;i++){Axis[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 77 Angle[0]=vecf[3];
khaledelmadawi 0:efef62b55c86 78 st[0]='\n';
khaledelmadawi 0:efef62b55c86 79 }
khaledelmadawi 0:efef62b55c86 80 void SpaceSensor::ReadfiltereduntaredTwoVector(float *Forward,float *Down){
khaledelmadawi 0:efef62b55c86 81 _SpaceSensor.puts(":10\n");
khaledelmadawi 0:efef62b55c86 82 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 83 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 84 for(int i=0;i<3;i++){Forward[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 85 for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 86 st[0]='\n';
khaledelmadawi 0:efef62b55c86 87 }
khaledelmadawi 0:efef62b55c86 88 void SpaceSensor::ReadfilteredtaredForwardandDownVectors(float *Forward,float *Down){
khaledelmadawi 0:efef62b55c86 89 _SpaceSensor.puts(":11\n");
khaledelmadawi 0:efef62b55c86 90 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 91 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 92 for(int i=0;i<3;i++){Forward[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 93 for(int i=0;i<3;i++){Down[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 94 st[0]='\n';
khaledelmadawi 0:efef62b55c86 95 }
khaledelmadawi 0:efef62b55c86 96 void SpaceSensor::ReadfilteredNorthEarthVectors(float *North,float *Earth){
khaledelmadawi 0:efef62b55c86 97 _SpaceSensor.puts(":12\n");
khaledelmadawi 0:efef62b55c86 98 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 99 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 100 for(int i=0;i<3;i++){North[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 101 for(int i=0;i<3;i++){Earth[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 102 st[0]='\n';
khaledelmadawi 0:efef62b55c86 103 }
khaledelmadawi 0:efef62b55c86 104
khaledelmadawi 0:efef62b55c86 105 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 106 /*4.3.2 Commands for Interfacing with Electronic Systems*/
khaledelmadawi 0:efef62b55c86 107 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 108 void SpaceSensor::Setinterrupttype(int mode){
khaledelmadawi 0:efef62b55c86 109 sprintf( st ,":29,%d\n",mode);
khaledelmadawi 0:efef62b55c86 110 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 111 }
khaledelmadawi 0:efef62b55c86 112 void SpaceSensor::Readinterrupttype(int *interrupttype){
khaledelmadawi 0:efef62b55c86 113 _SpaceSensor.puts(":30\n");
khaledelmadawi 0:efef62b55c86 114 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 115 interrupttype[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 116 interrupttype[1]=(int)vecf[1];
khaledelmadawi 0:efef62b55c86 117 }
khaledelmadawi 0:efef62b55c86 118 void SpaceSensor::Readinterruptstatus(int *interruptstatus){
khaledelmadawi 0:efef62b55c86 119 _SpaceSensor.puts(":31\n");
khaledelmadawi 0:efef62b55c86 120 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 121 interruptstatus[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 122 }
khaledelmadawi 0:efef62b55c86 123 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 124 /*4.3.3 Commands for Reading Normalized Sensor Data*/
khaledelmadawi 0:efef62b55c86 125 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 126 void SpaceSensor::Readall(float *gyro,float *accelerometer,float *compass){
khaledelmadawi 0:efef62b55c86 127 _SpaceSensor.puts(":32\n");
khaledelmadawi 0:efef62b55c86 128 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 129 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 130 for(int i=0;i<3;i++){gyro[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 131 for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 132 for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
khaledelmadawi 0:efef62b55c86 133 st[0]='\n';
khaledelmadawi 0:efef62b55c86 134 }
khaledelmadawi 0:efef62b55c86 135 void SpaceSensor::Readgyros(float *gyro){
khaledelmadawi 0:efef62b55c86 136 _SpaceSensor.puts(":33\n");
khaledelmadawi 0:efef62b55c86 137 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 138 read_string(gyro,st);
khaledelmadawi 0:efef62b55c86 139 st[0]='\n';
khaledelmadawi 0:efef62b55c86 140 }
khaledelmadawi 0:efef62b55c86 141 void SpaceSensor::Readaccelerometer(float *accelerometer){
khaledelmadawi 0:efef62b55c86 142 _SpaceSensor.puts(":34\n");
khaledelmadawi 0:efef62b55c86 143 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 144 read_string(accelerometer,st);
khaledelmadawi 0:efef62b55c86 145 st[0]='\n';
khaledelmadawi 0:efef62b55c86 146 }
khaledelmadawi 0:efef62b55c86 147 void SpaceSensor::Readcompass(float *compass){
khaledelmadawi 0:efef62b55c86 148 _SpaceSensor.puts(":35\n");
khaledelmadawi 0:efef62b55c86 149 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 150 read_string(compass,st);
khaledelmadawi 0:efef62b55c86 151 st[0]='\n';
khaledelmadawi 0:efef62b55c86 152 }
khaledelmadawi 0:efef62b55c86 153 void SpaceSensor::ReadtemperatureC(float *temp){
khaledelmadawi 0:efef62b55c86 154 _SpaceSensor.puts(":36\n");
khaledelmadawi 0:efef62b55c86 155 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 156 read_string(temp,st);
khaledelmadawi 0:efef62b55c86 157 st[0]='\n';
khaledelmadawi 0:efef62b55c86 158 }
khaledelmadawi 0:efef62b55c86 159 void SpaceSensor::ReadtemperatureF(float *temp){
khaledelmadawi 0:efef62b55c86 160 _SpaceSensor.puts(":37\n");
khaledelmadawi 0:efef62b55c86 161 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 162 read_string(temp,st);
khaledelmadawi 0:efef62b55c86 163 st[0]='\n';
khaledelmadawi 0:efef62b55c86 164 }
khaledelmadawi 0:efef62b55c86 165 void SpaceSensor::Readconfidencefactor(float *confidencefactor){
khaledelmadawi 0:efef62b55c86 166 _SpaceSensor.puts(":38\n");
khaledelmadawi 0:efef62b55c86 167 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 168 read_string(confidencefactor,st);
khaledelmadawi 0:efef62b55c86 169 st[0]='\n';
khaledelmadawi 0:efef62b55c86 170 }
khaledelmadawi 0:efef62b55c86 171 void SpaceSensor::Readaccelerometerunnormalized(float *accelunnormalized){
khaledelmadawi 0:efef62b55c86 172 _SpaceSensor.puts(":39\n");
khaledelmadawi 0:efef62b55c86 173 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 174 read_string(accelunnormalized,st);
khaledelmadawi 0:efef62b55c86 175 st[0]='\n';
khaledelmadawi 0:efef62b55c86 176 }
khaledelmadawi 0:efef62b55c86 177 void SpaceSensor::Readcompassunnormalized(float *compassunnormalized){
khaledelmadawi 0:efef62b55c86 178 _SpaceSensor.puts(":30\n");
khaledelmadawi 0:efef62b55c86 179 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 180 read_string(compassunnormalized,st);
khaledelmadawi 0:efef62b55c86 181 st[0]='\n';
khaledelmadawi 0:efef62b55c86 182 }
khaledelmadawi 0:efef62b55c86 183 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 184 /*4.3.4 Commands for Reading Raw Sensor Data*/
khaledelmadawi 0:efef62b55c86 185 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 186 void SpaceSensor::Readallraw(float *gyro,float *accelerometer,float *compass){
khaledelmadawi 0:efef62b55c86 187 _SpaceSensor.puts(":64\n");
khaledelmadawi 0:efef62b55c86 188 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 189 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 190 for(int i=0;i<3;i++){gyro[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 191 for(int i=0;i<3;i++){accelerometer[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 192 for(int i=0;i<3;i++){compass[i]=vecf[i+6];}
khaledelmadawi 0:efef62b55c86 193 st[0]='\n';
khaledelmadawi 0:efef62b55c86 194
khaledelmadawi 0:efef62b55c86 195 }
khaledelmadawi 0:efef62b55c86 196 void SpaceSensor::Readgyroraw(float *gyro){
khaledelmadawi 0:efef62b55c86 197 _SpaceSensor.puts(":65\n");
khaledelmadawi 0:efef62b55c86 198 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 199 read_string(gyro,st);
khaledelmadawi 0:efef62b55c86 200 st[0]='\n';
khaledelmadawi 0:efef62b55c86 201 }
khaledelmadawi 0:efef62b55c86 202 void SpaceSensor::Readaccelerometerraw(float *accelerometer){
khaledelmadawi 0:efef62b55c86 203 _SpaceSensor.puts(":66\n");
khaledelmadawi 0:efef62b55c86 204 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 205 read_string(accelerometer,st);
khaledelmadawi 0:efef62b55c86 206 st[0]='\n';
khaledelmadawi 0:efef62b55c86 207 }
khaledelmadawi 0:efef62b55c86 208 void SpaceSensor::Readcompassraw(float *compass){
khaledelmadawi 0:efef62b55c86 209 _SpaceSensor.puts(":67\n");
khaledelmadawi 0:efef62b55c86 210 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 211 read_string(compass,st);
khaledelmadawi 0:efef62b55c86 212 st[0]='\n';
khaledelmadawi 0:efef62b55c86 213 }
khaledelmadawi 0:efef62b55c86 214 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 215 /*4.3.5 Commands for Setting Filter Parameters*/
khaledelmadawi 0:efef62b55c86 216 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 217 void SpaceSensor::Tarewithcurrentorientation(){
khaledelmadawi 0:efef62b55c86 218 _SpaceSensor.puts(":96\n");
khaledelmadawi 0:efef62b55c86 219 }
khaledelmadawi 0:efef62b55c86 220 void SpaceSensor::Tarewithquaternion(float Quaternion[4]){
khaledelmadawi 0:efef62b55c86 221 sprintf( st ,":97,%f,%f,%f,%f\n",Quaternion[0],Quaternion[1],Quaternion[2],Quaternion[3]);
khaledelmadawi 0:efef62b55c86 222 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 223 }
khaledelmadawi 0:efef62b55c86 224 void SpaceSensor::Tarewithrotationmatrix(float RotationMatrix[9]){
khaledelmadawi 0:efef62b55c86 225 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]);
khaledelmadawi 0:efef62b55c86 226 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 227 }
khaledelmadawi 0:efef62b55c86 228 void SpaceSensor::SetStaticRhoModeAccelerometer(float rho){
khaledelmadawi 0:efef62b55c86 229 sprintf( st ,":99,%f\n",rho);
khaledelmadawi 0:efef62b55c86 230 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 231 }
khaledelmadawi 0:efef62b55c86 232 void SpaceSensor::SetConfidenceRhoModeAccelerometer(float min,float max){
khaledelmadawi 0:efef62b55c86 233 sprintf( st ,":100,%f,%f\n",min,max);
khaledelmadawi 0:efef62b55c86 234 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 235 }
khaledelmadawi 0:efef62b55c86 236 void SpaceSensor::SetStaticRhoModeCompass(float rho){
khaledelmadawi 0:efef62b55c86 237 sprintf( st ,":101,%f\n",rho);
khaledelmadawi 0:efef62b55c86 238 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 239 }
khaledelmadawi 0:efef62b55c86 240 void SpaceSensor::SetConfidenceRhoModeCompass(float min, float max){
khaledelmadawi 0:efef62b55c86 241 sprintf( st ,":102,%f,%f\n",min,max);
khaledelmadawi 0:efef62b55c86 242 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 243 }
khaledelmadawi 0:efef62b55c86 244 void SpaceSensor::SetDesiredUpdateRate(int t){
khaledelmadawi 0:efef62b55c86 245 sprintf( st ,":103,%d\n",t);
khaledelmadawi 0:efef62b55c86 246 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 247 }
khaledelmadawi 0:efef62b55c86 248 void SpaceSensor::SetMultiReferenceVectorsWithCurrentOrientation(){
khaledelmadawi 0:efef62b55c86 249 _SpaceSensor.puts(":l04\n");
khaledelmadawi 0:efef62b55c86 250 }
khaledelmadawi 0:efef62b55c86 251 void SpaceSensor::SetReferenceVectorMode(int mode){
khaledelmadawi 0:efef62b55c86 252 sprintf( st ,":105,%d\n",mode);
khaledelmadawi 0:efef62b55c86 253 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 254 }
khaledelmadawi 0:efef62b55c86 255 void SpaceSensor::SetOversampleRate(int rate){
khaledelmadawi 0:efef62b55c86 256 sprintf( st ,":106,%d\n",rate);
khaledelmadawi 0:efef62b55c86 257 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 258 }
khaledelmadawi 0:efef62b55c86 259 void SpaceSensor::EnableDisablegyros(int mode){
khaledelmadawi 0:efef62b55c86 260 sprintf( st ,":107,%d\n",mode);
khaledelmadawi 0:efef62b55c86 261 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 262 }
khaledelmadawi 0:efef62b55c86 263 void SpaceSensor::EnableDisableAccelerometer(int mode){
khaledelmadawi 0:efef62b55c86 264 sprintf( st ,":108,%d\n",mode);
khaledelmadawi 0:efef62b55c86 265 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 266 }
khaledelmadawi 0:efef62b55c86 267 void SpaceSensor::EnableDisableCompass(int mode){
khaledelmadawi 0:efef62b55c86 268 sprintf( st ,":109,%d\n",mode);
khaledelmadawi 0:efef62b55c86 269 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 270 }
khaledelmadawi 0:efef62b55c86 271 void SpaceSensor::ResetMultiReferenceVectorsToZero(){
khaledelmadawi 0:efef62b55c86 272 _SpaceSensor.puts(":110\n");
khaledelmadawi 0:efef62b55c86 273 }
khaledelmadawi 0:efef62b55c86 274 void SpaceSensor::SetMultiReferenceResolution(int mode){
khaledelmadawi 0:efef62b55c86 275 sprintf( st ,":111,%d\n",mode);
khaledelmadawi 0:efef62b55c86 276 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 277 }
khaledelmadawi 0:efef62b55c86 278 void SpaceSensor::SetCompassMultiReferenceVector(int Index,float Vector[3]){
khaledelmadawi 0:efef62b55c86 279 sprintf( st ,":112,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
khaledelmadawi 0:efef62b55c86 280 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 281 }
khaledelmadawi 0:efef62b55c86 282 void SpaceSensor::SetCompassMultiReferenceCheckVector(int Index,float Vector[3]){
khaledelmadawi 0:efef62b55c86 283 sprintf( st ,":113,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
khaledelmadawi 0:efef62b55c86 284 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 285 }
khaledelmadawi 0:efef62b55c86 286 void SpaceSensor::SetAccelMultiReferenceVector(int Index,float Vector[3]){
khaledelmadawi 0:efef62b55c86 287 sprintf( st ,":114,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
khaledelmadawi 0:efef62b55c86 288 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 289 }
khaledelmadawi 0:efef62b55c86 290 void SpaceSensor::SetAccelMultiReferenceCheckVector(int Index,float Vector[3]){
khaledelmadawi 0:efef62b55c86 291 sprintf( st ,":115,%d,%f,%f,%f\n",Index,Vector[0],Vector[1],Vector[2]);
khaledelmadawi 0:efef62b55c86 292 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 293 }
khaledelmadawi 0:efef62b55c86 294 void SpaceSensor::SetAxisDirections(int direction){
khaledelmadawi 0:efef62b55c86 295 sprintf( st ,":116,%d\n",direction);
khaledelmadawi 0:efef62b55c86 296 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 297 }
khaledelmadawi 0:efef62b55c86 298 void SpaceSensor::SetRunningAveragePercent(float Percent){
khaledelmadawi 0:efef62b55c86 299 sprintf( st ,":117,%f\n",Percent);
khaledelmadawi 0:efef62b55c86 300 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 301 }
khaledelmadawi 0:efef62b55c86 302 void SpaceSensor::SetCompassReferenceVector(float Vector[3]){
khaledelmadawi 0:efef62b55c86 303 sprintf( st ,":118,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
khaledelmadawi 0:efef62b55c86 304 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 305 }
khaledelmadawi 0:efef62b55c86 306 void SpaceSensor::SetAccelerometerReferenceVector(float Vector[3]){
khaledelmadawi 0:efef62b55c86 307 sprintf( st ,":119,%f,%f,%f\n",Vector[0],Vector[1],Vector[2]);
khaledelmadawi 0:efef62b55c86 308 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 309 }
khaledelmadawi 0:efef62b55c86 310 void SpaceSensor::ResetKalmanFilter(){
khaledelmadawi 0:efef62b55c86 311 _SpaceSensor.puts(":120\n");
khaledelmadawi 0:efef62b55c86 312 }
khaledelmadawi 0:efef62b55c86 313 void SpaceSensor::SetAccelerometerRange(int Accelrange){
khaledelmadawi 0:efef62b55c86 314 sprintf( st ,":121,%d\n",Accelrange);
khaledelmadawi 0:efef62b55c86 315 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 316 }
khaledelmadawi 0:efef62b55c86 317 void SpaceSensor::SetMultiReferenceWeightPower(float WeightPower){
khaledelmadawi 0:efef62b55c86 318 sprintf( st ,":122,%f\n",WeightPower);
khaledelmadawi 0:efef62b55c86 319 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 320 }
khaledelmadawi 0:efef62b55c86 321 void SpaceSensor::EnableDisableFilter(int Mode){
khaledelmadawi 0:efef62b55c86 322 sprintf( st ,":123,%d\n",Mode);
khaledelmadawi 0:efef62b55c86 323 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 324 }
khaledelmadawi 0:efef62b55c86 325 void SpaceSensor::SetRunningAverageMode(int Mode){
khaledelmadawi 0:efef62b55c86 326 sprintf( st ,":124,%d\n",Mode);
khaledelmadawi 0:efef62b55c86 327 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 328 }
khaledelmadawi 0:efef62b55c86 329 void SpaceSensor::SetGyroscopeRange(int mode){
khaledelmadawi 0:efef62b55c86 330 sprintf( st ,":125,%d\n",mode);
khaledelmadawi 0:efef62b55c86 331 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 332 }
khaledelmadawi 0:efef62b55c86 333 void SpaceSensor::SetCompassRange(int mode){
khaledelmadawi 0:efef62b55c86 334 sprintf( st ,":126,%d\n",mode);
khaledelmadawi 0:efef62b55c86 335 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 336 }
khaledelmadawi 0:efef62b55c86 337 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 338 /*4.3.6 Commands for Reading Filter Parameters*/
khaledelmadawi 0:efef62b55c86 339 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 340 void SpaceSensor::ReadTareOrientationQuaternion(float *Quaternion){
khaledelmadawi 0:efef62b55c86 341 _SpaceSensor.puts(":128\n");
khaledelmadawi 0:efef62b55c86 342 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 343 read_string(Quaternion,st);
khaledelmadawi 0:efef62b55c86 344 st[0]='\n';
khaledelmadawi 0:efef62b55c86 345 }
khaledelmadawi 0:efef62b55c86 346 void SpaceSensor::ReadTareOrientationRotationMatrix(float *RotationMatrix){
khaledelmadawi 0:efef62b55c86 347 _SpaceSensor.puts(":129\n");
khaledelmadawi 0:efef62b55c86 348 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 349 read_string(RotationMatrix,st);
khaledelmadawi 0:efef62b55c86 350 st[0]='\n';
khaledelmadawi 0:efef62b55c86 351 }
khaledelmadawi 0:efef62b55c86 352 void SpaceSensor::ReadRhoDataAccelerometer(int *Rhomode,float *minroh,float *maxroh){
khaledelmadawi 0:efef62b55c86 353 _SpaceSensor.puts(":130\n");
khaledelmadawi 0:efef62b55c86 354 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 355 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 356 Rhomode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 357 minroh[0]=vecf[1];
khaledelmadawi 0:efef62b55c86 358 maxroh[0]=vecf[2];
khaledelmadawi 0:efef62b55c86 359 st[0]='\n';
khaledelmadawi 0:efef62b55c86 360 }
khaledelmadawi 0:efef62b55c86 361 void SpaceSensor::ReadRhoDataCompass(int *Rhomode,float *minroh,float *maxroh){
khaledelmadawi 0:efef62b55c86 362 _SpaceSensor.puts(":131\n");
khaledelmadawi 0:efef62b55c86 363 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 364 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 365 Rhomode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 366 minroh[0]=vecf[1];
khaledelmadawi 0:efef62b55c86 367 maxroh[0]=vecf[2];
khaledelmadawi 0:efef62b55c86 368 st[0]='\n';
khaledelmadawi 0:efef62b55c86 369 }
khaledelmadawi 0:efef62b55c86 370 void SpaceSensor::ReadCurrentUpdateRate(int *rate){
khaledelmadawi 0:efef62b55c86 371 _SpaceSensor.puts(":132\n");
khaledelmadawi 0:efef62b55c86 372 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 373 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 374 rate[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 375 st[0]='\n';
khaledelmadawi 0:efef62b55c86 376 }
khaledelmadawi 0:efef62b55c86 377 void SpaceSensor::ReadCompassReferenceVector(float *Vector){
khaledelmadawi 0:efef62b55c86 378 _SpaceSensor.puts(":133\n");
khaledelmadawi 0:efef62b55c86 379 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 380 read_string(Vector,st);
khaledelmadawi 0:efef62b55c86 381 st[0]='\n';
khaledelmadawi 0:efef62b55c86 382 }
khaledelmadawi 0:efef62b55c86 383 void SpaceSensor::ReadAccelerometerReferenceVector(float *Vector){
khaledelmadawi 0:efef62b55c86 384 _SpaceSensor.puts(":134\n");
khaledelmadawi 0:efef62b55c86 385 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 386 read_string(Vector,st);
khaledelmadawi 0:efef62b55c86 387 st[0]='\n';
khaledelmadawi 0:efef62b55c86 388 }
khaledelmadawi 0:efef62b55c86 389 void SpaceSensor::ReadReferenceVectorMode(int *Mode){
khaledelmadawi 0:efef62b55c86 390 _SpaceSensor.puts(":135\n");
khaledelmadawi 0:efef62b55c86 391 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 392 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 393 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 394 st[0]='\n';
khaledelmadawi 0:efef62b55c86 395 }
khaledelmadawi 0:efef62b55c86 396 void SpaceSensor::ReadCompassMultiReferenceVector(int Index,float *Vector){
khaledelmadawi 0:efef62b55c86 397 sprintf( st ,":136,%d\n",Index);
khaledelmadawi 0:efef62b55c86 398 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 399 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 400 read_string(Vector,st);
khaledelmadawi 0:efef62b55c86 401 st[0]='\n';
khaledelmadawi 0:efef62b55c86 402 }
khaledelmadawi 0:efef62b55c86 403 void SpaceSensor::ReadCompassMultiReferenceCheckVector(int Index,float *Vector){
khaledelmadawi 0:efef62b55c86 404 sprintf( st ,":137,%d\n",Index);
khaledelmadawi 0:efef62b55c86 405 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 406 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 407 read_string(Vector,st);
khaledelmadawi 0:efef62b55c86 408 st[0]='\n';
khaledelmadawi 0:efef62b55c86 409 }
khaledelmadawi 0:efef62b55c86 410 void SpaceSensor::ReadAccelMultiReferenceVector(int Index,float *Vector){
khaledelmadawi 0:efef62b55c86 411 sprintf( st ,":138,%d\n",Index);
khaledelmadawi 0:efef62b55c86 412 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 413 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 414 read_string(Vector,st);
khaledelmadawi 0:efef62b55c86 415 st[0]='\n';
khaledelmadawi 0:efef62b55c86 416 }
khaledelmadawi 0:efef62b55c86 417 void SpaceSensor::ReadAccelMultiReferenceCheckVector(int Index,float *Vector){
khaledelmadawi 0:efef62b55c86 418 sprintf( st ,":139,%d\n",Index);
khaledelmadawi 0:efef62b55c86 419 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 420 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 421 read_string(Vector,st);
khaledelmadawi 0:efef62b55c86 422 st[0]='\n';
khaledelmadawi 0:efef62b55c86 423 }
khaledelmadawi 0:efef62b55c86 424 void SpaceSensor::ReadGyroEnabledState(int *Mode){
khaledelmadawi 0:efef62b55c86 425 _SpaceSensor.puts(":140\n");
khaledelmadawi 0:efef62b55c86 426 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 427 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 428 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 429 st[0]='\n';
khaledelmadawi 0:efef62b55c86 430 }
khaledelmadawi 0:efef62b55c86 431 void SpaceSensor::ReadAccelerometerEnabledState(int *Mode){
khaledelmadawi 0:efef62b55c86 432 _SpaceSensor.puts(":141\n");
khaledelmadawi 0:efef62b55c86 433 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 434 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 435 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 436 st[0]='\n';
khaledelmadawi 0:efef62b55c86 437 }
khaledelmadawi 0:efef62b55c86 438 void SpaceSensor::ReadCompassEnabledState(int *Mode){
khaledelmadawi 0:efef62b55c86 439 _SpaceSensor.puts(":142\n");
khaledelmadawi 0:efef62b55c86 440 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 441 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 442 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 443 st[0]='\n';
khaledelmadawi 0:efef62b55c86 444 }
khaledelmadawi 0:efef62b55c86 445 void SpaceSensor::ReadAxisDirections(int *direction){
khaledelmadawi 0:efef62b55c86 446 _SpaceSensor.puts(":143\n");
khaledelmadawi 0:efef62b55c86 447 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 448 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 449 direction[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 450 st[0]='\n';
khaledelmadawi 0:efef62b55c86 451 }
khaledelmadawi 0:efef62b55c86 452 void SpaceSensor::ReadOversampleRate(int *Rate){
khaledelmadawi 0:efef62b55c86 453 _SpaceSensor.puts(":144\n");
khaledelmadawi 0:efef62b55c86 454 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 455 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 456 Rate[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 457 st[0]='\n';
khaledelmadawi 0:efef62b55c86 458 }
khaledelmadawi 0:efef62b55c86 459 void SpaceSensor::ReadRunningAveragePercent(float *Percent){
khaledelmadawi 0:efef62b55c86 460 _SpaceSensor.puts(":145\n");
khaledelmadawi 0:efef62b55c86 461 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 462 read_string(Percent,st);
khaledelmadawi 0:efef62b55c86 463 st[0]='\n';
khaledelmadawi 0:efef62b55c86 464 }
khaledelmadawi 0:efef62b55c86 465 void SpaceSensor::ReadDesiredUpdateRate(float *rate){
khaledelmadawi 0:efef62b55c86 466 _SpaceSensor.puts(":146\n");
khaledelmadawi 0:efef62b55c86 467 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 468 read_string(rate,st);
khaledelmadawi 0:efef62b55c86 469 st[0]='\n';
khaledelmadawi 0:efef62b55c86 470 }
khaledelmadawi 0:efef62b55c86 471 void SpaceSensor::ReadKalmanFilterCovarianceMatrix(float *CovarianceMatrix){
khaledelmadawi 0:efef62b55c86 472 _SpaceSensor.puts(":147\n");
khaledelmadawi 0:efef62b55c86 473 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 474 read_string(CovarianceMatrix,st);
khaledelmadawi 0:efef62b55c86 475 st[0]='\n';
khaledelmadawi 0:efef62b55c86 476 }
khaledelmadawi 0:efef62b55c86 477 void SpaceSensor::ReadAccelerometerRange(int *Accelrange){
khaledelmadawi 0:efef62b55c86 478 _SpaceSensor.puts(":148\n");
khaledelmadawi 0:efef62b55c86 479 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 480 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 481 Accelrange[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 482 st[0]='\n';
khaledelmadawi 0:efef62b55c86 483 }
khaledelmadawi 0:efef62b55c86 484 void SpaceSensor::ReadMultiReferenceWeightPower(float *WeightPower){
khaledelmadawi 0:efef62b55c86 485 _SpaceSensor.puts(":149\n");
khaledelmadawi 0:efef62b55c86 486 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 487 read_string(WeightPower,st);
khaledelmadawi 0:efef62b55c86 488 st[0]='\n';
khaledelmadawi 0:efef62b55c86 489 }
khaledelmadawi 0:efef62b55c86 490 void SpaceSensor::ReadMultiReferenceResolution(int *Resolution){
khaledelmadawi 0:efef62b55c86 491 _SpaceSensor.puts(":150\n");
khaledelmadawi 0:efef62b55c86 492 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 493 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 494 Resolution[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 495 st[0]='\n';
khaledelmadawi 0:efef62b55c86 496 }
khaledelmadawi 0:efef62b55c86 497 void SpaceSensor::ReadNumberOfmultiReferenceCells(int *NumberOfCells){
khaledelmadawi 0:efef62b55c86 498 _SpaceSensor.puts(":151\n");
khaledelmadawi 0:efef62b55c86 499 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 500 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 501 NumberOfCells[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 502 st[0]='\n';
khaledelmadawi 0:efef62b55c86 503 }
khaledelmadawi 0:efef62b55c86 504 void SpaceSensor::ReadFilterEnableState(int *Mode){
khaledelmadawi 0:efef62b55c86 505 _SpaceSensor.puts(":152\n");
khaledelmadawi 0:efef62b55c86 506 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 507 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 508 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 509 st[0]='\n';
khaledelmadawi 0:efef62b55c86 510 }
khaledelmadawi 0:efef62b55c86 511 void SpaceSensor::ReadRunningAverageMode(int *Mode){
khaledelmadawi 0:efef62b55c86 512 _SpaceSensor.puts(":153\n");
khaledelmadawi 0:efef62b55c86 513 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 514 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 515 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 516 st[0]='\n';
khaledelmadawi 0:efef62b55c86 517 }
khaledelmadawi 0:efef62b55c86 518 void SpaceSensor::ReadGyroscopeRange(int *mode){
khaledelmadawi 0:efef62b55c86 519 _SpaceSensor.puts(":154\n");
khaledelmadawi 0:efef62b55c86 520 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 521 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 522 mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 523 st[0]='\n';
khaledelmadawi 0:efef62b55c86 524 }
khaledelmadawi 0:efef62b55c86 525 void SpaceSensor::ReadCompassRange(int *mode){
khaledelmadawi 0:efef62b55c86 526 _SpaceSensor.puts(":155\n");
khaledelmadawi 0:efef62b55c86 527 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 528 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 529 mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 530 st[0]='\n';
khaledelmadawi 0:efef62b55c86 531 }
khaledelmadawi 0:efef62b55c86 532 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 533 /*4.3.7 Commands for Calibration*/
khaledelmadawi 0:efef62b55c86 534 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 535 void SpaceSensor::SetCompassCalibrationParameters(float Bias[3],float Matrix[9]){
khaledelmadawi 0:efef62b55c86 536 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]);
khaledelmadawi 0:efef62b55c86 537 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 538 st[0]='\n';
khaledelmadawi 0:efef62b55c86 539 }
khaledelmadawi 0:efef62b55c86 540 void SpaceSensor::SetAccelerometerCalibrationParameters(float Bias[3],float Matrix[9]){
khaledelmadawi 0:efef62b55c86 541 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]);
khaledelmadawi 0:efef62b55c86 542 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 543 st[0]='\n';
khaledelmadawi 0:efef62b55c86 544 }
khaledelmadawi 0:efef62b55c86 545 void SpaceSensor::ReadCompassCalibrationParameters(float *Bias,float *Matrix){
khaledelmadawi 0:efef62b55c86 546 _SpaceSensor.puts(":162\n");
khaledelmadawi 0:efef62b55c86 547 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 548 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 549 for(int i=0;i<3;i++){Bias[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 550 for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 551 st[0]='\n';
khaledelmadawi 0:efef62b55c86 552 }
khaledelmadawi 0:efef62b55c86 553 void SpaceSensor::ReadAccelerometerCalibrationParameters(float *Bias,float *Matrix){
khaledelmadawi 0:efef62b55c86 554 _SpaceSensor.puts(":163\n");
khaledelmadawi 0:efef62b55c86 555 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 556 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 557 for(int i=0;i<3;i++){Bias[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 558 for(int i=0;i<9;i++){Matrix[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 559 st[0]='\n';
khaledelmadawi 0:efef62b55c86 560 }
khaledelmadawi 0:efef62b55c86 561 void SpaceSensor::ReadGyroCalibrationParameters(float *Bias, float *range){
khaledelmadawi 0:efef62b55c86 562 _SpaceSensor.puts(":164\n");
khaledelmadawi 0:efef62b55c86 563 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 564 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 565 for(int i=0;i<3;i++){Bias[i]=vecf[i];}
khaledelmadawi 0:efef62b55c86 566 for(int i=0;i<3;i++){range[i]=vecf[i+3];}
khaledelmadawi 0:efef62b55c86 567 st[0]='\n';
khaledelmadawi 0:efef62b55c86 568 }
khaledelmadawi 0:efef62b55c86 569 void SpaceSensor::BeginGyroAutocalibration(){
khaledelmadawi 0:efef62b55c86 570 _SpaceSensor.puts(":165\n");
khaledelmadawi 0:efef62b55c86 571 }
khaledelmadawi 0:efef62b55c86 572 void SpaceSensor::SetGyroCalibrationParameters(float Bias[3], float range[3]){
khaledelmadawi 0:efef62b55c86 573 sprintf( st ,":166,%f,%f,%f,%f,%f,%f\n",Bias[0],Bias[1],Bias[2],range[0],range[1],range[2]);
khaledelmadawi 0:efef62b55c86 574 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 575 st[0]='\n';
khaledelmadawi 0:efef62b55c86 576 }
khaledelmadawi 0:efef62b55c86 577 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 578 /*4.3.8 General Commands*/
khaledelmadawi 0:efef62b55c86 579 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 580 void SpaceSensor::ReadSoftwareVersion(char *version){
khaledelmadawi 0:efef62b55c86 581 _SpaceSensor.puts(":223\n");
khaledelmadawi 0:efef62b55c86 582 _SpaceSensor.scanf("%s", version);
khaledelmadawi 0:efef62b55c86 583 }
khaledelmadawi 0:efef62b55c86 584 void SpaceSensor::RestoreFactorySettings(){
khaledelmadawi 0:efef62b55c86 585 _SpaceSensor.puts(":224\n");
khaledelmadawi 0:efef62b55c86 586 }
khaledelmadawi 0:efef62b55c86 587 void SpaceSensor::CommitSettings(){
khaledelmadawi 0:efef62b55c86 588 _SpaceSensor.puts(":225\n");
khaledelmadawi 0:efef62b55c86 589 }
khaledelmadawi 0:efef62b55c86 590 void SpaceSensor::SoftwareReset(){
khaledelmadawi 0:efef62b55c86 591 _SpaceSensor.puts(":226\n");
khaledelmadawi 0:efef62b55c86 592 }
khaledelmadawi 0:efef62b55c86 593 void SpaceSensor::EnableWatchdogTimer(int timer){
khaledelmadawi 0:efef62b55c86 594 sprintf( st ,":227,%d\n",timer);
khaledelmadawi 0:efef62b55c86 595 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 596 }
khaledelmadawi 0:efef62b55c86 597 void SpaceSensor::DisableWatchdogTimer(){
khaledelmadawi 0:efef62b55c86 598 _SpaceSensor.puts(":228\n");
khaledelmadawi 0:efef62b55c86 599 }
khaledelmadawi 0:efef62b55c86 600 void SpaceSensor::EnterFirmwareUpdateMode(){
khaledelmadawi 0:efef62b55c86 601 _SpaceSensor.puts(":229\n");
khaledelmadawi 0:efef62b55c86 602 }
khaledelmadawi 0:efef62b55c86 603 void SpaceSensor::ReadHardwareVersion(char *version){
khaledelmadawi 0:efef62b55c86 604 _SpaceSensor.puts(":230\n");
khaledelmadawi 0:efef62b55c86 605 _SpaceSensor.scanf("%s", version);
khaledelmadawi 0:efef62b55c86 606 }
khaledelmadawi 0:efef62b55c86 607 void SpaceSensor::SetUARTBaudRate(int Baudrate){
khaledelmadawi 0:efef62b55c86 608 sprintf( st ,":231,%d\n",Baudrate);
khaledelmadawi 0:efef62b55c86 609 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 610 }
khaledelmadawi 0:efef62b55c86 611 void SpaceSensor::GetUARTBaudRate(int *Baudrate){
khaledelmadawi 0:efef62b55c86 612 _SpaceSensor.puts(":232\n");
khaledelmadawi 0:efef62b55c86 613 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 614 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 615 Baudrate[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 616 }
khaledelmadawi 0:efef62b55c86 617 void SpaceSensor::SetUSBmode(int mode){
khaledelmadawi 0:efef62b55c86 618 sprintf( st ,":233,%d\n",mode);
khaledelmadawi 0:efef62b55c86 619 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 620 }
khaledelmadawi 0:efef62b55c86 621 void SpaceSensor::GetUSBmode(int *mode){
khaledelmadawi 0:efef62b55c86 622 _SpaceSensor.puts(":234\n");
khaledelmadawi 0:efef62b55c86 623 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 624 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 625 mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 626 }
khaledelmadawi 0:efef62b55c86 627 void SpaceSensor::SetClockSpeed(int clock){
khaledelmadawi 0:efef62b55c86 628 sprintf( st ,":235,%d\n",clock);
khaledelmadawi 0:efef62b55c86 629 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 630 }
khaledelmadawi 0:efef62b55c86 631 void SpaceSensor::GetClockSpeed(int *clock){
khaledelmadawi 0:efef62b55c86 632 _SpaceSensor.puts(":236\n");
khaledelmadawi 0:efef62b55c86 633 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 634 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 635 clock[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 636 }
khaledelmadawi 0:efef62b55c86 637 void SpaceSensor::GetSerialnumber(int *serialnum){
khaledelmadawi 0:efef62b55c86 638 _SpaceSensor.puts(":237\n");
khaledelmadawi 0:efef62b55c86 639 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 640 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 641 serialnum[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 642 }
khaledelmadawi 0:efef62b55c86 643 void SpaceSensor::SetLEDColor(float color[3]){
khaledelmadawi 0:efef62b55c86 644 sprintf( st ,":238,%f,%f,%f\n",color[0],color[1],color[2]);
khaledelmadawi 0:efef62b55c86 645 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 646 }
khaledelmadawi 0:efef62b55c86 647 void SpaceSensor::GetLEDColor(float *color){
khaledelmadawi 0:efef62b55c86 648 _SpaceSensor.puts(":239\n");
khaledelmadawi 0:efef62b55c86 649 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 650 read_string(color,st);
khaledelmadawi 0:efef62b55c86 651 }
khaledelmadawi 0:efef62b55c86 652 void SpaceSensor::EnableDisableJoystick(int Mode){
khaledelmadawi 0:efef62b55c86 653 sprintf( st ,":240,%d\n",Mode);
khaledelmadawi 0:efef62b55c86 654 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 655 }
khaledelmadawi 0:efef62b55c86 656 void SpaceSensor::EnableDisablemouse(int Mode){
khaledelmadawi 0:efef62b55c86 657 sprintf( st ,":241,%d\n",Mode);
khaledelmadawi 0:efef62b55c86 658 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 659 }
khaledelmadawi 0:efef62b55c86 660 void SpaceSensor::ReadJoystickEnabledState(int *Mode){
khaledelmadawi 0:efef62b55c86 661 _SpaceSensor.puts(":242\n");
khaledelmadawi 0:efef62b55c86 662 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 663 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 664 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 665 }
khaledelmadawi 0:efef62b55c86 666 void SpaceSensor::ReadMouseEnabledState(int *Mode){
khaledelmadawi 0:efef62b55c86 667 _SpaceSensor.puts(":243\n");
khaledelmadawi 0:efef62b55c86 668 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 669 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 670 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 671 }
khaledelmadawi 0:efef62b55c86 672 void SpaceSensor::SetControlMode(float Controlclass,float controlindex,float handlerindex){
khaledelmadawi 0:efef62b55c86 673 sprintf( st ,":244,%f,%f,%f\n",Controlclass,controlindex,handlerindex);
khaledelmadawi 0:efef62b55c86 674 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 675 }
khaledelmadawi 0:efef62b55c86 676 void SpaceSensor::SetControlData(float Controlclass,float controlindex,float datapointindex,float datapoint ){
khaledelmadawi 0:efef62b55c86 677 sprintf( st ,":245,%f,%f,%f,%f\n",Controlclass,controlindex,datapointindex,datapoint);
khaledelmadawi 0:efef62b55c86 678 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 679 }
khaledelmadawi 0:efef62b55c86 680 void SpaceSensor::ReadControlMode(float *Handlerindex, float Controlclass,float controlindex){
khaledelmadawi 0:efef62b55c86 681 sprintf( st ,":246,%f,%f\n",Controlclass,controlindex);
khaledelmadawi 0:efef62b55c86 682 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 683 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 684 read_string(Handlerindex,st);
khaledelmadawi 0:efef62b55c86 685 }
khaledelmadawi 0:efef62b55c86 686 void SpaceSensor::ReadControlData(float *Datapoint,float ControlClass,float controlIndex,float dataPointIndex){
khaledelmadawi 0:efef62b55c86 687 sprintf( st ,":247,%f,%f,%f\n",ControlClass,controlIndex,dataPointIndex);
khaledelmadawi 0:efef62b55c86 688 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 689 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 690 read_string(Datapoint,st);
khaledelmadawi 0:efef62b55c86 691 }
khaledelmadawi 0:efef62b55c86 692 void SpaceSensor::SetMouseAbsoluteRelative(int Mode){
khaledelmadawi 0:efef62b55c86 693 sprintf( st ,":251,%d\n",Mode);
khaledelmadawi 0:efef62b55c86 694 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 695 }
khaledelmadawi 0:efef62b55c86 696 void SpaceSensor::ReadMouseAbsoluteRelative(int *Mode){
khaledelmadawi 0:efef62b55c86 697 _SpaceSensor.puts(":252\n");
khaledelmadawi 0:efef62b55c86 698 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 699 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 700 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 701 }
khaledelmadawi 0:efef62b55c86 702 void SpaceSensor::SetjoystickAndMousePresentRemoved(int Mode){
khaledelmadawi 0:efef62b55c86 703 sprintf( st ,":253,%d\n",Mode);
khaledelmadawi 0:efef62b55c86 704 _SpaceSensor.puts(st);
khaledelmadawi 0:efef62b55c86 705 }
khaledelmadawi 0:efef62b55c86 706 void SpaceSensor::ReadjoystickAndMousePresentRemoved(int *Mode){
khaledelmadawi 0:efef62b55c86 707 _SpaceSensor.puts(":254\n");
khaledelmadawi 0:efef62b55c86 708 _SpaceSensor.scanf("%s", st);
khaledelmadawi 0:efef62b55c86 709 read_string(vecf,st);
khaledelmadawi 0:efef62b55c86 710 Mode[0]=(int)vecf[0];
khaledelmadawi 0:efef62b55c86 711 }
khaledelmadawi 0:efef62b55c86 712
khaledelmadawi 0:efef62b55c86 713 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 714 /*Private functions*/
khaledelmadawi 0:efef62b55c86 715 /**********************************************************************************************************************/
khaledelmadawi 0:efef62b55c86 716 void SpaceSensor::read_string(float *vector,char *c){
khaledelmadawi 0:efef62b55c86 717 char var[10];
khaledelmadawi 0:efef62b55c86 718 int var_size=0,j=0,i=0;
khaledelmadawi 0:efef62b55c86 719 while(1){
khaledelmadawi 0:efef62b55c86 720
khaledelmadawi 0:efef62b55c86 721 if(j==strlen(c)){
khaledelmadawi 0:efef62b55c86 722 vector[i]=atof(var);
khaledelmadawi 0:efef62b55c86 723 break;
khaledelmadawi 0:efef62b55c86 724
khaledelmadawi 0:efef62b55c86 725 }
khaledelmadawi 0:efef62b55c86 726 if(c[j]==','){
khaledelmadawi 0:efef62b55c86 727 vector[i]=atof(var);
khaledelmadawi 0:efef62b55c86 728 i++;
khaledelmadawi 0:efef62b55c86 729 j++;
khaledelmadawi 0:efef62b55c86 730 var_size=0;
khaledelmadawi 0:efef62b55c86 731 }
khaledelmadawi 0:efef62b55c86 732 var[var_size]=c[j];
khaledelmadawi 0:efef62b55c86 733 j++;
khaledelmadawi 0:efef62b55c86 734 var_size++;
khaledelmadawi 0:efef62b55c86 735
khaledelmadawi 0:efef62b55c86 736 }
khaledelmadawi 0:efef62b55c86 737 }
khaledelmadawi 0:efef62b55c86 738
khaledelmadawi 0:efef62b55c86 739 void SpaceSensor::begin(long baud) {
khaledelmadawi 0:efef62b55c86 740 _SpaceSensor.baud(baud);
khaledelmadawi 0:efef62b55c86 741 }