Statistics
| Branch: | Revision:

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
}