root / quad1 / AeroQuad / SerialCom.pde @ 9240aaa3
History | View | Annotate | Download (17.8 KB)
1 |
/* |
---|---|
2 |
AeroQuad v2.1 - October 2010 |
3 |
www.AeroQuad.com |
4 |
Copyright (c) 2010 Ted Carancho. All rights reserved. |
5 |
An Open Source Arduino based multicopter. |
6 |
|
7 |
This program is free software: you can redistribute it and/or modify |
8 |
it under the terms of the GNU General Public License as published by |
9 |
the Free Software Foundation, either version 3 of the License, or |
10 |
(at your option) any later version. |
11 |
|
12 |
This program is distributed in the hope that it will be useful, |
13 |
but WITHOUT ANY WARRANTY; without even the implied warranty of |
14 |
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
15 |
GNU General Public License for more details. |
16 |
|
17 |
You should have received a copy of the GNU General Public License |
18 |
along with this program. If not, see <http://www.gnu.org/licenses/>. |
19 |
*/ |
20 |
|
21 |
// SerialCom.pde is responsible for the serial communication for commands and telemetry from the AeroQuad |
22 |
// This comtains readSerialCommand() which listens for a serial command and it's arguments |
23 |
// This also contains readSerialTelemetry() which listens for a telemetry request and responds with the requested data |
24 |
// For more information on each command/telemetry look at: http://aeroquad.com/content.php?117 |
25 |
|
26 |
//*************************************************************************************************** |
27 |
//********************************** Serial Commands ************************************************ |
28 |
//*************************************************************************************************** |
29 |
void readSerialCommand() { |
30 |
// Check for serial message |
31 |
if (DebugPort.available()) { |
32 |
digitalWrite(LEDPIN, LOW); |
33 |
queryType = DebugPort.read(); |
34 |
switch (queryType) { |
35 |
case 'z' : |
36 |
DebugPort.print(receiver.getRaw(ROLL)); |
37 |
DebugPort.print("\t"); |
38 |
DebugPort.print(receiver.getRaw(AUX)); |
39 |
DebugPort.print("\t"); |
40 |
DebugPort.println(receiver.getRaw(AUX2)); |
41 |
break; |
42 |
|
43 |
case 'y': // Receive roll and pitch gyro PID |
44 |
PID[SONAR].P = readFloatSerial(); |
45 |
DebugPort.println(PID[SONAR].P); |
46 |
break; |
47 |
|
48 |
case 'u': // Receive roll and pitch gyro PID |
49 |
PID[SONAR].D = readFloatSerial(); |
50 |
DebugPort.println(PID[SONAR].D); |
51 |
break; |
52 |
|
53 |
case 'A': // Receive roll and pitch gyro PID |
54 |
PID[ROLL].P = readFloatSerial(); |
55 |
PID[ROLL].I = readFloatSerial(); |
56 |
PID[ROLL].D = readFloatSerial(); |
57 |
PID[ROLL].lastPosition = 0; |
58 |
PID[ROLL].integratedError = 0; |
59 |
PID[PITCH].P = readFloatSerial(); |
60 |
PID[PITCH].I = readFloatSerial(); |
61 |
PID[PITCH].D = readFloatSerial(); |
62 |
PID[PITCH].lastPosition = 0; |
63 |
PID[PITCH].integratedError = 0; |
64 |
minAcro = readFloatSerial(); |
65 |
break; |
66 |
case 'C': // Receive yaw PID |
67 |
PID[YAW].P = readFloatSerial(); |
68 |
PID[YAW].I = readFloatSerial(); |
69 |
PID[YAW].D = readFloatSerial(); |
70 |
PID[YAW].lastPosition = 0; |
71 |
PID[YAW].integratedError = 0; |
72 |
PID[HEADING].P = readFloatSerial(); |
73 |
PID[HEADING].I = readFloatSerial(); |
74 |
PID[HEADING].D = readFloatSerial(); |
75 |
PID[HEADING].lastPosition = 0; |
76 |
PID[HEADING].integratedError = 0; |
77 |
headingHoldConfig = readFloatSerial(); |
78 |
heading = 0; |
79 |
relativeHeading = 0; |
80 |
headingHold = 0; |
81 |
break; |
82 |
case 'E': // Receive roll and pitch auto level PID |
83 |
PID[LEVELROLL].P = readFloatSerial(); |
84 |
PID[LEVELROLL].I = readFloatSerial(); |
85 |
PID[LEVELROLL].D = readFloatSerial(); |
86 |
PID[LEVELROLL].lastPosition = 0; |
87 |
PID[LEVELROLL].integratedError = 0; |
88 |
PID[LEVELPITCH].P = readFloatSerial(); |
89 |
PID[LEVELPITCH].I = readFloatSerial(); |
90 |
PID[LEVELPITCH].D = readFloatSerial(); |
91 |
PID[LEVELPITCH].lastPosition = 0; |
92 |
PID[LEVELPITCH].integratedError = 0; |
93 |
PID[LEVELGYROROLL].P = readFloatSerial(); |
94 |
PID[LEVELGYROROLL].I = readFloatSerial(); |
95 |
PID[LEVELGYROROLL].D = readFloatSerial(); |
96 |
PID[LEVELGYROROLL].lastPosition = 0; |
97 |
PID[LEVELGYROROLL].integratedError = 0; |
98 |
PID[LEVELGYROPITCH].P = readFloatSerial(); |
99 |
PID[LEVELGYROPITCH].I = readFloatSerial(); |
100 |
PID[LEVELGYROPITCH].D = readFloatSerial(); |
101 |
PID[LEVELGYROPITCH].lastPosition = 0; |
102 |
PID[LEVELGYROPITCH].integratedError = 0; |
103 |
windupGuard = readFloatSerial(); |
104 |
break; |
105 |
case 'G': // Receive auto level configuration |
106 |
levelLimit = readFloatSerial(); |
107 |
levelOff = readFloatSerial(); |
108 |
break; |
109 |
case 'I': // Receiver altitude hold PID |
110 |
#ifdef AltitudeHold |
111 |
PID[ALTITUDE].P = readFloatSerial(); |
112 |
PID[ALTITUDE].I = readFloatSerial(); |
113 |
PID[ALTITUDE].D = readFloatSerial(); |
114 |
PID[ALTITUDE].lastPosition = 0; |
115 |
PID[ALTITUDE].integratedError = 0; |
116 |
minThrottleAdjust = readFloatSerial(); |
117 |
maxThrottleAdjust = readFloatSerial(); |
118 |
altitude.setSmoothFactor(readFloatSerial()); |
119 |
PID[ZDAMPENING].P = readFloatSerial(); |
120 |
PID[ZDAMPENING].I = readFloatSerial(); |
121 |
PID[ZDAMPENING].D = readFloatSerial(); |
122 |
#endif |
123 |
break; |
124 |
case 'K': // Receive data filtering values |
125 |
gyro.setSmoothFactor(readFloatSerial()); |
126 |
accel.setSmoothFactor(readFloatSerial()); |
127 |
timeConstant = readFloatSerial(); |
128 |
//flightMode = readFloatSerial(); |
129 |
break; |
130 |
case 'M': // Receive transmitter smoothing values |
131 |
receiver.setXmitFactor(readFloatSerial()); |
132 |
receiver.setSmoothFactor(ROLL, readFloatSerial()); |
133 |
receiver.setSmoothFactor(PITCH, readFloatSerial()); |
134 |
receiver.setSmoothFactor(YAW, readFloatSerial()); |
135 |
receiver.setSmoothFactor(THROTTLE, readFloatSerial()); |
136 |
receiver.setSmoothFactor(MODE, readFloatSerial()); |
137 |
receiver.setSmoothFactor(AUX, readFloatSerial()); |
138 |
break; |
139 |
case 'O': // Receive transmitter calibration values |
140 |
receiver.setTransmitterSlope(ROLL, readFloatSerial()); |
141 |
receiver.setTransmitterOffset(ROLL, readFloatSerial()); |
142 |
receiver.setTransmitterSlope(PITCH, readFloatSerial()); |
143 |
receiver.setTransmitterOffset(PITCH, readFloatSerial()); |
144 |
receiver.setTransmitterSlope(YAW, readFloatSerial()); |
145 |
receiver.setTransmitterOffset(YAW, readFloatSerial()); |
146 |
receiver.setTransmitterSlope(THROTTLE, readFloatSerial()); |
147 |
receiver.setTransmitterOffset(THROTTLE, readFloatSerial()); |
148 |
receiver.setTransmitterSlope(MODE, readFloatSerial()); |
149 |
receiver.setTransmitterOffset(MODE, readFloatSerial()); |
150 |
receiver.setTransmitterSlope(AUX, readFloatSerial()); |
151 |
receiver.setTransmitterOffset(AUX, readFloatSerial()); |
152 |
break; |
153 |
case 'W': // Write all user configurable values to EEPROM |
154 |
writeEEPROM(); // defined in DataStorage.h |
155 |
zeroIntegralError(); |
156 |
break; |
157 |
case 'Y': // Initialize EEPROM with default values |
158 |
initializeEEPROM(); // defined in DataStorage.h |
159 |
gyro.calibrate(); |
160 |
accel.calibrate(); |
161 |
zeroIntegralError(); |
162 |
break; |
163 |
case '1': // Calibrate ESCS's by setting Throttle high on all channels |
164 |
armed = 0; |
165 |
calibrateESC = 1; |
166 |
break; |
167 |
case '2': // Calibrate ESC's by setting Throttle low on all channels |
168 |
armed = 0; |
169 |
calibrateESC = 2; |
170 |
break; |
171 |
case '3': // Test ESC calibration |
172 |
armed = 0; |
173 |
testCommand = readFloatSerial(); |
174 |
calibrateESC = 3; |
175 |
break; |
176 |
case '4': // Turn off ESC calibration |
177 |
armed = 0; |
178 |
calibrateESC = 0; |
179 |
testCommand = 1000; |
180 |
break; |
181 |
case '5': // Send individual motor commands (motor, command) |
182 |
armed = 0; |
183 |
calibrateESC = 5; |
184 |
for (motor = FRONT; motor < LASTMOTOR; motor++) |
185 |
motors.setRemoteCommand(motor, readFloatSerial()); |
186 |
break; |
187 |
case 'a': |
188 |
// spare |
189 |
break; |
190 |
case 'b': // calibrate gyros |
191 |
gyro.calibrate(); |
192 |
break; |
193 |
case 'c': // calibrate accels |
194 |
accel.calibrate(); |
195 |
break; |
196 |
case 'd': // send aref |
197 |
aref = readFloatSerial(); |
198 |
break; |
199 |
} |
200 |
digitalWrite(LEDPIN, HIGH); |
201 |
} |
202 |
} |
203 |
|
204 |
//*************************************************************************************************** |
205 |
//********************************* Serial Telemetry ************************************************ |
206 |
//*************************************************************************************************** |
207 |
void sendSerialTelemetry() { |
208 |
update = 0; |
209 |
switch (queryType) { |
210 |
case '=': // Reserved debug command to view any variable from Serial Monitor |
211 |
DebugPort.print(accel.getZaxis()); |
212 |
comma(); |
213 |
DebugPort.print(accel.getOneG()); |
214 |
comma(); |
215 |
DebugPort.print(zDampening); |
216 |
comma(); |
217 |
DebugPort.print(throttleAdjust); |
218 |
DebugPort.println(); |
219 |
//queryType = 'X'; |
220 |
break; |
221 |
case 'B': // Send roll and pitch gyro PID values |
222 |
DebugPort.print(PID[ROLL].P); |
223 |
comma(); |
224 |
DebugPort.print(PID[ROLL].I); |
225 |
comma(); |
226 |
DebugPort.print(PID[ROLL].D); |
227 |
comma(); |
228 |
DebugPort.print(PID[PITCH].P); |
229 |
comma(); |
230 |
DebugPort.print(PID[PITCH].I); |
231 |
comma(); |
232 |
DebugPort.print(PID[PITCH].D); |
233 |
comma(); |
234 |
DebugPort.println(minAcro); |
235 |
queryType = 'X'; |
236 |
break; |
237 |
case 'D': // Send yaw PID values |
238 |
DebugPort.print(PID[YAW].P); |
239 |
comma(); |
240 |
DebugPort.print(PID[YAW].I); |
241 |
comma(); |
242 |
DebugPort.print(PID[YAW].D); |
243 |
comma(); |
244 |
DebugPort.print(PID[HEADING].P); |
245 |
comma(); |
246 |
DebugPort.print(PID[HEADING].I); |
247 |
comma(); |
248 |
DebugPort.print(PID[HEADING].D); |
249 |
comma(); |
250 |
DebugPort.println(headingHoldConfig, BIN); |
251 |
queryType = 'X'; |
252 |
break; |
253 |
case 'F': // Send roll and pitch auto level PID values |
254 |
DebugPort.print(PID[LEVELROLL].P); |
255 |
comma(); |
256 |
DebugPort.print(PID[LEVELROLL].I); |
257 |
comma(); |
258 |
DebugPort.print(PID[LEVELROLL].D); |
259 |
comma(); |
260 |
DebugPort.print(PID[LEVELPITCH].P); |
261 |
comma(); |
262 |
DebugPort.print(PID[LEVELPITCH].I); |
263 |
comma(); |
264 |
DebugPort.print(PID[LEVELPITCH].D); |
265 |
comma(); |
266 |
DebugPort.print(PID[LEVELGYROROLL].P); |
267 |
comma(); |
268 |
DebugPort.print(PID[LEVELGYROROLL].I); |
269 |
comma(); |
270 |
DebugPort.print(PID[LEVELGYROROLL].D); |
271 |
comma(); |
272 |
DebugPort.print(PID[LEVELGYROPITCH].P); |
273 |
comma(); |
274 |
DebugPort.print(PID[LEVELGYROPITCH].I); |
275 |
comma(); |
276 |
DebugPort.print(PID[LEVELGYROPITCH].D); |
277 |
comma(); |
278 |
DebugPort.println(windupGuard); |
279 |
queryType = 'X'; |
280 |
break; |
281 |
case 'H': // Send auto level configuration values |
282 |
DebugPort.print(levelLimit); |
283 |
comma(); |
284 |
DebugPort.println(levelOff); |
285 |
queryType = 'X'; |
286 |
break; |
287 |
case 'J': // Altitude Hold |
288 |
#ifdef AltitudeHold |
289 |
DebugPort.print(PID[ALTITUDE].P); |
290 |
comma(); |
291 |
DebugPort.print(PID[ALTITUDE].I); |
292 |
comma(); |
293 |
DebugPort.print(PID[ALTITUDE].D); |
294 |
comma(); |
295 |
DebugPort.print(minThrottleAdjust); |
296 |
comma(); |
297 |
DebugPort.print(maxThrottleAdjust); |
298 |
comma(); |
299 |
DebugPort.print(altitude.getSmoothFactor()); |
300 |
comma(); |
301 |
DebugPort.print(PID[ZDAMPENING].P); |
302 |
comma(); |
303 |
DebugPort.print(PID[ZDAMPENING].I); |
304 |
comma(); |
305 |
DebugPort.println(PID[ZDAMPENING].D); |
306 |
#else |
307 |
DebugPort.print('0'); |
308 |
comma(); |
309 |
DebugPort.print('0'); |
310 |
comma(); |
311 |
DebugPort.print('0'); |
312 |
comma(); |
313 |
DebugPort.print('0'); |
314 |
comma(); |
315 |
DebugPort.print('0'); |
316 |
comma(); |
317 |
DebugPort.print('0'); |
318 |
comma(); |
319 |
DebugPort.print('0'); |
320 |
comma(); |
321 |
DebugPort.print('0'); |
322 |
comma(); |
323 |
DebugPort.println('0'); |
324 |
#endif |
325 |
queryType = 'X'; |
326 |
break; |
327 |
case 'L': // Send data filtering values |
328 |
DebugPort.print(gyro.getSmoothFactor()); |
329 |
comma(); |
330 |
DebugPort.print(accel.getSmoothFactor()); |
331 |
comma(); |
332 |
DebugPort.println(timeConstant); |
333 |
// comma(); |
334 |
// Serial.println(flightMode, DEC); |
335 |
queryType = 'X'; |
336 |
break; |
337 |
case 'N': // Send transmitter smoothing values |
338 |
DebugPort.print(receiver.getXmitFactor()); |
339 |
comma(); |
340 |
for (axis = ROLL; axis < AUX; axis++) { |
341 |
DebugPort.print(receiver.getSmoothFactor(axis)); |
342 |
comma(); |
343 |
} |
344 |
DebugPort.println(receiver.getSmoothFactor(AUX)); |
345 |
queryType = 'X'; |
346 |
break; |
347 |
case 'P': // Send transmitter calibration data |
348 |
for (axis = ROLL; axis < AUX; axis++) { |
349 |
DebugPort.print(receiver.getTransmitterSlope(axis)); |
350 |
comma(); |
351 |
DebugPort.print(receiver.getTransmitterOffset(axis)); |
352 |
comma(); |
353 |
} |
354 |
DebugPort.print(receiver.getTransmitterSlope(AUX)); |
355 |
comma(); |
356 |
DebugPort.println(receiver.getTransmitterOffset(AUX)); |
357 |
queryType = 'X'; |
358 |
break; |
359 |
case 'Q': // Send sensor data |
360 |
for (axis = ROLL; axis < LASTAXIS; axis++) { |
361 |
DebugPort.print(gyro.getData(axis)); |
362 |
comma(); |
363 |
} |
364 |
for (axis = ROLL; axis < LASTAXIS; axis++) { |
365 |
DebugPort.print(accel.getData(axis)); |
366 |
comma(); |
367 |
} |
368 |
for (axis = ROLL; axis < YAW; axis++) { |
369 |
DebugPort.print(levelAdjust[axis]); |
370 |
comma(); |
371 |
} |
372 |
DebugPort.print(flightAngle.getData(ROLL)); |
373 |
comma(); |
374 |
DebugPort.print(flightAngle.getData(PITCH)); |
375 |
DebugPort.println(); |
376 |
break; |
377 |
case 'R': // Send raw sensor data |
378 |
/*Serial.print(analogRead(ROLLRATEPIN)); |
379 |
comma(); |
380 |
Serial.print(analogRead(PITCHRATEPIN)); |
381 |
comma(); |
382 |
Serial.print(analogRead(YAWRATEPIN)); |
383 |
comma(); |
384 |
Serial.print(analogRead(ROLLACCELPIN)); |
385 |
comma(); |
386 |
Serial.print(analogRead(PITCHACCELPIN)); |
387 |
comma(); |
388 |
Serial.println(analogRead(ZACCELPIN));*/ |
389 |
//queryType = 'X'; |
390 |
break; |
391 |
case 'S': // Send all flight data |
392 |
DebugPort.print(deltaTime); |
393 |
comma(); |
394 |
for (axis = ROLL; axis < LASTAXIS; axis++) { |
395 |
DebugPort.print(gyro.getFlightData(axis)); |
396 |
comma(); |
397 |
} |
398 |
DebugPort.print(receiver.getData(THROTTLE)); |
399 |
comma(); |
400 |
for (axis = ROLL; axis < LASTAXIS; axis++) { |
401 |
DebugPort.print(motors.getMotorAxisCommand(axis)); |
402 |
comma(); |
403 |
} |
404 |
for (motor = FRONT; motor < LASTMOTOR; motor++) { |
405 |
DebugPort.print(motors.getMotorCommand(motor)); |
406 |
comma(); |
407 |
} |
408 |
for (axis = ROLL; axis < LASTAXIS; axis++) { |
409 |
DebugPort.print(accel.getFlightData(axis)); |
410 |
comma(); |
411 |
} |
412 |
DebugPort.print(armed, BIN); |
413 |
comma(); |
414 |
if (flightMode == STABLE) |
415 |
DebugPort.print(2000); |
416 |
if (flightMode == ACRO) |
417 |
DebugPort.print(1000); |
418 |
#ifdef HeadingMagHold |
419 |
comma(); |
420 |
DebugPort.print(compass.getAbsoluteHeading()); |
421 |
#else |
422 |
comma(); |
423 |
DebugPort.print('0'); |
424 |
#endif |
425 |
#ifdef AltitudeHold |
426 |
comma(); |
427 |
DebugPort.print(altitude.getData()); |
428 |
comma(); |
429 |
DebugPort.print(altitudeHold, DEC); |
430 |
#else |
431 |
comma(); |
432 |
DebugPort.print('0'); |
433 |
comma(); |
434 |
DebugPort.print('0'); |
435 |
#endif |
436 |
DebugPort.println(); |
437 |
break; |
438 |
case 'T': // Send processed transmitter values |
439 |
DebugPort.print(receiver.getXmitFactor()); |
440 |
comma(); |
441 |
for (axis = ROLL; axis < LASTAXIS; axis++) { |
442 |
DebugPort.print(receiver.getData(axis)); |
443 |
comma(); |
444 |
} |
445 |
for (axis = ROLL; axis < YAW; axis++) { |
446 |
DebugPort.print(levelAdjust[axis]); |
447 |
comma(); |
448 |
} |
449 |
DebugPort.print(motors.getMotorAxisCommand(ROLL)); |
450 |
comma(); |
451 |
DebugPort.print(motors.getMotorAxisCommand(PITCH)); |
452 |
comma(); |
453 |
DebugPort.println(motors.getMotorAxisCommand(YAW)); |
454 |
break; |
455 |
case 'U': // Send smoothed receiver with Transmitter Factor applied values |
456 |
for (channel = ROLL; channel < AUX; channel++) { |
457 |
DebugPort.print(receiver.getData(channel)); |
458 |
comma(); |
459 |
} |
460 |
DebugPort.println(receiver.getData(AUX)); |
461 |
break; |
462 |
case 'V': // Send receiver status |
463 |
for (channel = ROLL; channel < AUX; channel++) { |
464 |
DebugPort.print(receiver.getRaw(channel)); |
465 |
comma(); |
466 |
} |
467 |
DebugPort.println(receiver.getRaw(AUX)); |
468 |
break; |
469 |
case 'X': // Stop sending messages |
470 |
break; |
471 |
case 'Z': // Send heading |
472 |
DebugPort.print(receiver.getData(YAW)); |
473 |
comma(); |
474 |
DebugPort.print(headingHold); |
475 |
comma(); |
476 |
DebugPort.print(setHeading); |
477 |
comma(); |
478 |
DebugPort.println(relativeHeading); |
479 |
break; |
480 |
case '6': // Report remote commands |
481 |
for (motor = FRONT; motor < LEFT; motor++) { |
482 |
DebugPort.print(motors.getRemoteCommand(motor)); |
483 |
comma(); |
484 |
} |
485 |
DebugPort.println(motors.getRemoteCommand(LEFT)); |
486 |
break; |
487 |
case '!': // Send flight software version |
488 |
DebugPort.println(VERSION, 1); |
489 |
queryType = 'X'; |
490 |
break; |
491 |
case '#': // Send software configuration |
492 |
// Determine which hardware is used to define max/min sensor values for Configurator plots |
493 |
#if defined(AeroQuad_v1) |
494 |
DebugPort.print('0'); |
495 |
#elif defined(AeroQuadMega_v1) |
496 |
DebugPort.print('1'); |
497 |
#elif defined(AeroQuad_v18) |
498 |
DebugPort.print('2'); |
499 |
#elif defined(AeroQuadMega_v2) |
500 |
DebugPort.print('3'); |
501 |
#elif defined(AeroQuad_Wii) |
502 |
DebugPort.print('4'); |
503 |
#elif defined(AeroQuadMega_Wii) |
504 |
DebugPort.print('5'); |
505 |
#elif defined(ArduCopter) |
506 |
DebugPort.print('6'); |
507 |
#elif defined(Multipilot) |
508 |
DebugPort.print('7'); |
509 |
#elif defined(MultipilotI2C) |
510 |
DebugPort.print('8'); |
511 |
#endif |
512 |
comma(); |
513 |
// Determine which motor flight configuration for Configurator GUI |
514 |
#if defined(plusConfig) |
515 |
DebugPort.print('0'); |
516 |
#elif defined(XConfig) |
517 |
DebugPort.print('1'); |
518 |
#elif defined(HEXACOAXIAL) |
519 |
DebugPort.print('2'); |
520 |
#elif defined(HEXARADIAL) |
521 |
DebugPort.print('3'); |
522 |
#endif |
523 |
DebugPort.println(); |
524 |
queryType = 'X'; |
525 |
break; |
526 |
case 'e': // Send AREF value |
527 |
DebugPort.println(aref); |
528 |
queryType = 'X'; |
529 |
break; |
530 |
|
531 |
case 'p': // send altimeter |
532 |
#ifdef AltitudeHold |
533 |
comma(); |
534 |
DebugPort.print(altitude.getData()); |
535 |
comma(); |
536 |
DebugPort.print(altitudeHold, DEC); |
537 |
DebugPort.print('0'); |
538 |
#endif |
539 |
break; |
540 |
|
541 |
case 'q': // send sonar |
542 |
#ifdef SonarHold |
543 |
DebugPort.println(sonar.range); |
544 |
#endif |
545 |
break; |
546 |
} |
547 |
} |
548 |
|
549 |
// Used to read floating point values from the serial port |
550 |
float readFloatSerial() { |
551 |
byte index = 0; |
552 |
byte timeout = 0; |
553 |
char data[128] = ""; |
554 |
|
555 |
do { |
556 |
if (DebugPort.available() == 0) { |
557 |
delay(10); |
558 |
timeout++; |
559 |
} |
560 |
else { |
561 |
data[index] = DebugPort.read(); |
562 |
timeout = 0; |
563 |
index++; |
564 |
} |
565 |
} while ((data[constrain(index-1, 0, 128)] != ';') && (timeout < 5) && (index < 128)); |
566 |
return atof(data); |
567 |
} |
568 |
|
569 |
|
570 |
void comma() { |
571 |
DebugPort.print(','); |
572 |
} |
573 |
|
574 |
void printInt(int data) { |
575 |
byte msb, lsb; |
576 |
|
577 |
msb = data >> 8; |
578 |
lsb = data & 0xff; |
579 |
|
580 |
DebugPort.print(msb, BYTE); |
581 |
DebugPort.print(lsb, BYTE); |
582 |
} |