![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
Go to goal robot with follow wall algo
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?
User | Revision | Line number | New 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 | } |