telemetry.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 void RxTelemetryPacket(uint8);
00024 void InitTelemetryPacket(void);
00025 void BuildTelemetryPacket(uint8);
00026 
00027 void SendPacketHeader(void);
00028 void SendPacketTrailer(void);
00029 
00030 void SendTelemetry(void);
00031 void SendCycle(void);
00032 void SendControl(void);
00033 void SendFlightPacket(void);
00034 void SendNavPacket(void);
00035 void SendControlPacket(void);
00036 void SendStatsPacket(void);
00037 void SendParamPacket(uint8, uint8);
00038 void SendParameters(uint8);
00039 void SendMinPacket(void);
00040 void SendArduStation(void);
00041 void SendPIDTuning(void);
00042 void SendCustom(void);
00043 void SensorTrace(void);
00044 
00045 uint8 UAVXCurrPacketTag;
00046 uint8 RxPacketLength, RxPacketByteCount;
00047 uint8 RxCheckSum;
00048 uint8 RxPacketTag, ReceivedPacketTag;
00049 uint8 PacketRxState;
00050 boolean CheckSumError, TelemetryPacketReceived;
00051 
00052 int16 RxLengthErrors, RxTypeErrors, RxCheckSumErrors;
00053 
00054 uint8 UAVXPacket[256];
00055 
00056 FILE *logfile = NULL;
00057 boolean EchoToLogFile = false;
00058 uint32 LogChars;
00059 boolean LogfileIsOpen = false;
00060 
00061 uint8 TxCheckSum;
00062 
00063 void InitTelemetryPacket(void) {
00064     RxPacketByteCount = 0;
00065     RxCheckSum = 0;
00066     RxPacketTag = UnknownPacketTag;
00067     RxPacketLength = 2; // set as minimum
00068     PacketRxState = WaitRxSentinel;
00069 } // InitTelemetryPacket
00070 
00071 void BuildTelemetryPacket(uint8 ch) {
00072     static boolean RxPacketError;
00073 
00074     UAVXPacket[RxPacketByteCount++] = ch;
00075 
00076     if (RxPacketByteCount == 1) {
00077         RxPacketTag = ch;
00078         PacketRxState=WaitRxBody;
00079     } else
00080         if (RxPacketByteCount == 2) {
00081             RxPacketLength = ch; // ignore
00082             PacketRxState = WaitRxBody;
00083         } else
00084             if (RxPacketByteCount >= (RxPacketLength + 3)) {
00085                 RxPacketError = CheckSumError = RxCheckSum != 0;
00086 
00087                 if (CheckSumError)
00088                     RxCheckSumErrors++;
00089 
00090                 if (!RxPacketError) {
00091                     TelemetryPacketReceived = true;
00092                     ReceivedPacketTag=RxPacketTag;
00093                 }
00094                 PacketRxState = WaitRxSentinel;
00095                 //   InitPollPacket();
00096             } else
00097                 PacketRxState = WaitRxBody;
00098 } // BuildTelemetryPacket
00099 
00100 void RxTelemetryPacket(uint8 ch) {
00101 
00102     RxCheckSum ^= ch;
00103     switch (PacketRxState) {
00104         case WaitRxSentinel:
00105             if (ch == SOH) {
00106                 InitTelemetryPacket();
00107                 CheckSumError = false;
00108                 PacketRxState = WaitRxBody;
00109             }
00110             break;
00111         case WaitRxBody:
00112             if (ch == ESC)
00113                 PacketRxState = WaitRxESC;
00114             else
00115                 if (ch == SOH) { // unexpected start of packet
00116                     RxLengthErrors++;
00117 
00118                     InitTelemetryPacket();
00119                     PacketRxState = WaitRxBody;
00120                 } else
00121                     if (ch == EOT) { // unexpected end of packet
00122                         RxLengthErrors++;
00123                         PacketRxState = WaitRxSentinel;
00124                     } else
00125                         BuildTelemetryPacket(ch);
00126             break;
00127         case WaitRxESC:
00128             BuildTelemetryPacket(ch);
00129             break;
00130         default:
00131             PacketRxState = WaitRxSentinel;
00132             break;
00133     }
00134 } // ParseTelemetryPacket
00135 
00136 
00137 #define NAV_STATS_INTERLEAVE    10
00138 static int8 StatsNavAlternate = 0;
00139 
00140 void CheckTelemetry(void) {
00141 
00142     // check incoming - switch on associated action
00143 
00144     // do routine telemetry Tx
00145     if ( mSClock() > mS[TelemetryUpdate] )
00146         switch ( P[TelemetryType] ) {
00147             case UAVXTelemetry:
00148                 mS[TelemetryUpdate] = mSClock() + UAVX_TEL_INTERVAL_MS;
00149                 SendCycle();
00150                 break;
00151             case ArduStationTelemetry:
00152                 mS[TelemetryUpdate] = mSClock() + UAVX_CONTROL_TEL_INTERVAL_MS;
00153                 SendArduStation();
00154                 break;
00155             case UAVXControlTelemetry:
00156                 mS[TelemetryUpdate] = mSClock() + UAVX_CONTROL_TEL_INTERVAL_MS;
00157                 SendControlPacket();
00158                 break;
00159             case UAVXMinTelemetry:
00160                 mS[TelemetryUpdate] = mSClock() + UAVX_MIN_TEL_INTERVAL_MS;
00161                 SendMinPacket();
00162                 break;
00163             case CustomTelemetry:
00164                 mS[TelemetryUpdate] = mSClock() + CUSTOM_TEL_INTERVAL_MS;
00165                 SendPIDTuning();//SendCustom();
00166                 break;
00167             case GPSTelemetry:
00168                 break;
00169         }
00170 
00171 } // CheckTelemetry
00172 
00173 void SendPacketHeader(void) {
00174 
00175     EchoToLogFile = true;
00176 
00177     TxChar(0xff); // synchronisation to "jolt" USART
00178     TxChar(SOH);
00179     TxCheckSum = 0;
00180 } // SendPacketHeader
00181 
00182 void SendPacketTrailer(void) {
00183     TxESCu8(TxCheckSum);
00184     TxChar(EOT);
00185 
00186     TxChar(CR);
00187     TxChar(LF);
00188 
00189     EchoToLogFile = false;
00190 } // SendPacketTrailer
00191 
00192 void ShowAttitude(void) {
00193     // Stick units
00194     TxESCi16(DesiredRoll);
00195     TxESCi16(DesiredPitch);
00196     TxESCi16(DesiredYaw);
00197 
00198     // mRadian and mG
00199     TxESCi16(Gyro[Roll] * 1000.0); // Rate
00200     TxESCi16(Gyro[Pitch] * 1000.0);
00201     TxESCi16(Gyro[Yaw] * 1000.0);
00202 
00203     TxESCi16(Angle[Roll] * 1000.0);
00204     TxESCi16(Angle[Pitch] * 1000.0);
00205     TxESCi16(Angle[Yaw] * 1000.0);
00206 
00207     TxESCi16(Acc[LR] * 1000.0);
00208     TxESCi16(Acc[BF] * 1000.0);
00209     TxESCi16(Acc[UD] * 1000.0);
00210 
00211 } // ShowAttitude
00212 
00213 void SendFlightPacket(void) {
00214     static int8 b;
00215 
00216     SendPacketHeader();
00217 
00218     TxESCu8(UAVXFlightPacketTag);
00219     TxESCu8(58 + TELEMETRY_FLAG_BYTES);
00220     for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
00221         TxESCu8(F.AllFlags[b]);
00222 
00223     TxESCu8(State);
00224 
00225     // dV, dA, mAH
00226     TxESCi16(BatteryVolts * 10.0); // to do scaling
00227     TxESCi16(BatteryCurrent * 10.0);
00228     TxESCi16(BatteryChargeUsedAH * 1000.0);
00229 
00230     TxESCi16(RCGlitches);
00231     TxESCi16(DesiredThrottle);
00232 
00233     ShowAttitude();
00234 
00235     TxESCi8((int8)Correction[LR]);
00236     TxESCi8((int8)Correction[BF]);
00237     TxESCi8((int8)AccAltComp);
00238     TxESCi8((int8)AltComp);
00239 
00240     for ( b = 0; b < 8; b++ )
00241         TxESCi16((int16)PWM[b]);
00242 
00243     TxESCi24(mSClock() - mS[StartTime]);
00244 
00245     SendPacketTrailer();
00246 } // SendFlightPacket
00247 
00248 void SendControlPacket(void) {
00249     static int8 b;
00250 
00251     SendPacketHeader();
00252 
00253     TxESCu8(UAVXControlPacketTag);
00254     TxESCu8(48);
00255 
00256     TxESCi16(DesiredThrottle);
00257 
00258     ShowAttitude();
00259 
00260     TxESCu8(UAVXAirframe | 0x80);
00261 
00262     for ( b = 0; b < 8; b++ ) // motor/servo channels
00263         TxESCi16((int16)PWM[b]);
00264 
00265     TxESCi24( time(NULL) );
00266 
00267     SendPacketTrailer();
00268 
00269 } // SendControlPacket
00270 
00271 void SendNavPacket(void) {
00272     SendPacketHeader();
00273 
00274     TxESCu8(UAVXNavPacketTag);
00275     TxESCu8(59);
00276 
00277     TxESCu8(NavState);
00278     TxESCu8(FailState);
00279     TxESCu8(GPSNoOfSats);
00280     TxESCu8(GPSFix);
00281 
00282     TxESCu8(CurrWP);
00283 
00284     TxESCi16(ROC * 10.0 );                         // dm/S
00285     TxESCi24(BaroRelAltitude * 10.0);
00286 
00287     TxESCi16(GPSHeading * 1000.0);
00288     TxESCi16(RangefinderAltitude * 100.0);          // cm
00289 
00290     TxESCi16(GPSHDilute);
00291     TxESCi16(Heading * 1000.0);
00292     TxESCi16(WayHeading * 1000.0);
00293 
00294     TxESCi16(GPSVel * 10.0);
00295     TxESCi16(0);                                   // was GPSROC;
00296 
00297     TxESCi24(GPSRelAltitude * 10.0);               // dm
00298     TxESCi32(GPSLatitude);                         // 5 decimal minute units
00299     TxESCi32(GPSLongitude);
00300 
00301     TxESCi24(DesiredAltitude * 10.0 );
00302     TxESCi32(DesiredLatitude);
00303     TxESCi32(DesiredLongitude);
00304 
00305     TxESCi24(mS[NavStateTimeout] - mSClock());    // mS
00306 
00307     TxESCi16(AmbientTemperature.i16);             // 0.1C
00308     TxESCi32( time(NULL) );                       //GPSSeconds);
00309 
00310     TxESCu8(NavSensitivity);
00311     TxESCi8(NavCorr[Roll]);
00312     TxESCi8(NavCorr[Pitch]);
00313     TxESCi8(NavCorr[Yaw]);
00314 
00315     SendPacketTrailer();
00316 
00317 } // SendNavPacket
00318 
00319 void SendStickPacket(void) {
00320     static uint8 c;
00321 
00322     SendPacketHeader();
00323 
00324     TxESCu8(UAVXStickPacketTag);
00325     TxESCu8( 1 + RC_CONTROLS * 2 );
00326     TxESCu8(RC_CONTROLS);
00327     for ( c = 0; c < RC_CONTROLS; c++ )
00328         TxESCi16(RC[c]);
00329 
00330     SendPacketTrailer();
00331 } // SendStickPacket
00332 
00333 void SendStatsPacket(void) {
00334 
00335     static uint8 i;
00336 
00337     SendPacketHeader();
00338 
00339     TxESCu8(UAVXStatsPacketTag);
00340     TxESCu8(MAX_STATS * 2 + 2);
00341 
00342     for ( i = 0; i < MAX_STATS ; i++)
00343         TxESCi16(Stats[i]);
00344 
00345     TxESCu8(UAVXAirframe | 0x80);
00346     TxESCu8(Orientation);
00347 
00348     SendPacketTrailer();
00349 
00350 } // SendStatsPacket
00351 
00352 void SendMinPacket(void) {
00353     static int8 b;
00354 
00355     SendPacketHeader();
00356 
00357     TxESCu8(UAVXMinPacketTag);
00358     TxESCu8(33 + TELEMETRY_FLAG_BYTES);
00359     for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
00360         TxESCu8(F.AllFlags[b]);
00361 
00362     TxESCu8(State);
00363     TxESCu8(NavState);
00364     TxESCu8(FailState);
00365 
00366     TxESCi16(BatteryVolts * 10.0); // to do scaling
00367     TxESCi16(BatteryCurrent * 10.0);
00368     TxESCi16(BatteryChargeUsedAH * 1000.0);
00369 
00370     TxESCi16(Angle[Roll] * 1000.0);
00371     TxESCi16(Angle[Pitch] * 1000.0);
00372 
00373     TxESCi24(BaroRelAltitude * 10.0);
00374     TxESCi16(RangefinderAltitude * 100.0);                 // cm
00375 
00376     TxESCi16(Heading * 1000.0);
00377 
00378     TxESCi32(GPSLatitude);                         // 5 decimal minute units
00379     TxESCi32(GPSLongitude);
00380 
00381     TxESCu8(UAVXAirframe | 0x80 ); // ARM has top bit set
00382     TxESCu8(Orientation);
00383 
00384     TxESCi24(mSClock() - mS[StartTime]);
00385 
00386     SendPacketTrailer();
00387 
00388 } // SendMinPacket
00389 
00390 void SendParamPacket(uint8 s, uint8 p) {
00391 
00392     SendPacketHeader();
00393 
00394     TxESCu8(UAVXArmParamPacketTag);
00395     TxESCu8(6);
00396     TxESCu8(s);
00397     TxESCu8(p);
00398     TxESCi32(K[p] * 1000.0 );
00399 
00400     SendPacketTrailer();
00401 
00402 } // SendParamPacket
00403 
00404 void SendParameters(uint8 s) {
00405     static uint8 p;
00406 
00407     for ( p = 0; p < MAX_PARAMETERS; p++ )
00408         SendParamPacket(s, p);
00409     SendParamPacket(0, MAX_PARAMETERS);
00410 
00411 } // SendParameters
00412 
00413 void SendCycle(void) {
00414 
00415     switch ( UAVXCurrPacketTag ) {
00416         case UAVXFlightPacketTag:
00417             SendFlightPacket();
00418 
00419             UAVXCurrPacketTag = UAVXNavPacketTag;
00420             break;
00421 
00422         case UAVXNavPacketTag:
00423             if ( ++StatsNavAlternate < NAV_STATS_INTERLEAVE)
00424                 SendNavPacket();
00425             else {
00426                 SendStatsPacket();
00427                 StatsNavAlternate = 0;
00428             }
00429 
00430             UAVXCurrPacketTag = UAVXFlightPacketTag;
00431             break;
00432 
00433         default:
00434             UAVXCurrPacketTag = UAVXFlightPacketTag;
00435             break;
00436     }
00437 
00438 } // SendCycle
00439 
00440 void SendArduStation(void) {
00441 
00442     static int8 Count = 0;
00443     /*
00444     Definitions of the low rate telemetry (1Hz):
00445     LAT: Latitude
00446     LON: Longitude
00447     SPD: Speed over ground from GPS
00448     CRT: Climb Rate in M/S
00449     ALT: Altitude in meters
00450     ALH: The altitude is trying to hold
00451     CRS: Course over ground in degrees.
00452     BER: Bearing is the heading you want to go
00453     WPN: Waypoint number, where WP0 is home.
00454     DST: Distance from Waypoint
00455     BTV: Battery Voltage.
00456     RSP: Roll setpoint used to debug, (not displayed here).
00457 
00458     Definitions of the high rate telemetry (~4Hz):
00459     ASP: Airspeed, right now is the raw data.
00460     TTH: Throttle in 100% the autopilot is applying.
00461     RLL: Roll in degrees + is right - is left
00462     PCH: Pitch in degrees
00463     SST: Switch Status, used for debugging, but is disabled in the current version.
00464     */
00465 
00466     if ( ++Count == 4 ) {
00467         TxString("!!!");
00468         TxString("LAT:");
00469         TxVal32(GPSLatitude / 6000, 3, 0);
00470         TxString(",LON:");
00471         TxVal32(GPSLongitude / 6000, 3, 0);
00472         TxString(",ALT:");
00473         TxVal32(Altitude / 10,0,0);
00474         TxString(",ALH:");
00475         TxVal32(DesiredAltitude / 10, 0, 0);
00476         TxString(",CRT:");
00477         TxVal32(ROC / 100, 0, 0);
00478         TxString(",CRS:");
00479         TxVal32(Heading * RADDEG, 0, 0); // scaling to degrees?
00480         TxString(",BER:");
00481         TxVal32(WayHeading * RADDEG, 0, 0);
00482         TxString(",SPD:");
00483         TxVal32(GPSVel, 0, 0);
00484         TxString(",WPN:");
00485         TxVal32(CurrWP,0,0);
00486         TxString(",DST:");
00487         TxVal32(0, 0, 0); // distance to WP
00488         TxString(",BTV:");
00489         TxVal32((BatteryVoltsADC * 61)/205, 1, 0);
00490         TxString(",RSP:");
00491         TxVal32(DesiredRoll, 0, 0);
00492 
00493         Count = 0;
00494     } else {
00495         TxString("+++");
00496         TxString("ASP:");
00497         TxVal32(GPSVel / 100, 0, 0);
00498         TxString(",RLL:");
00499         TxVal32(Angle[Roll] / 35, 0, 0); // scale to degrees?
00500         TxString(",PCH:");
00501         TxVal32(Angle[Pitch] / 35, 0, 0);
00502         TxString(",THH:");
00503         TxVal32( ((int24)DesiredThrottle * 100L) / RC_MAXIMUM, 0, 0);
00504     }
00505 
00506     TxString(",***\r\n");
00507 
00508 } // SendArduStation
00509 
00510 void SendPIDTuning(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
00511 
00512     // Fixed to roll axis
00513 
00514     SendPacketHeader();
00515 
00516     TxESCu8(UAVXCustomPacketTag);
00517     TxESCu8(1 + 10);
00518     TxESCu8(5); // how many 16bit elements
00519 /*
00520     TxESCi16(DesiredRoll);
00521     TxESCi16(PWM[RightC]);
00522 
00523     TxESCi16(Gyro[Roll] * 1000.0);
00524     TxESCi16(Acc[Roll] * 1000.0);
00525     TxESCi16(Angle[Roll] * 1000.0 );
00526     */
00527     
00528     TxESCi16(DesiredYaw);
00529     TxESCi16(PWM[RightC]);
00530 
00531     TxESCi16(Gyro[Yaw] * 1000.0);
00532     TxESCi16(HE * 1000.0);
00533     TxESCi16(Angle[Yaw] * 1000.0 );
00534 
00535     SendPacketTrailer();
00536 
00537 } // SendPIDTuning
00538 
00539 
00540 void SendCustom(void) { // user defined telemetry human readable OK for small amounts of data < 1mS
00541 
00542     static uint8 s;
00543 
00544     // always a vector of int16;
00545     SendPacketHeader();
00546 
00547     TxESCu8(UAVXCustomPacketTag);
00548     TxESCu8(1 + 8 + MaxAttitudeScheme * 2);
00549     TxESCu8(4 + MaxAttitudeScheme ); // how many 16bit elements
00550     TxESCi16(AttitudeMethod);
00551 
00552     TxESCi16(0); // spare
00553 
00554     TxESCi16(Gyro[Roll] * 1000.0);
00555     TxESCi16(Acc[LR] * 1000.0);
00556 
00557     for ( s = 0; s < MaxAttitudeScheme; s++ )
00558         TxESCi16( EstAngle[Roll][s] * 1000.0 );
00559 
00560     SendPacketTrailer();
00561 
00562 } // SendCustom
00563 
00564 void SensorTrace(void) {
00565 #ifdef TESTING
00566 
00567     if ( DesiredThrottle > 20 ) {
00568         EchoToLogFile = false; // direct to USART
00569 
00570         TxValH16(((int24)Heading * 180)/MILLIPI);
00571         TxChar(';');
00572 
00573         TxValH16(BaroRelAltitude);
00574         TxChar(';');
00575         TxValH16(RangefinderAltitude);
00576         TxChar(';');
00577         TxValH16(0);
00578         TxChar(';');
00579 
00580         TxValH16(DesiredThrottle);
00581         TxChar(';');
00582         TxValH16(DesiredRoll);
00583         TxChar(';');
00584         TxValH16(DesiredPitch);
00585         TxChar(';');
00586         TxValH16(DesiredYaw);
00587         TxChar(';');
00588 
00589         TxValH16(Rate[Roll]);
00590         TxChar(';');
00591         TxValH16(Rate[Pitch]);
00592         TxChar(';');
00593         TxValH16(Rate[Yaw]);
00594         TxChar(';');
00595 
00596         TxValH16(Angle[Roll]);
00597         TxChar(';');
00598         TxValH16(Angle[Pitch]);
00599         TxChar(';');
00600         TxValH16(Angle[Yaw]);
00601         TxChar(';');
00602 
00603         TxValH16(Acc[LR]);
00604         TxChar(';');
00605         TxValH16(Acc[FB]);
00606         TxChar(';');
00607         TxValH16(Acc[DU]);
00608         TxChar(';');
00609 
00610         TxValH16(Correction[LR]);
00611         TxChar(';');
00612         TxValH16(Correction[FB]);
00613         TxChar(';');
00614         TxValH16(AccAltComp);
00615         TxChar(';');
00616         TxValH16(AltComp);
00617         TxChar(';');
00618         TxNextLine();
00619     }
00620 #endif // TESTING
00621 } // SensorTrace