Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

telemetry.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
Not flightworthy. Posted for others to make use of the I2C SW code.

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