Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
0:62a1c91a859a 
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 DoRxPolarity(void);
gke 0:62a1c91a859a 24 void InitRC(void);
gke 0:62a1c91a859a 25 void MapRC(void);
gke 0:62a1c91a859a 26 void CheckSticksHaveChanged(void);
gke 0:62a1c91a859a 27 void UpdateControls(void);
gke 0:62a1c91a859a 28 void CaptureTrims(void);
gke 0:62a1c91a859a 29 void CheckThrottleMoved(void);
gke 0:62a1c91a859a 30 void ReceiverTest(void);
gke 0:62a1c91a859a 31
gke 0:62a1c91a859a 32 const uint8 Map[CustomTxRx+1][CONTROLS] = {
gke 0:62a1c91a859a 33 { 2,0,1,3,4,5,6 }, // Futaba Thr 3 Throttle
gke 0:62a1c91a859a 34 { 1,0,3,2,4,5,6 }, // Futaba Thr 2 Throttle
gke 0:62a1c91a859a 35 { 4,2,1,0,5,3,6 }, // Futaba 9C Spektrum DM8/AR7000
gke 0:62a1c91a859a 36 { 0,1,2,3,4,5,6 }, // JR XP8103/PPM
gke 0:62a1c91a859a 37 { 6,0,3,5,2,4,1 }, // JR 9XII Spektrum DM9 ?
gke 0:62a1c91a859a 38
gke 0:62a1c91a859a 39 { 5,0,3,6,2,1,4 }, // JR DXS12
gke 0:62a1c91a859a 40 { 5,0,3,6,2,1,4 }, // Spektrum DX7/AR7000
gke 0:62a1c91a859a 41 { 4,0,3,5,2,1,6 }, // Spektrum DX7/AR6200
gke 0:62a1c91a859a 42
gke 0:62a1c91a859a 43 { 2,0,1,3,4,6,5 }, // Futaba Thr 3 Sw 6/7
gke 0:62a1c91a859a 44 { 0,1,2,3,4,5,6 }, // Spektrum DX7/AR6000
gke 0:62a1c91a859a 45 { 0,1,2,3,4,5,6 }, // Graupner MX16S
gke 0:62a1c91a859a 46 { 4,0,2,3,1,5,6 }, // Spektrum DX6i/AR6200
gke 0:62a1c91a859a 47 { 2,0,1,3,4,5,6 }, // Futaba Th 3/R617FS
gke 0:62a1c91a859a 48 { 4,0,2,3,5,1,6 }, // Spektrum DX7a/AR7000
gke 0:62a1c91a859a 49 { 2,0,1,3,4,6,5 }, // External decoder (Futaba Thr 3 6/7 swap)
gke 0:62a1c91a859a 50 { 0,1,2,3,4,5,6 }, // FrSky DJT/D8R-SP
gke 0:62a1c91a859a 51 { 5,0,3,6,2,1,4 }, // UNDEFINED Spektrum DX7/AR7000
gke 0:62a1c91a859a 52 { 2,0,1,3,4,5,6 } // Custom
gke 0:62a1c91a859a 53 //{ 4,0,2,1,3,5,6 } // Custom
gke 0:62a1c91a859a 54 };
gke 0:62a1c91a859a 55
gke 0:62a1c91a859a 56 // Rx signalling polarity used only for serial PPM frames usually
gke 0:62a1c91a859a 57 // by tapping internal Rx circuitry.
gke 0:62a1c91a859a 58 const boolean PPMPosPolarity[CustomTxRx+1] =
gke 0:62a1c91a859a 59 {
gke 0:62a1c91a859a 60 false, // Futaba Ch3 Throttle
gke 0:62a1c91a859a 61 false, // Futaba Ch2 Throttle
gke 0:62a1c91a859a 62 true, // Futaba 9C Spektrum DM8/AR7000
gke 0:62a1c91a859a 63 true, // JR XP8103/PPM
gke 0:62a1c91a859a 64 true, // JR 9XII Spektrum DM9/AR7000
gke 0:62a1c91a859a 65
gke 0:62a1c91a859a 66 true, // JR DXS12
gke 0:62a1c91a859a 67 true, // Spektrum DX7/AR7000
gke 0:62a1c91a859a 68 true, // Spektrum DX7/AR6200
gke 0:62a1c91a859a 69 false, // Futaba Thr 3 Sw 6/7
gke 0:62a1c91a859a 70 true, // Spektrum DX7/AR6000
gke 0:62a1c91a859a 71 true, // Graupner MX16S
gke 0:62a1c91a859a 72 true, // Graupner DX6i/AR6200
gke 0:62a1c91a859a 73 true, // Futaba Thr 3/R617FS
gke 0:62a1c91a859a 74 true, // Spektrum DX7a/AR7000
gke 0:62a1c91a859a 75 false, // External decoder (Futaba Ch3 Throttle)
gke 0:62a1c91a859a 76 true, // FrSky DJT/D8R-SP
gke 0:62a1c91a859a 77 true, // UNKNOWN using Spektrum DX7/AR7000
gke 0:62a1c91a859a 78 true // custom Tx/Rx combination
gke 0:62a1c91a859a 79 };
gke 0:62a1c91a859a 80
gke 0:62a1c91a859a 81 // Reference Internal Quadrocopter Channel Order
gke 0:62a1c91a859a 82 // 0 Throttle
gke 0:62a1c91a859a 83 // 1 Aileron
gke 0:62a1c91a859a 84 // 2 Elevator
gke 0:62a1c91a859a 85 // 3 Rudder
gke 0:62a1c91a859a 86 // 4 Gear
gke 0:62a1c91a859a 87 // 5 Aux1
gke 0:62a1c91a859a 88 // 6 Aux2
gke 0:62a1c91a859a 89
gke 0:62a1c91a859a 90 int8 RMap[CONTROLS];
gke 0:62a1c91a859a 91
gke 0:62a1c91a859a 92 int16x8x4Q PPMQ;
gke 0:62a1c91a859a 93 int16 PPMQSum[CONTROLS];
gke 0:62a1c91a859a 94 int16 RC[CONTROLS], RCp[CONTROLS];
gke 0:62a1c91a859a 95 int16 ThrLow, ThrNeutral, ThrHigh;
gke 0:62a1c91a859a 96
gke 0:62a1c91a859a 97 boolean RCPositiveEdge;
gke 0:62a1c91a859a 98
gke 0:62a1c91a859a 99 void DoRxPolarity(void) {
gke 0:62a1c91a859a 100
gke 0:62a1c91a859a 101 RCInterrupt.rise(&RCNullISR);
gke 0:62a1c91a859a 102 RCInterrupt.fall(&RCNullISR);
gke 0:62a1c91a859a 103
gke 0:62a1c91a859a 104 if ( F.UsingSerialPPM ) // serial PPM frame from within an Rx
gke 0:62a1c91a859a 105 if ( PPMPosPolarity[P[TxRxType]] )
gke 0:62a1c91a859a 106 RCInterrupt.rise(&RCISR);
gke 0:62a1c91a859a 107 else
gke 0:62a1c91a859a 108 RCInterrupt.fall(&RCISR);
gke 0:62a1c91a859a 109 else {
gke 0:62a1c91a859a 110 RCInterrupt.rise(&RCISR);
gke 0:62a1c91a859a 111 RCInterrupt.fall(&RCISR);
gke 0:62a1c91a859a 112 }
gke 0:62a1c91a859a 113
gke 0:62a1c91a859a 114 } // DoRxPolarity
gke 0:62a1c91a859a 115
gke 0:62a1c91a859a 116 void InitRC(void) {
gke 2:90292f8bd179 117 static int8 c, q;
gke 0:62a1c91a859a 118
gke 0:62a1c91a859a 119 DoRxPolarity();
gke 0:62a1c91a859a 120
gke 0:62a1c91a859a 121 SignalCount = -RC_GOOD_BUCKET_MAX;
gke 0:62a1c91a859a 122 F.Signal = F.RCNewValues = false;
gke 0:62a1c91a859a 123
gke 0:62a1c91a859a 124 for (c = 0; c < RC_CONTROLS; c++) {
gke 0:62a1c91a859a 125 PPM[c] = 0;
gke 0:62a1c91a859a 126 RC[c] = RCp[c] = RC_NEUTRAL;
gke 0:62a1c91a859a 127
gke 0:62a1c91a859a 128 for (q = 0; q <= PPMQMASK; q++)
gke 0:62a1c91a859a 129 PPMQ.B[q][c] = RC_NEUTRAL;
gke 0:62a1c91a859a 130 PPMQSum[c] = RC_NEUTRAL * 4;
gke 0:62a1c91a859a 131 }
gke 0:62a1c91a859a 132 PPMQ.Head = 0;
gke 0:62a1c91a859a 133
gke 0:62a1c91a859a 134 DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = StickThrottle = 0;
gke 2:90292f8bd179 135 Trim[Roll] = Trim[Pitch] = Trim[Yaw] = 0;
gke 0:62a1c91a859a 136
gke 0:62a1c91a859a 137 PPM_Index = PrevEdge = RCGlitches = 0;
gke 0:62a1c91a859a 138 } // InitRC
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 void MapRC(void) { // re-maps captured PPM to Rx channel sequence
gke 0:62a1c91a859a 141 static int8 c;
gke 0:62a1c91a859a 142 static int16 LastThrottle, i;
gke 0:62a1c91a859a 143 static uint16 Sum;
gke 0:62a1c91a859a 144
gke 0:62a1c91a859a 145 LastThrottle = RC[ThrottleC];
gke 0:62a1c91a859a 146
gke 0:62a1c91a859a 147 for ( c = 0 ; c < RC_CONTROLS ; c++ ) {
gke 0:62a1c91a859a 148 Sum = PPM[c];
gke 0:62a1c91a859a 149 PPMQSum[c] -= PPMQ.B[PPMQ.Head][c];
gke 0:62a1c91a859a 150 PPMQ.B[PPMQ.Head][c] = Sum;
gke 0:62a1c91a859a 151 PPMQSum[c] += Sum;
gke 0:62a1c91a859a 152 PPMQ.Head = (PPMQ.Head + 1) & PPMQMASK;
gke 0:62a1c91a859a 153 }
gke 0:62a1c91a859a 154
gke 0:62a1c91a859a 155 for ( c = 0 ; c < RC_CONTROLS ; c++ ) {
gke 0:62a1c91a859a 156 i = Map[P[TxRxType]][c];
gke 0:62a1c91a859a 157 RC[c] = ( PPMQSum[i] * RC_MAXIMUM ) / 4000; // needs rethink - throwing away precision
gke 0:62a1c91a859a 158 }
gke 0:62a1c91a859a 159
gke 0:62a1c91a859a 160 if ( THROTTLE_SLEW_LIMIT > 0 )
gke 0:62a1c91a859a 161 RC[ThrottleRC] = SlewLimit(LastThrottle, RC[ThrottleRC], THROTTLE_SLEW_LIMIT);
gke 0:62a1c91a859a 162
gke 0:62a1c91a859a 163 } // MapRC
gke 0:62a1c91a859a 164
gke 0:62a1c91a859a 165 void CheckSticksHaveChanged(void) {
gke 0:62a1c91a859a 166 static boolean Change;
gke 0:62a1c91a859a 167 static uint8 c;
gke 0:62a1c91a859a 168
gke 0:62a1c91a859a 169 if ( F.ReturnHome || F.Navigate ) {
gke 0:62a1c91a859a 170 if ( mSClock() > mS[StickChangeUpdate] ) {
gke 0:62a1c91a859a 171 mS[StickChangeUpdate] = mSClock() + 500;
gke 0:62a1c91a859a 172 if ( !F.ForceFailsafe && ( State == InFlight )) {
gke 0:62a1c91a859a 173 Stats[RCFailsafesS]++;
gke 0:62a1c91a859a 174 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
gke 0:62a1c91a859a 175 mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
gke 0:62a1c91a859a 176 DescentComp = 0; // for no Baro case
gke 0:62a1c91a859a 177 }
gke 0:62a1c91a859a 178
gke 0:62a1c91a859a 179 F.ForceFailsafe = State == InFlight; // abort if not navigating
gke 0:62a1c91a859a 180 }
gke 0:62a1c91a859a 181 } else {
gke 0:62a1c91a859a 182 if ( mSClock() > mS[StickChangeUpdate] ) {
gke 0:62a1c91a859a 183 mS[StickChangeUpdate] = mSClock() + 500;
gke 0:62a1c91a859a 184
gke 0:62a1c91a859a 185 Change = false;
gke 0:62a1c91a859a 186 for ( c = ThrottleC; c <= (uint8)RTHC; c++ ) {
gke 0:62a1c91a859a 187 Change |= abs( RC[c] - RCp[c]) > RC_STICK_MOVEMENT;
gke 0:62a1c91a859a 188 RCp[c] = RC[c];
gke 0:62a1c91a859a 189 }
gke 0:62a1c91a859a 190 }
gke 0:62a1c91a859a 191
gke 0:62a1c91a859a 192 if ( Change ) {
gke 0:62a1c91a859a 193 mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
gke 0:62a1c91a859a 194 mS[NavStateTimeout] = mSClock();
gke 0:62a1c91a859a 195 F.ForceFailsafe = false;
gke 0:62a1c91a859a 196 if ( FailState == MonitoringRx ) {
gke 0:62a1c91a859a 197 if ( F.LostModel ) {
gke 0:62a1c91a859a 198 Beeper_OFF;
gke 0:62a1c91a859a 199 F.LostModel = false;
gke 0:62a1c91a859a 200 DescentComp = 0;
gke 0:62a1c91a859a 201 }
gke 0:62a1c91a859a 202 }
gke 0:62a1c91a859a 203 } else
gke 0:62a1c91a859a 204 if ( mSClock() > mS[RxFailsafeTimeout] ) {
gke 0:62a1c91a859a 205 if ( !F.ForceFailsafe && ( ( State == InFlight ) || ( ( mSClock() - mS[RxFailsafeTimeout]) > 120000 ) ) ) {
gke 0:62a1c91a859a 206 Stats[RCFailsafesS]++;
gke 0:62a1c91a859a 207 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
gke 0:62a1c91a859a 208 mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS;
gke 0:62a1c91a859a 209 DescentComp = 0; // for no Baro case
gke 0:62a1c91a859a 210 F.ForceFailsafe = true;
gke 0:62a1c91a859a 211 }
gke 0:62a1c91a859a 212
gke 0:62a1c91a859a 213 // F.ForceFailsafe = State == InFlight; // abort if not navigating
gke 0:62a1c91a859a 214 }
gke 0:62a1c91a859a 215 }
gke 0:62a1c91a859a 216
gke 0:62a1c91a859a 217 } // CheckSticksHaveChanged
gke 0:62a1c91a859a 218
gke 0:62a1c91a859a 219 void UpdateControls(void) {
gke 0:62a1c91a859a 220 static int16 HoldRoll, HoldPitch, RollPitchScale;
gke 0:62a1c91a859a 221 static boolean NewCh5Active;
gke 0:62a1c91a859a 222
gke 0:62a1c91a859a 223 F.RCNewValues = false;
gke 0:62a1c91a859a 224
gke 0:62a1c91a859a 225 MapRC(); // remap channel order for specific Tx/Rx
gke 0:62a1c91a859a 226
gke 0:62a1c91a859a 227 StickThrottle = RC[ThrottleRC];
gke 0:62a1c91a859a 228
gke 0:62a1c91a859a 229 //_________________________________________________________________________________________
gke 0:62a1c91a859a 230
gke 0:62a1c91a859a 231 // Navigation
gke 0:62a1c91a859a 232
gke 0:62a1c91a859a 233 F.ReturnHome = F.Navigate = F.UsingPolar = false;
gke 0:62a1c91a859a 234 NewCh5Active = RC[RTHRC] > RC_NEUTRAL;
gke 0:62a1c91a859a 235
gke 0:62a1c91a859a 236 if ( F.UsingPositionHoldLock )
gke 0:62a1c91a859a 237 if ( NewCh5Active & !F.Ch5Active )
gke 0:62a1c91a859a 238 F.AllowTurnToWP = true;
gke 0:62a1c91a859a 239 else
gke 0:62a1c91a859a 240 F.AllowTurnToWP = SaveAllowTurnToWP;
gke 0:62a1c91a859a 241 else
gke 0:62a1c91a859a 242 if ( RC[RTHRC] > ((3L*RC_MAXIMUM)/4) )
gke 0:62a1c91a859a 243 F.ReturnHome = true;
gke 0:62a1c91a859a 244 else
gke 0:62a1c91a859a 245 if ( RC[RTHRC] > (RC_NEUTRAL/2) )
gke 0:62a1c91a859a 246 F.Navigate = true;
gke 0:62a1c91a859a 247
gke 0:62a1c91a859a 248 F.Ch5Active = NewCh5Active;
gke 0:62a1c91a859a 249
gke 0:62a1c91a859a 250 #ifdef RX6CH
gke 0:62a1c91a859a 251 DesiredCamPitchTrim = RC_NEUTRAL;
gke 0:62a1c91a859a 252 // NavSensitivity set in ReadParametersPX
gke 0:62a1c91a859a 253 #else
gke 0:62a1c91a859a 254 DesiredCamPitchTrim = RC[CamPitchRC] - RC_NEUTRAL;
gke 0:62a1c91a859a 255 NavSensitivity = RC[NavGainRC];
gke 0:62a1c91a859a 256 NavSensitivity = Limit(NavSensitivity, 0, RC_MAXIMUM);
gke 0:62a1c91a859a 257 #endif // !RX6CH
gke 0:62a1c91a859a 258
gke 0:62a1c91a859a 259 //_________________________________________________________________________________________
gke 0:62a1c91a859a 260
gke 0:62a1c91a859a 261 // Altitude Hold
gke 0:62a1c91a859a 262
gke 0:62a1c91a859a 263 F.AltHoldEnabled = NavSensitivity > NAV_SENS_ALTHOLD_THRESHOLD;
gke 0:62a1c91a859a 264
gke 0:62a1c91a859a 265 if ( NavState == HoldingStation ) { // Manual
gke 0:62a1c91a859a 266 if ( StickThrottle < RC_THRES_STOP ) // to deal with usual non-zero EPA
gke 0:62a1c91a859a 267 StickThrottle = 0;
gke 0:62a1c91a859a 268 } else // Autonomous
gke 0:62a1c91a859a 269 if ( F.AllowNavAltitudeHold && F.AltHoldEnabled )
gke 0:62a1c91a859a 270 StickThrottle = CruiseThrottle;
gke 0:62a1c91a859a 271
gke 0:62a1c91a859a 272 if ( (! F.HoldingAlt) && (!(F.Navigate || F.ReturnHome )) ) // cancel any current altitude hold setting
gke 0:62a1c91a859a 273 DesiredAltitude = Altitude;
gke 0:62a1c91a859a 274
gke 0:62a1c91a859a 275 //_________________________________________________________________________________________
gke 0:62a1c91a859a 276
gke 0:62a1c91a859a 277 // Attitude
gke 0:62a1c91a859a 278
gke 0:62a1c91a859a 279 #ifdef ATTITUDE_NO_LIMITS
gke 0:62a1c91a859a 280 DesiredRoll = RC[RollRC] - RC_NEUTRAL;
gke 0:62a1c91a859a 281 DesiredPitch = RC[PitchRC] - RC_NEUTRAL;
gke 0:62a1c91a859a 282 #else
gke 0:62a1c91a859a 283 RollPitchScale = MAX_ROLL_PITCH - (NavSensitivity >> 2);
gke 0:62a1c91a859a 284 DesiredRoll = SRS16((RC[RollRC] - RC_NEUTRAL) * RollPitchScale, 7);
gke 0:62a1c91a859a 285 DesiredPitch = SRS16((RC[PitchRC] - RC_NEUTRAL) * RollPitchScale, 7);
gke 0:62a1c91a859a 286 #endif // ATTITUDE_NO_LIMITS
gke 0:62a1c91a859a 287 DesiredYaw = RC[YawRC] - RC_NEUTRAL;
gke 2:90292f8bd179 288
gke 2:90292f8bd179 289 AdaptiveYawLPFreq();
gke 0:62a1c91a859a 290
gke 2:90292f8bd179 291 HoldRoll = abs(DesiredRoll - Trim[Roll]);
gke 2:90292f8bd179 292 HoldPitch = abs(DesiredPitch - Trim[Pitch]);
gke 0:62a1c91a859a 293 CurrMaxRollPitch = Max(HoldRoll, HoldPitch);
gke 0:62a1c91a859a 294
gke 0:62a1c91a859a 295 if ( CurrMaxRollPitch > ATTITUDE_HOLD_LIMIT )
gke 0:62a1c91a859a 296 if ( AttitudeHoldResetCount > ATTITUDE_HOLD_RESET_INTERVAL )
gke 0:62a1c91a859a 297 F.AttitudeHold = false;
gke 0:62a1c91a859a 298 else {
gke 0:62a1c91a859a 299 AttitudeHoldResetCount++;
gke 0:62a1c91a859a 300 F.AttitudeHold = true;
gke 0:62a1c91a859a 301 }
gke 0:62a1c91a859a 302 else {
gke 0:62a1c91a859a 303 F.AttitudeHold = true;
gke 0:62a1c91a859a 304 if ( AttitudeHoldResetCount > 1 )
gke 0:62a1c91a859a 305 AttitudeHoldResetCount -= 2; // Faster decay
gke 0:62a1c91a859a 306 }
gke 0:62a1c91a859a 307
gke 0:62a1c91a859a 308 //_________________________________________________________________________________________
gke 0:62a1c91a859a 309
gke 0:62a1c91a859a 310 // Rx has gone to failsafe
gke 0:62a1c91a859a 311
gke 0:62a1c91a859a 312 //zzz CheckSticksHaveChanged();
gke 0:62a1c91a859a 313
gke 0:62a1c91a859a 314 F.NewCommands = true;
gke 0:62a1c91a859a 315
gke 0:62a1c91a859a 316 } // UpdateControls
gke 0:62a1c91a859a 317
gke 2:90292f8bd179 318 void CaptureTrims(void)
gke 2:90292f8bd179 319 { // only used in detecting movement from neutral in hold GPS position
gke 0:62a1c91a859a 320 // Trims are invalidated if Nav sensitivity is changed - Answer do not use trims ?
gke 2:90292f8bd179 321 #ifndef TESTING
gke 2:90292f8bd179 322 Trim[Roll] = Limit1(DesiredRoll, NAV_MAX_TRIM);
gke 2:90292f8bd179 323 Trim[Yaw] = Limit1(DesiredPitch, NAV_MAX_TRIM);
gke 2:90292f8bd179 324 Trim[Yaw] = Limit1(DesiredYaw, NAV_MAX_TRIM);
gke 2:90292f8bd179 325 #endif // TESTING
gke 0:62a1c91a859a 326
gke 0:62a1c91a859a 327 HoldYaw = 0;
gke 0:62a1c91a859a 328 } // CaptureTrims
gke 0:62a1c91a859a 329
gke 0:62a1c91a859a 330 void CheckThrottleMoved(void) {
gke 0:62a1c91a859a 331 if ( mSClock() < mS[ThrottleUpdate] )
gke 0:62a1c91a859a 332 ThrNeutral = DesiredThrottle;
gke 0:62a1c91a859a 333 else {
gke 0:62a1c91a859a 334 ThrLow = ThrNeutral - THROTTLE_MIDDLE;
gke 0:62a1c91a859a 335 ThrLow = Max(ThrLow, THROTTLE_MIN_ALT_HOLD);
gke 0:62a1c91a859a 336 ThrHigh = ThrNeutral + THROTTLE_MIDDLE;
gke 0:62a1c91a859a 337 if ( ( DesiredThrottle <= ThrLow ) || ( DesiredThrottle >= ThrHigh ) ) {
gke 0:62a1c91a859a 338 mS[ThrottleUpdate] = mSClock() + THROTTLE_UPDATE_MS;
gke 0:62a1c91a859a 339 F.ThrottleMoving = true;
gke 0:62a1c91a859a 340 } else
gke 0:62a1c91a859a 341 F.ThrottleMoving = false;
gke 0:62a1c91a859a 342 }
gke 0:62a1c91a859a 343 } // CheckThrottleMoved
gke 0:62a1c91a859a 344
gke 0:62a1c91a859a 345 void ReceiverTest(void) {
gke 0:62a1c91a859a 346 static int8 s;
gke 0:62a1c91a859a 347
gke 0:62a1c91a859a 348 TxString("\r\nRx Test \r\n");
gke 0:62a1c91a859a 349
gke 0:62a1c91a859a 350 TxString("\r\nRx: ");
gke 0:62a1c91a859a 351 ShowRxSetup();
gke 0:62a1c91a859a 352 TxString("\r\n");
gke 0:62a1c91a859a 353
gke 0:62a1c91a859a 354 TxString("RAW Rx frame values - neutrals NOT applied\r\n");
gke 0:62a1c91a859a 355 TxString("Channel order is: ");
gke 0:62a1c91a859a 356 for ( s = 0; s < RC_CONTROLS; s++)
gke 0:62a1c91a859a 357 TxChar(RxChMnem[RMap[s]]);
gke 0:62a1c91a859a 358
gke 0:62a1c91a859a 359 if ( F.Signal )
gke 0:62a1c91a859a 360 TxString("\r\nSignal OK ");
gke 0:62a1c91a859a 361 else
gke 0:62a1c91a859a 362 TxString("\r\nSignal FAIL ");
gke 0:62a1c91a859a 363 TxVal32(mSClock() - mS[LastValidRx], 0, 0);
gke 0:62a1c91a859a 364 TxString(" mS ago\r\n");
gke 0:62a1c91a859a 365
gke 0:62a1c91a859a 366 // Be wary as new RC frames are being received as this
gke 0:62a1c91a859a 367 // is being displayed so data may be from overlapping frames
gke 0:62a1c91a859a 368
gke 0:62a1c91a859a 369 for ( s = 0; s < RC_CONTROLS ; s++ ) {
gke 0:62a1c91a859a 370 TxChar(s+'1');
gke 0:62a1c91a859a 371 TxString(": ");
gke 0:62a1c91a859a 372 TxChar(RxChMnem[RMap[s]]);
gke 0:62a1c91a859a 373 TxString(":\t");
gke 0:62a1c91a859a 374
gke 0:62a1c91a859a 375 TxVal32((int32)PPM[s] + 1000, 3, 0);
gke 0:62a1c91a859a 376 TxChar(HT);
gke 0:62a1c91a859a 377 TxVal32(PPM[s] / 10, 0, '%');
gke 0:62a1c91a859a 378 if ( ( PPM[s] < -200 ) || ( PPM[s] > 1200 ) )
gke 0:62a1c91a859a 379 TxString(" FAIL");
gke 0:62a1c91a859a 380
gke 0:62a1c91a859a 381 TxNextLine();
gke 0:62a1c91a859a 382 }
gke 0:62a1c91a859a 383
gke 0:62a1c91a859a 384 TxString("Glitches:\t");
gke 0:62a1c91a859a 385 TxVal32(RCGlitches,0,0);
gke 0:62a1c91a859a 386 TxNextLine();
gke 0:62a1c91a859a 387
gke 0:62a1c91a859a 388 } // ReceiverTest
gke 0:62a1c91a859a 389
gke 0:62a1c91a859a 390
gke 0:62a1c91a859a 391