Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

telemetry.c@1:1e3318a30ddd, 2011-02-25 (annotated)

Committer:
gke
Date:
Fri Feb 25 01:35:24 2011 +0000
Revision:
1:1e3318a30ddd
Parent:
0:62a1c91a859a
Child:
2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.

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