Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

telemetry.c@0:62a1c91a859a, 2011-02-18 (annotated)

Committer:
gke
Date:
Fri Feb 18 22:28:05 2011 +0000
Revision:
0:62a1c91a859a
Child:
1:1e3318a30ddd
First release

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1 // ===============================================================================================
gke 0:62a1c91a859a 2 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 3 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 4 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 0:62a1c91a859a 5 // = http://code.google.com/p/uavp-mods/ http://uavp.ch =
gke 0:62a1c91a859a 6 // ===============================================================================================
gke 0:62a1c91a859a 7
gke 0:62a1c91a859a 8 // This is part of UAVXArm.
gke 0:62a1c91a859a 9
gke 0:62a1c91a859a 10 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 11 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 12 // License, or (at your option) any later version.
gke 0:62a1c91a859a 13
gke 0:62a1c91a859a 14 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 15 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 16 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 17
gke 0:62a1c91a859a 18 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 19 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 20
gke 0:62a1c91a859a 21 #include "UAVXArm.h"
gke 0:62a1c91a859a 22
gke 0:62a1c91a859a 23 void RxTelemetryPacket(uint8);
gke 0:62a1c91a859a 24 void InitTelemetryPacket(void);
gke 0:62a1c91a859a 25 void BuildTelemetryPacket(uint8);
gke 0:62a1c91a859a 26
gke 0:62a1c91a859a 27 void SendPacketHeader(void);
gke 0:62a1c91a859a 28 void SendPacketTrailer(void);
gke 0:62a1c91a859a 29
gke 0:62a1c91a859a 30 void SendTelemetry(void);
gke 0:62a1c91a859a 31 void SendCycle(void);
gke 0:62a1c91a859a 32 void SendControl(void);
gke 0:62a1c91a859a 33 void SendFlightPacket(void);
gke 0:62a1c91a859a 34 void SendNavPacket(void);
gke 0:62a1c91a859a 35 void SendControlPacket(void);
gke 0:62a1c91a859a 36 void SendStatsPacket(void);
gke 0:62a1c91a859a 37 void SendParamPacket(uint8);
gke 0:62a1c91a859a 38 void SendMinPacket(void);
gke 0:62a1c91a859a 39 void SendArduStation(void);
gke 0:62a1c91a859a 40 void SendCustom(void);
gke 0:62a1c91a859a 41 void SensorTrace(void);
gke 0:62a1c91a859a 42
gke 0:62a1c91a859a 43 uint8 UAVXCurrPacketTag;
gke 0:62a1c91a859a 44 uint8 RxPacketLength, RxPacketByteCount;
gke 0:62a1c91a859a 45 uint8 RxCheckSum;
gke 0:62a1c91a859a 46 uint8 RxPacketTag, ReceivedPacketTag;
gke 0:62a1c91a859a 47 uint8 PacketRxState;
gke 0:62a1c91a859a 48 boolean CheckSumError, TelemetryPacketReceived;
gke 0:62a1c91a859a 49
gke 0:62a1c91a859a 50 int16 RxLengthErrors, RxTypeErrors, RxCheckSumErrors;
gke 0:62a1c91a859a 51
gke 0:62a1c91a859a 52 uint8 UAVXPacket[256];
gke 0:62a1c91a859a 53
gke 0:62a1c91a859a 54 FILE *logfile = NULL;
gke 0:62a1c91a859a 55 boolean EchoToLogFile = false;
gke 0:62a1c91a859a 56 uint32 LogChars;
gke 0:62a1c91a859a 57 boolean LogfileIsOpen = false;
gke 0:62a1c91a859a 58
gke 0:62a1c91a859a 59 uint8 TxCheckSum;
gke 0:62a1c91a859a 60
gke 0:62a1c91a859a 61 void InitTelemetryPacket(void) {
gke 0:62a1c91a859a 62 RxPacketByteCount = 0;
gke 0:62a1c91a859a 63 RxCheckSum = 0;
gke 0:62a1c91a859a 64 RxPacketTag = UnknownPacketTag;
gke 0:62a1c91a859a 65 RxPacketLength = 2; // set as minimum
gke 0:62a1c91a859a 66 PacketRxState = WaitRxSentinel;
gke 0:62a1c91a859a 67 } // InitTelemetryPacket
gke 0:62a1c91a859a 68
gke 0:62a1c91a859a 69 void BuildTelemetryPacket(uint8 ch) {
gke 0:62a1c91a859a 70 static boolean RxPacketError;
gke 0:62a1c91a859a 71
gke 0:62a1c91a859a 72 UAVXPacket[RxPacketByteCount++] = ch;
gke 0:62a1c91a859a 73
gke 0:62a1c91a859a 74 if (RxPacketByteCount == 1) {
gke 0:62a1c91a859a 75 RxPacketTag = ch;
gke 0:62a1c91a859a 76 PacketRxState=WaitRxBody;
gke 0:62a1c91a859a 77 } else
gke 0:62a1c91a859a 78 if (RxPacketByteCount == 2) {
gke 0:62a1c91a859a 79 RxPacketLength = ch; // ignore
gke 0:62a1c91a859a 80 PacketRxState = WaitRxBody;
gke 0:62a1c91a859a 81 } else
gke 0:62a1c91a859a 82 if (RxPacketByteCount >= (RxPacketLength + 3)) {
gke 0:62a1c91a859a 83 RxPacketError = CheckSumError = RxCheckSum != 0;
gke 0:62a1c91a859a 84
gke 0:62a1c91a859a 85 if (CheckSumError)
gke 0:62a1c91a859a 86 RxCheckSumErrors++;
gke 0:62a1c91a859a 87
gke 0:62a1c91a859a 88 if (!RxPacketError) {
gke 0:62a1c91a859a 89 TelemetryPacketReceived = true;
gke 0:62a1c91a859a 90 ReceivedPacketTag=RxPacketTag;
gke 0:62a1c91a859a 91 }
gke 0:62a1c91a859a 92 PacketRxState = WaitRxSentinel;
gke 0:62a1c91a859a 93 // InitPollPacket();
gke 0:62a1c91a859a 94 } else
gke 0:62a1c91a859a 95 PacketRxState = WaitRxBody;
gke 0:62a1c91a859a 96 } // BuildTelemetryPacket
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98 void RxTelemetryPacket(uint8 ch) {
gke 0:62a1c91a859a 99
gke 0:62a1c91a859a 100 RxCheckSum ^= ch;
gke 0:62a1c91a859a 101 switch (PacketRxState) {
gke 0:62a1c91a859a 102 case WaitRxSentinel:
gke 0:62a1c91a859a 103 if (ch == SOH) {
gke 0:62a1c91a859a 104 InitTelemetryPacket();
gke 0:62a1c91a859a 105 CheckSumError = false;
gke 0:62a1c91a859a 106 PacketRxState = WaitRxBody;
gke 0:62a1c91a859a 107 }
gke 0:62a1c91a859a 108 break;
gke 0:62a1c91a859a 109 case WaitRxBody:
gke 0:62a1c91a859a 110 if (ch == ESC)
gke 0:62a1c91a859a 111 PacketRxState = WaitRxESC;
gke 0:62a1c91a859a 112 else
gke 0:62a1c91a859a 113 if (ch == SOH) { // unexpected start of packet
gke 0:62a1c91a859a 114 RxLengthErrors++;
gke 0:62a1c91a859a 115
gke 0:62a1c91a859a 116 InitTelemetryPacket();
gke 0:62a1c91a859a 117 PacketRxState = WaitRxBody;
gke 0:62a1c91a859a 118 } else
gke 0:62a1c91a859a 119 if (ch == EOT) { // unexpected end of packet
gke 0:62a1c91a859a 120 RxLengthErrors++;
gke 0:62a1c91a859a 121 PacketRxState = WaitRxSentinel;
gke 0:62a1c91a859a 122 } else
gke 0:62a1c91a859a 123 BuildTelemetryPacket(ch);
gke 0:62a1c91a859a 124 break;
gke 0:62a1c91a859a 125 case WaitRxESC:
gke 0:62a1c91a859a 126 BuildTelemetryPacket(ch);
gke 0:62a1c91a859a 127 break;
gke 0:62a1c91a859a 128 default:
gke 0:62a1c91a859a 129 PacketRxState = WaitRxSentinel;
gke 0:62a1c91a859a 130 break;
gke 0:62a1c91a859a 131 }
gke 0:62a1c91a859a 132 } // ParseTelemetryPacket
gke 0:62a1c91a859a 133
gke 0:62a1c91a859a 134
gke 0:62a1c91a859a 135 #define NAV_STATS_INTERLEAVE 10
gke 0:62a1c91a859a 136 static int8 StatsNavAlternate = 0;
gke 0:62a1c91a859a 137
gke 0:62a1c91a859a 138 void CheckTelemetry(void) {
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 // check incoming - switch on associated action
gke 0:62a1c91a859a 141
gke 0:62a1c91a859a 142 // do routine telemetry Tx
gke 0:62a1c91a859a 143 if ( mSClock() > mS[TelemetryUpdate] )
gke 0:62a1c91a859a 144 switch ( P[TelemetryType] ) {
gke 0:62a1c91a859a 145 case UAVXTelemetry:
gke 0:62a1c91a859a 146 mS[TelemetryUpdate] = mSClock() + UAVX_TEL_INTERVAL_MS;
gke 0:62a1c91a859a 147 SendCycle();
gke 0:62a1c91a859a 148 break;
gke 0:62a1c91a859a 149 case ArduStationTelemetry:
gke 0:62a1c91a859a 150 mS[TelemetryUpdate] = mSClock() + UAVX_CONTROL_TEL_INTERVAL_MS;
gke 0:62a1c91a859a 151 SendArduStation();
gke 0:62a1c91a859a 152 break;
gke 0:62a1c91a859a 153 case UAVXControlTelemetry:
gke 0:62a1c91a859a 154 mS[TelemetryUpdate] = mSClock() + UAVX_CONTROL_TEL_INTERVAL_MS;
gke 0:62a1c91a859a 155 SendControlPacket();
gke 0:62a1c91a859a 156 break;
gke 0:62a1c91a859a 157 case UAVXMinTelemetry:
gke 0:62a1c91a859a 158 mS[TelemetryUpdate] = mSClock() + UAVX_MIN_TEL_INTERVAL_MS;
gke 0:62a1c91a859a 159 SendMinPacket();
gke 0:62a1c91a859a 160 break;
gke 0:62a1c91a859a 161 case CustomTelemetry:
gke 0:62a1c91a859a 162 mS[TelemetryUpdate] = mSClock() + CUSTOM_TEL_INTERVAL_MS;
gke 0:62a1c91a859a 163 SendCustom();
gke 0:62a1c91a859a 164 break;
gke 0:62a1c91a859a 165 case GPSTelemetry:
gke 0:62a1c91a859a 166 break;
gke 0:62a1c91a859a 167 }
gke 0:62a1c91a859a 168
gke 0:62a1c91a859a 169 } // CheckTelemetry
gke 0:62a1c91a859a 170
gke 0:62a1c91a859a 171 void SendPacketHeader(void) {
gke 0:62a1c91a859a 172 static int8 b;
gke 0:62a1c91a859a 173
gke 0:62a1c91a859a 174 EchoToLogFile = true;
gke 0:62a1c91a859a 175
gke 0:62a1c91a859a 176 #ifdef TELEMETRY_PREAMBLE
gke 0:62a1c91a859a 177 for (b=10;b;b--)
gke 0:62a1c91a859a 178 TxChar(0x55);
gke 0:62a1c91a859a 179 #endif // TELEMETRY_PREAMBLE
gke 0:62a1c91a859a 180
gke 0:62a1c91a859a 181 TxChar(0xff); // synchronisation to "jolt" USART
gke 0:62a1c91a859a 182 TxChar(SOH);
gke 0:62a1c91a859a 183 TxCheckSum = 0;
gke 0:62a1c91a859a 184 } // SendPacketHeader
gke 0:62a1c91a859a 185
gke 0:62a1c91a859a 186 void SendPacketTrailer(void) {
gke 0:62a1c91a859a 187 TxESCu8(TxCheckSum);
gke 0:62a1c91a859a 188 TxChar(EOT);
gke 0:62a1c91a859a 189
gke 0:62a1c91a859a 190 TxChar(CR);
gke 0:62a1c91a859a 191 TxChar(LF);
gke 0:62a1c91a859a 192
gke 0:62a1c91a859a 193 EchoToLogFile = false;
gke 0:62a1c91a859a 194 } // SendPacketTrailer
gke 0:62a1c91a859a 195
gke 0:62a1c91a859a 196 void ShowAttitude(void) {
gke 0:62a1c91a859a 197 // Stick units
gke 0:62a1c91a859a 198 TxESCi16(DesiredRoll);
gke 0:62a1c91a859a 199 TxESCi16(DesiredPitch);
gke 0:62a1c91a859a 200 TxESCi16(DesiredYaw);
gke 0:62a1c91a859a 201
gke 0:62a1c91a859a 202 // mRadian and mG
gke 0:62a1c91a859a 203 TxESCi16(Gyro[Roll] * 1000.0); // Rate
gke 0:62a1c91a859a 204 TxESCi16(Gyro[Pitch] * 1000.0);
gke 0:62a1c91a859a 205 TxESCi16(Gyro[Yaw] * 1000.0);
gke 0:62a1c91a859a 206
gke 0:62a1c91a859a 207 TxESCi16(Angle[Roll] * 1000.0);
gke 0:62a1c91a859a 208 TxESCi16(Angle[Pitch] * 1000.0);
gke 0:62a1c91a859a 209 TxESCi16(Angle[Yaw] * 1000.0);
gke 0:62a1c91a859a 210
gke 0:62a1c91a859a 211 TxESCi16(Acc[LR] * 1000.0);
gke 0:62a1c91a859a 212 TxESCi16(Acc[BF] * 1000.0);
gke 0:62a1c91a859a 213 TxESCi16(Acc[UD] * 1000.0);
gke 0:62a1c91a859a 214 } // ShowAttitude
gke 0:62a1c91a859a 215
gke 0:62a1c91a859a 216 void SendFlightPacket(void) {
gke 0:62a1c91a859a 217 static int8 b;
gke 0:62a1c91a859a 218
gke 0:62a1c91a859a 219 SendPacketHeader();
gke 0:62a1c91a859a 220
gke 0:62a1c91a859a 221 TxESCu8(UAVXFlightPacketTag);
gke 0:62a1c91a859a 222 TxESCu8(48 + TELEMETRY_FLAG_BYTES);
gke 0:62a1c91a859a 223 for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
gke 0:62a1c91a859a 224 TxESCu8(F.AllFlags[b]);
gke 0:62a1c91a859a 225
gke 0:62a1c91a859a 226 TxESCu8(State);
gke 0:62a1c91a859a 227
gke 0:62a1c91a859a 228 // dV, dA, mAH
gke 0:62a1c91a859a 229 TxESCi16(BatteryVolts * 10.0); // to do scaling
gke 0:62a1c91a859a 230 TxESCi16(BatteryCurrent * 10.0);
gke 0:62a1c91a859a 231 TxESCi16(BatteryChargeUsedAH * 1000.0);
gke 0:62a1c91a859a 232
gke 0:62a1c91a859a 233 TxESCi16(RCGlitches);
gke 0:62a1c91a859a 234 TxESCi16(DesiredThrottle);
gke 0:62a1c91a859a 235
gke 0:62a1c91a859a 236 ShowAttitude();
gke 0:62a1c91a859a 237
gke 0:62a1c91a859a 238 TxESCi8((int8)Comp[LR]);
gke 0:62a1c91a859a 239 TxESCi8((int8)Comp[BF]);
gke 0:62a1c91a859a 240 TxESCi8((int8)Comp[UD]);
gke 0:62a1c91a859a 241 TxESCi8((int8)Comp[Alt]);
gke 0:62a1c91a859a 242
gke 0:62a1c91a859a 243 for ( b = 0; b < 4; b++ )
gke 0:62a1c91a859a 244 TxESCu8((uint8)PWM[b]);
gke 0:62a1c91a859a 245
gke 0:62a1c91a859a 246 TxESCu8((uint8)PWM[CamRollC]);
gke 0:62a1c91a859a 247 TxESCu8((uint8)PWM[CamPitchC]);
gke 0:62a1c91a859a 248
gke 0:62a1c91a859a 249 TxESCi24(mSClock() - mS[StartTime]);
gke 0:62a1c91a859a 250
gke 0:62a1c91a859a 251 SendPacketTrailer();
gke 0:62a1c91a859a 252 } // SendFlightPacket
gke 0:62a1c91a859a 253
gke 0:62a1c91a859a 254 void SendControlPacket(void) {
gke 0:62a1c91a859a 255 static int8 b;
gke 0:62a1c91a859a 256
gke 0:62a1c91a859a 257 SendPacketHeader();
gke 0:62a1c91a859a 258
gke 0:62a1c91a859a 259 TxESCu8(UAVXControlPacketTag);
gke 0:62a1c91a859a 260 TxESCu8(35);
gke 0:62a1c91a859a 261
gke 0:62a1c91a859a 262 TxESCi16(DesiredThrottle);
gke 0:62a1c91a859a 263
gke 0:62a1c91a859a 264 ShowAttitude();
gke 0:62a1c91a859a 265
gke 0:62a1c91a859a 266 for ( b = 0; b < 6; b++ ) // motor/servo channels
gke 0:62a1c91a859a 267 TxESCu8((uint8)PWM[b]);
gke 0:62a1c91a859a 268
gke 0:62a1c91a859a 269 TxESCi24( time(NULL) );
gke 0:62a1c91a859a 270
gke 0:62a1c91a859a 271 SendPacketTrailer();
gke 0:62a1c91a859a 272
gke 0:62a1c91a859a 273 } // SendControlPacket
gke 0:62a1c91a859a 274
gke 0:62a1c91a859a 275 void SendNavPacket(void) {
gke 0:62a1c91a859a 276 SendPacketHeader();
gke 0:62a1c91a859a 277
gke 0:62a1c91a859a 278 TxESCu8(UAVXNavPacketTag);
gke 0:62a1c91a859a 279 TxESCu8(59);
gke 0:62a1c91a859a 280
gke 0:62a1c91a859a 281 TxESCu8(NavState);
gke 0:62a1c91a859a 282 TxESCu8(FailState);
gke 0:62a1c91a859a 283 TxESCu8(GPSNoOfSats);
gke 0:62a1c91a859a 284 TxESCu8(GPSFix);
gke 0:62a1c91a859a 285
gke 0:62a1c91a859a 286 TxESCu8(CurrWP);
gke 0:62a1c91a859a 287
gke 0:62a1c91a859a 288 TxESCi16(ROC * 10.0 ); // dm/S
gke 0:62a1c91a859a 289 TxESCi24(BaroRelAltitude * 10.0);
gke 0:62a1c91a859a 290
gke 0:62a1c91a859a 291 TxESCi16(GPSHeading * 1000.0);
gke 0:62a1c91a859a 292 TxESCi16(RangefinderAltitude * 100.0); // cm
gke 0:62a1c91a859a 293
gke 0:62a1c91a859a 294 TxESCi16(GPSHDilute);
gke 0:62a1c91a859a 295 TxESCi16(Heading * 1000.0);
gke 0:62a1c91a859a 296 TxESCi16(WayHeading * 1000.0);
gke 0:62a1c91a859a 297
gke 0:62a1c91a859a 298 TxESCi16(GPSVel * 10.0);
gke 0:62a1c91a859a 299 TxESCi16(0); // was GPSROC;
gke 0:62a1c91a859a 300
gke 0:62a1c91a859a 301 TxESCi24(GPSRelAltitude * 10.0); // dm
gke 0:62a1c91a859a 302 TxESCi32(GPSLatitude); // 5 decimal minute units
gke 0:62a1c91a859a 303 TxESCi32(GPSLongitude);
gke 0:62a1c91a859a 304
gke 0:62a1c91a859a 305 TxESCi24(DesiredAltitude * 10.0 );
gke 0:62a1c91a859a 306 TxESCi32(DesiredLatitude);
gke 0:62a1c91a859a 307 TxESCi32(DesiredLongitude);
gke 0:62a1c91a859a 308
gke 0:62a1c91a859a 309 TxESCi24(mS[NavStateTimeout] - mSClock()); // mS
gke 0:62a1c91a859a 310
gke 0:62a1c91a859a 311 TxESCi16(AmbientTemperature.i16); // 0.1C
gke 0:62a1c91a859a 312 TxESCi32( time(NULL) ); //GPSSeconds);
gke 0:62a1c91a859a 313
gke 0:62a1c91a859a 314 TxESCu8(NavSensitivity);
gke 0:62a1c91a859a 315 TxESCi8(NavCorr[Roll]);
gke 0:62a1c91a859a 316 TxESCi8(NavCorr[Pitch]);
gke 0:62a1c91a859a 317 TxESCi8(NavCorr[Yaw]);
gke 0:62a1c91a859a 318
gke 0:62a1c91a859a 319 SendPacketTrailer();
gke 0:62a1c91a859a 320
gke 0:62a1c91a859a 321 } // SendNavPacket
gke 0:62a1c91a859a 322
gke 0:62a1c91a859a 323 void SendStatsPacket(void) {
gke 0:62a1c91a859a 324 SendPacketHeader();
gke 0:62a1c91a859a 325
gke 0:62a1c91a859a 326 TxESCu8(UAVXStatsPacketTag);
gke 0:62a1c91a859a 327 TxESCu8(44);
gke 0:62a1c91a859a 328
gke 0:62a1c91a859a 329 TxESCi16(Stats[I2CFailS]);
gke 0:62a1c91a859a 330 TxESCi16(Stats[GPSInvalidS]);
gke 0:62a1c91a859a 331 TxESCi16(Stats[AccFailS]);
gke 0:62a1c91a859a 332 TxESCi16(Stats[GyroFailS]);
gke 0:62a1c91a859a 333 TxESCi16(Stats[CompassFailS]);
gke 0:62a1c91a859a 334 TxESCi16(Stats[BaroFailS]);
gke 0:62a1c91a859a 335 TxESCi16(Stats[ESCI2CFailS]);
gke 0:62a1c91a859a 336
gke 0:62a1c91a859a 337 TxESCi16(Stats[RCFailsafesS]);
gke 0:62a1c91a859a 338
gke 0:62a1c91a859a 339 TxESCi16(Stats[GPSAltitudeS]);
gke 0:62a1c91a859a 340 TxESCi16(Stats[GPSVelS]);
gke 0:62a1c91a859a 341 TxESCi16(Stats[GPSMinSatsS]);
gke 0:62a1c91a859a 342 TxESCi16(Stats[GPSMaxSatsS]);
gke 0:62a1c91a859a 343 TxESCi16(Stats[MinHDiluteS]);
gke 0:62a1c91a859a 344 TxESCi16(Stats[MaxHDiluteS]);
gke 0:62a1c91a859a 345
gke 0:62a1c91a859a 346 TxESCi16(Stats[BaroRelAltitudeS]);
gke 0:62a1c91a859a 347 TxESCi16(0);//Stats[MinBaroROCS]);
gke 0:62a1c91a859a 348 TxESCi16(0);//Stats[MaxBaroROCS]);
gke 0:62a1c91a859a 349
gke 0:62a1c91a859a 350 TxESCi16(Stats[MinTempS]);
gke 0:62a1c91a859a 351 TxESCi16(Stats[MaxTempS]);
gke 0:62a1c91a859a 352
gke 0:62a1c91a859a 353 TxESCi16(Stats[BadS]);
gke 0:62a1c91a859a 354
gke 0:62a1c91a859a 355 TxESCu8(UAVXAirframe | 0x80);
gke 0:62a1c91a859a 356 TxESCu8(Orientation);
gke 0:62a1c91a859a 357 TxESCi16(Stats[BadNumS]);
gke 0:62a1c91a859a 358
gke 0:62a1c91a859a 359 SendPacketTrailer();
gke 0:62a1c91a859a 360
gke 0:62a1c91a859a 361 } // SendStatsPacket
gke 0:62a1c91a859a 362
gke 0:62a1c91a859a 363 void SendMinPacket(void) {
gke 0:62a1c91a859a 364 static int8 b;
gke 0:62a1c91a859a 365
gke 0:62a1c91a859a 366 SendPacketHeader();
gke 0:62a1c91a859a 367
gke 0:62a1c91a859a 368 TxESCu8(UAVXMinPacketTag);
gke 0:62a1c91a859a 369 TxESCu8(33 + TELEMETRY_FLAG_BYTES);
gke 0:62a1c91a859a 370 for ( b = 0; b < TELEMETRY_FLAG_BYTES; b++ )
gke 0:62a1c91a859a 371 TxESCu8(F.AllFlags[b]);
gke 0:62a1c91a859a 372
gke 0:62a1c91a859a 373 TxESCu8(State);
gke 0:62a1c91a859a 374 TxESCu8(NavState);
gke 0:62a1c91a859a 375 TxESCu8(FailState);
gke 0:62a1c91a859a 376
gke 0:62a1c91a859a 377 TxESCi16(BatteryVolts * 10.0); // to do scaling
gke 0:62a1c91a859a 378 TxESCi16(BatteryCurrent * 10.0);
gke 0:62a1c91a859a 379 TxESCi16(BatteryChargeUsedAH * 1000.0);
gke 0:62a1c91a859a 380
gke 0:62a1c91a859a 381 TxESCi16(Angle[Roll] * 1000.0);
gke 0:62a1c91a859a 382 TxESCi16(Angle[Pitch] * 1000.0);
gke 0:62a1c91a859a 383
gke 0:62a1c91a859a 384 TxESCi24(BaroRelAltitude * 10.0);
gke 0:62a1c91a859a 385 TxESCi16(RangefinderAltitude * 100.0); // cm
gke 0:62a1c91a859a 386
gke 0:62a1c91a859a 387 TxESCi16(Heading * 1000.0);
gke 0:62a1c91a859a 388
gke 0:62a1c91a859a 389 TxESCi32(GPSLatitude); // 5 decimal minute units
gke 0:62a1c91a859a 390 TxESCi32(GPSLongitude);
gke 0:62a1c91a859a 391
gke 0:62a1c91a859a 392 TxESCu8(UAVXAirframe | 0x80 ); // ARM has top bit set
gke 0:62a1c91a859a 393 TxESCu8(Orientation);
gke 0:62a1c91a859a 394
gke 0:62a1c91a859a 395 TxESCi24(mSClock() - mS[StartTime]);
gke 0:62a1c91a859a 396
gke 0:62a1c91a859a 397 SendPacketTrailer();
gke 0:62a1c91a859a 398
gke 0:62a1c91a859a 399 } // SendMinPacket
gke 0:62a1c91a859a 400
gke 0:62a1c91a859a 401 void SendParamPacket(uint8 p) {
gke 0:62a1c91a859a 402
gke 0:62a1c91a859a 403 static uint8 b;
gke 0:62a1c91a859a 404
gke 0:62a1c91a859a 405 SendPacketHeader();
gke 0:62a1c91a859a 406
gke 0:62a1c91a859a 407 TxESCu8(UAVXParamsPacketTag);
gke 0:62a1c91a859a 408 TxESCu8(MAX_PARAMETERS+1);
gke 0:62a1c91a859a 409 TxESCu8(p);
gke 0:62a1c91a859a 410 for (b = 0; b < (uint8)MAX_PARAMETERS; b++ )
gke 0:62a1c91a859a 411 TxESCi8(PX[MAX_PARAMETERS*2 + b]);
gke 0:62a1c91a859a 412
gke 0:62a1c91a859a 413 SendPacketTrailer();
gke 0:62a1c91a859a 414
gke 0:62a1c91a859a 415 } // SendParamPacket
gke 0:62a1c91a859a 416
gke 0:62a1c91a859a 417 void SendCycle(void) {
gke 0:62a1c91a859a 418
gke 0:62a1c91a859a 419 switch ( UAVXCurrPacketTag ) {
gke 0:62a1c91a859a 420 case UAVXFlightPacketTag:
gke 0:62a1c91a859a 421 SendFlightPacket();
gke 0:62a1c91a859a 422
gke 0:62a1c91a859a 423 UAVXCurrPacketTag = UAVXNavPacketTag;
gke 0:62a1c91a859a 424 break;
gke 0:62a1c91a859a 425
gke 0:62a1c91a859a 426 case UAVXNavPacketTag:
gke 0:62a1c91a859a 427 if ( ++StatsNavAlternate < NAV_STATS_INTERLEAVE)
gke 0:62a1c91a859a 428 SendNavPacket();
gke 0:62a1c91a859a 429 else {
gke 0:62a1c91a859a 430 SendStatsPacket();
gke 0:62a1c91a859a 431 StatsNavAlternate = 0;
gke 0:62a1c91a859a 432 }
gke 0:62a1c91a859a 433
gke 0:62a1c91a859a 434 UAVXCurrPacketTag = UAVXFlightPacketTag;
gke 0:62a1c91a859a 435 break;
gke 0:62a1c91a859a 436
gke 0:62a1c91a859a 437 default:
gke 0:62a1c91a859a 438 UAVXCurrPacketTag = UAVXFlightPacketTag;
gke 0:62a1c91a859a 439 break;
gke 0:62a1c91a859a 440 }
gke 0:62a1c91a859a 441
gke 0:62a1c91a859a 442 } // SendCycle
gke 0:62a1c91a859a 443
gke 0:62a1c91a859a 444 void SendArduStation(void) {
gke 0:62a1c91a859a 445
gke 0:62a1c91a859a 446 static int8 Count = 0;
gke 0:62a1c91a859a 447 /*
gke 0:62a1c91a859a 448 Definitions of the low rate telemetry (1Hz):
gke 0:62a1c91a859a 449 LAT: Latitude
gke 0:62a1c91a859a 450 LON: Longitude
gke 0:62a1c91a859a 451 SPD: Speed over ground from GPS
gke 0:62a1c91a859a 452 CRT: Climb Rate in M/S
gke 0:62a1c91a859a 453 ALT: Altitude in meters
gke 0:62a1c91a859a 454 ALH: The altitude is trying to hold
gke 0:62a1c91a859a 455 CRS: Course over ground in degrees.
gke 0:62a1c91a859a 456 BER: Bearing is the heading you want to go
gke 0:62a1c91a859a 457 WPN: Waypoint number, where WP0 is home.
gke 0:62a1c91a859a 458 DST: Distance from Waypoint
gke 0:62a1c91a859a 459 BTV: Battery Voltage.
gke 0:62a1c91a859a 460 RSP: Roll setpoint used to debug, (not displayed here).
gke 0:62a1c91a859a 461
gke 0:62a1c91a859a 462 Definitions of the high rate telemetry (~4Hz):
gke 0:62a1c91a859a 463 ASP: Airspeed, right now is the raw data.
gke 0:62a1c91a859a 464 TTH: Throttle in 100% the autopilot is applying.
gke 0:62a1c91a859a 465 RLL: Roll in degrees + is right - is left
gke 0:62a1c91a859a 466 PCH: Pitch in degrees
gke 0:62a1c91a859a 467 SST: Switch Status, used for debugging, but is disabled in the current version.
gke 0:62a1c91a859a 468 */
gke 0:62a1c91a859a 469
gke 0:62a1c91a859a 470 if ( ++Count == 4 ) {
gke 0:62a1c91a859a 471 TxString("!!!");
gke 0:62a1c91a859a 472 TxString("LAT:");
gke 0:62a1c91a859a 473 TxVal32(GPSLatitude / 6000, 3, 0);
gke 0:62a1c91a859a 474 TxString(",LON:");
gke 0:62a1c91a859a 475 TxVal32(GPSLongitude / 6000, 3, 0);
gke 0:62a1c91a859a 476 TxString(",ALT:");
gke 0:62a1c91a859a 477 TxVal32(Altitude / 10,0,0);
gke 0:62a1c91a859a 478 TxString(",ALH:");
gke 0:62a1c91a859a 479 TxVal32(DesiredAltitude / 10, 0, 0);
gke 0:62a1c91a859a 480 TxString(",CRT:");
gke 0:62a1c91a859a 481 TxVal32(ROC / 100, 0, 0);
gke 0:62a1c91a859a 482 TxString(",CRS:");
gke 0:62a1c91a859a 483 TxVal32(Heading * RADDEG, 0, 0); // scaling to degrees?
gke 0:62a1c91a859a 484 TxString(",BER:");
gke 0:62a1c91a859a 485 TxVal32(WayHeading * RADDEG, 0, 0);
gke 0:62a1c91a859a 486 TxString(",SPD:");
gke 0:62a1c91a859a 487 TxVal32(GPSVel, 0, 0);
gke 0:62a1c91a859a 488 TxString(",WPN:");
gke 0:62a1c91a859a 489 TxVal32(CurrWP,0,0);
gke 0:62a1c91a859a 490 TxString(",DST:");
gke 0:62a1c91a859a 491 TxVal32(0, 0, 0); // distance to WP
gke 0:62a1c91a859a 492 TxString(",BTV:");
gke 0:62a1c91a859a 493 TxVal32((BatteryVoltsADC * 61)/205, 1, 0);
gke 0:62a1c91a859a 494 TxString(",RSP:");
gke 0:62a1c91a859a 495 TxVal32(DesiredRoll, 0, 0);
gke 0:62a1c91a859a 496
gke 0:62a1c91a859a 497 Count = 0;
gke 0:62a1c91a859a 498 } else {
gke 0:62a1c91a859a 499 TxString("+++");
gke 0:62a1c91a859a 500 TxString("ASP:");
gke 0:62a1c91a859a 501 TxVal32(GPSVel / 100, 0, 0);
gke 0:62a1c91a859a 502 TxString(",RLL:");
gke 0:62a1c91a859a 503 TxVal32(Angle[Roll] / 35, 0, 0); // scale to degrees?
gke 0:62a1c91a859a 504 TxString(",PCH:");
gke 0:62a1c91a859a 505 TxVal32(Angle[Pitch] / 35, 0, 0);
gke 0:62a1c91a859a 506 TxString(",THH:");
gke 0:62a1c91a859a 507 TxVal32( ((int24)DesiredThrottle * 100L) / RC_MAXIMUM, 0, 0);
gke 0:62a1c91a859a 508 }
gke 0:62a1c91a859a 509
gke 0:62a1c91a859a 510 TxString(",***\r\n");
gke 0:62a1c91a859a 511
gke 0:62a1c91a859a 512 } // SendArduStation
gke 0:62a1c91a859a 513
gke 0:62a1c91a859a 514 void SendCustom(void)
gke 0:62a1c91a859a 515 { // user defined telemetry human readable OK for small amounts of data < 1mS
gke 0:62a1c91a859a 516
gke 0:62a1c91a859a 517 EchoToLogFile = true;
gke 0:62a1c91a859a 518
gke 0:62a1c91a859a 519 // insert values here using TxVal32(n, dp, separator)
gke 0:62a1c91a859a 520 // dp is the scaling to decimal places, separator
gke 0:62a1c91a859a 521 // separator may be a single 'char', HT for tab, or 0 (no space)
gke 0:62a1c91a859a 522 // ->
gke 0:62a1c91a859a 523
gke 0:62a1c91a859a 524 TxVal32(mSClock(), 3, HT);
gke 0:62a1c91a859a 525
gke 0:62a1c91a859a 526 if ( F.HoldingAlt ) // are we holding
gke 0:62a1c91a859a 527 TxChar('H');
gke 0:62a1c91a859a 528 else
gke 0:62a1c91a859a 529 TxChar('N');
gke 0:62a1c91a859a 530 TxChar(HT);
gke 0:62a1c91a859a 531
gke 0:62a1c91a859a 532 if (F.UsingRangefinderAlt ) // are we using the rangefinder
gke 0:62a1c91a859a 533 TxChar('R');
gke 0:62a1c91a859a 534 else
gke 0:62a1c91a859a 535 TxChar('B');
gke 0:62a1c91a859a 536 TxChar(HT);
gke 0:62a1c91a859a 537
gke 0:62a1c91a859a 538 TxVal32(SRS32(Comp[Alt],1), 1, HT); // ~% throttle compensation
gke 0:62a1c91a859a 539
gke 0:62a1c91a859a 540 TxVal32(GPSRelAltitude, 1, HT);
gke 0:62a1c91a859a 541 TxVal32(BaroRelAltitude, 1, HT);
gke 0:62a1c91a859a 542 TxVal32(RangefinderAltitude, 2, HT);
gke 0:62a1c91a859a 543
gke 0:62a1c91a859a 544 TxVal32(BaroPressure, 0, HT); // eff. sensor reading
gke 0:62a1c91a859a 545 TxVal32(BaroTemperature, 0, HT); // eff. sensor reading redundant for MPX4115
gke 0:62a1c91a859a 546 TxVal32(CompBaroPressure, 0, HT); // moving sum of last 8 readings
gke 0:62a1c91a859a 547
gke 0:62a1c91a859a 548 // <-
gke 0:62a1c91a859a 549
gke 0:62a1c91a859a 550 TxChar(CR);
gke 0:62a1c91a859a 551 TxChar(LF);
gke 0:62a1c91a859a 552
gke 0:62a1c91a859a 553 EchoToLogFile = false;
gke 0:62a1c91a859a 554 } // SendCustom
gke 0:62a1c91a859a 555
gke 0:62a1c91a859a 556 void SensorTrace(void) {
gke 0:62a1c91a859a 557 #ifdef TESTING
gke 0:62a1c91a859a 558
gke 0:62a1c91a859a 559 if ( DesiredThrottle > 20 ) {
gke 0:62a1c91a859a 560 EchoToLogFile = false; // direct to USART
gke 0:62a1c91a859a 561
gke 0:62a1c91a859a 562 TxValH16(((int24)Heading * 180)/MILLIPI);
gke 0:62a1c91a859a 563 TxChar(';');
gke 0:62a1c91a859a 564
gke 0:62a1c91a859a 565 TxValH16(BaroRelAltitude);
gke 0:62a1c91a859a 566 TxChar(';');
gke 0:62a1c91a859a 567 TxValH16(RangefinderAltitude);
gke 0:62a1c91a859a 568 TxChar(';');
gke 0:62a1c91a859a 569 TxValH16(0);
gke 0:62a1c91a859a 570 TxChar(';');
gke 0:62a1c91a859a 571
gke 0:62a1c91a859a 572 TxValH16(DesiredThrottle);
gke 0:62a1c91a859a 573 TxChar(';');
gke 0:62a1c91a859a 574 TxValH16(DesiredRoll);
gke 0:62a1c91a859a 575 TxChar(';');
gke 0:62a1c91a859a 576 TxValH16(DesiredPitch);
gke 0:62a1c91a859a 577 TxChar(';');
gke 0:62a1c91a859a 578 TxValH16(DesiredYaw);
gke 0:62a1c91a859a 579 TxChar(';');
gke 0:62a1c91a859a 580
gke 0:62a1c91a859a 581 TxValH16(Rate[Roll]);
gke 0:62a1c91a859a 582 TxChar(';');
gke 0:62a1c91a859a 583 TxValH16(Rate[Pitch]);
gke 0:62a1c91a859a 584 TxChar(';');
gke 0:62a1c91a859a 585 TxValH16(Rate[Yaw]);
gke 0:62a1c91a859a 586 TxChar(';');
gke 0:62a1c91a859a 587
gke 0:62a1c91a859a 588 TxValH16(Angle[Roll]);
gke 0:62a1c91a859a 589 TxChar(';');
gke 0:62a1c91a859a 590 TxValH16(Angle[Pitch]);
gke 0:62a1c91a859a 591 TxChar(';');
gke 0:62a1c91a859a 592 TxValH16(Angle[Yaw]);
gke 0:62a1c91a859a 593 TxChar(';');
gke 0:62a1c91a859a 594
gke 0:62a1c91a859a 595 TxValH16(Acc[LR]);
gke 0:62a1c91a859a 596 TxChar(';');
gke 0:62a1c91a859a 597 TxValH16(Acc[FB]);
gke 0:62a1c91a859a 598 TxChar(';');
gke 0:62a1c91a859a 599 TxValH16(Acc[DU]);
gke 0:62a1c91a859a 600 TxChar(';');
gke 0:62a1c91a859a 601
gke 0:62a1c91a859a 602 TxValH16(Comp[LR]);
gke 0:62a1c91a859a 603 TxChar(';');
gke 0:62a1c91a859a 604 TxValH16(Comp[FB]);
gke 0:62a1c91a859a 605 TxChar(';');
gke 0:62a1c91a859a 606 TxValH16(Comp[DU]);
gke 0:62a1c91a859a 607 TxChar(';');
gke 0:62a1c91a859a 608 TxValH16(Comp[Alt]);
gke 0:62a1c91a859a 609 TxChar(';');
gke 0:62a1c91a859a 610 TxNextLine();
gke 0:62a1c91a859a 611 }
gke 0:62a1c91a859a 612 #endif // TESTING
gke 0:62a1c91a859a 613 } // SensorTrace