Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
rc.c@0:62a1c91a859a, 2011-02-18 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 18 22:28:05 2011 +0000
- Revision:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
First release
Who changed what in which revision?
User | Revision | Line number | New 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 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 Trim[3]; |
gke | 0:62a1c91a859a | 96 | int16 ThrLow, ThrNeutral, ThrHigh; |
gke | 0:62a1c91a859a | 97 | |
gke | 0:62a1c91a859a | 98 | boolean RCPositiveEdge; |
gke | 0:62a1c91a859a | 99 | |
gke | 0:62a1c91a859a | 100 | void DoRxPolarity(void) { |
gke | 0:62a1c91a859a | 101 | |
gke | 0:62a1c91a859a | 102 | RCInterrupt.rise(&RCNullISR); |
gke | 0:62a1c91a859a | 103 | RCInterrupt.fall(&RCNullISR); |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | if ( F.UsingSerialPPM ) // serial PPM frame from within an Rx |
gke | 0:62a1c91a859a | 106 | if ( PPMPosPolarity[P[TxRxType]] ) |
gke | 0:62a1c91a859a | 107 | RCInterrupt.rise(&RCISR); |
gke | 0:62a1c91a859a | 108 | else |
gke | 0:62a1c91a859a | 109 | RCInterrupt.fall(&RCISR); |
gke | 0:62a1c91a859a | 110 | else { |
gke | 0:62a1c91a859a | 111 | RCInterrupt.rise(&RCISR); |
gke | 0:62a1c91a859a | 112 | RCInterrupt.fall(&RCISR); |
gke | 0:62a1c91a859a | 113 | } |
gke | 0:62a1c91a859a | 114 | |
gke | 0:62a1c91a859a | 115 | } // DoRxPolarity |
gke | 0:62a1c91a859a | 116 | |
gke | 0:62a1c91a859a | 117 | void InitRC(void) { |
gke | 0:62a1c91a859a | 118 | static int8 c, i, q; |
gke | 0:62a1c91a859a | 119 | |
gke | 0:62a1c91a859a | 120 | DoRxPolarity(); |
gke | 0:62a1c91a859a | 121 | |
gke | 0:62a1c91a859a | 122 | SignalCount = -RC_GOOD_BUCKET_MAX; |
gke | 0:62a1c91a859a | 123 | F.Signal = F.RCNewValues = false; |
gke | 0:62a1c91a859a | 124 | |
gke | 0:62a1c91a859a | 125 | for (c = 0; c < RC_CONTROLS; c++) { |
gke | 0:62a1c91a859a | 126 | PPM[c] = 0; |
gke | 0:62a1c91a859a | 127 | RC[c] = RCp[c] = RC_NEUTRAL; |
gke | 0:62a1c91a859a | 128 | |
gke | 0:62a1c91a859a | 129 | for (q = 0; q <= PPMQMASK; q++) |
gke | 0:62a1c91a859a | 130 | PPMQ.B[q][c] = RC_NEUTRAL; |
gke | 0:62a1c91a859a | 131 | PPMQSum[c] = RC_NEUTRAL * 4; |
gke | 0:62a1c91a859a | 132 | } |
gke | 0:62a1c91a859a | 133 | PPMQ.Head = 0; |
gke | 0:62a1c91a859a | 134 | |
gke | 0:62a1c91a859a | 135 | DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = StickThrottle = 0; |
gke | 0:62a1c91a859a | 136 | |
gke | 0:62a1c91a859a | 137 | for ( i = 0; i < 3; i++ ) |
gke | 0:62a1c91a859a | 138 | Trim[i] = 0; |
gke | 0:62a1c91a859a | 139 | |
gke | 0:62a1c91a859a | 140 | PPM_Index = PrevEdge = RCGlitches = 0; |
gke | 0:62a1c91a859a | 141 | } // InitRC |
gke | 0:62a1c91a859a | 142 | |
gke | 0:62a1c91a859a | 143 | void MapRC(void) { // re-maps captured PPM to Rx channel sequence |
gke | 0:62a1c91a859a | 144 | static int8 c; |
gke | 0:62a1c91a859a | 145 | static int16 LastThrottle, i; |
gke | 0:62a1c91a859a | 146 | static uint16 Sum; |
gke | 0:62a1c91a859a | 147 | |
gke | 0:62a1c91a859a | 148 | LastThrottle = RC[ThrottleC]; |
gke | 0:62a1c91a859a | 149 | |
gke | 0:62a1c91a859a | 150 | for ( c = 0 ; c < RC_CONTROLS ; c++ ) { |
gke | 0:62a1c91a859a | 151 | Sum = PPM[c]; |
gke | 0:62a1c91a859a | 152 | PPMQSum[c] -= PPMQ.B[PPMQ.Head][c]; |
gke | 0:62a1c91a859a | 153 | PPMQ.B[PPMQ.Head][c] = Sum; |
gke | 0:62a1c91a859a | 154 | PPMQSum[c] += Sum; |
gke | 0:62a1c91a859a | 155 | PPMQ.Head = (PPMQ.Head + 1) & PPMQMASK; |
gke | 0:62a1c91a859a | 156 | } |
gke | 0:62a1c91a859a | 157 | |
gke | 0:62a1c91a859a | 158 | for ( c = 0 ; c < RC_CONTROLS ; c++ ) { |
gke | 0:62a1c91a859a | 159 | i = Map[P[TxRxType]][c]; |
gke | 0:62a1c91a859a | 160 | RC[c] = ( PPMQSum[i] * RC_MAXIMUM ) / 4000; // needs rethink - throwing away precision |
gke | 0:62a1c91a859a | 161 | } |
gke | 0:62a1c91a859a | 162 | |
gke | 0:62a1c91a859a | 163 | if ( THROTTLE_SLEW_LIMIT > 0 ) |
gke | 0:62a1c91a859a | 164 | RC[ThrottleRC] = SlewLimit(LastThrottle, RC[ThrottleRC], THROTTLE_SLEW_LIMIT); |
gke | 0:62a1c91a859a | 165 | |
gke | 0:62a1c91a859a | 166 | } // MapRC |
gke | 0:62a1c91a859a | 167 | |
gke | 0:62a1c91a859a | 168 | void CheckSticksHaveChanged(void) { |
gke | 0:62a1c91a859a | 169 | static boolean Change; |
gke | 0:62a1c91a859a | 170 | static uint8 c; |
gke | 0:62a1c91a859a | 171 | |
gke | 0:62a1c91a859a | 172 | if ( F.ReturnHome || F.Navigate ) { |
gke | 0:62a1c91a859a | 173 | if ( mSClock() > mS[StickChangeUpdate] ) { |
gke | 0:62a1c91a859a | 174 | mS[StickChangeUpdate] = mSClock() + 500; |
gke | 0:62a1c91a859a | 175 | if ( !F.ForceFailsafe && ( State == InFlight )) { |
gke | 0:62a1c91a859a | 176 | Stats[RCFailsafesS]++; |
gke | 0:62a1c91a859a | 177 | mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 178 | mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS; |
gke | 0:62a1c91a859a | 179 | DescentComp = 0; // for no Baro case |
gke | 0:62a1c91a859a | 180 | } |
gke | 0:62a1c91a859a | 181 | |
gke | 0:62a1c91a859a | 182 | F.ForceFailsafe = State == InFlight; // abort if not navigating |
gke | 0:62a1c91a859a | 183 | } |
gke | 0:62a1c91a859a | 184 | } else { |
gke | 0:62a1c91a859a | 185 | if ( mSClock() > mS[StickChangeUpdate] ) { |
gke | 0:62a1c91a859a | 186 | mS[StickChangeUpdate] = mSClock() + 500; |
gke | 0:62a1c91a859a | 187 | |
gke | 0:62a1c91a859a | 188 | Change = false; |
gke | 0:62a1c91a859a | 189 | for ( c = ThrottleC; c <= (uint8)RTHC; c++ ) { |
gke | 0:62a1c91a859a | 190 | Change |= abs( RC[c] - RCp[c]) > RC_STICK_MOVEMENT; |
gke | 0:62a1c91a859a | 191 | RCp[c] = RC[c]; |
gke | 0:62a1c91a859a | 192 | } |
gke | 0:62a1c91a859a | 193 | } |
gke | 0:62a1c91a859a | 194 | |
gke | 0:62a1c91a859a | 195 | if ( Change ) { |
gke | 0:62a1c91a859a | 196 | mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 197 | mS[NavStateTimeout] = mSClock(); |
gke | 0:62a1c91a859a | 198 | F.ForceFailsafe = false; |
gke | 0:62a1c91a859a | 199 | if ( FailState == MonitoringRx ) { |
gke | 0:62a1c91a859a | 200 | if ( F.LostModel ) { |
gke | 0:62a1c91a859a | 201 | Beeper_OFF; |
gke | 0:62a1c91a859a | 202 | F.LostModel = false; |
gke | 0:62a1c91a859a | 203 | DescentComp = 0; |
gke | 0:62a1c91a859a | 204 | } |
gke | 0:62a1c91a859a | 205 | } |
gke | 0:62a1c91a859a | 206 | } else |
gke | 0:62a1c91a859a | 207 | if ( mSClock() > mS[RxFailsafeTimeout] ) { |
gke | 0:62a1c91a859a | 208 | if ( !F.ForceFailsafe && ( ( State == InFlight ) || ( ( mSClock() - mS[RxFailsafeTimeout]) > 120000 ) ) ) { |
gke | 0:62a1c91a859a | 209 | Stats[RCFailsafesS]++; |
gke | 0:62a1c91a859a | 210 | mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS; |
gke | 0:62a1c91a859a | 211 | mS[DescentUpdate] = mSClock() + ALT_DESCENT_UPDATE_MS; |
gke | 0:62a1c91a859a | 212 | DescentComp = 0; // for no Baro case |
gke | 0:62a1c91a859a | 213 | F.ForceFailsafe = true; |
gke | 0:62a1c91a859a | 214 | } |
gke | 0:62a1c91a859a | 215 | |
gke | 0:62a1c91a859a | 216 | // F.ForceFailsafe = State == InFlight; // abort if not navigating |
gke | 0:62a1c91a859a | 217 | } |
gke | 0:62a1c91a859a | 218 | } |
gke | 0:62a1c91a859a | 219 | |
gke | 0:62a1c91a859a | 220 | } // CheckSticksHaveChanged |
gke | 0:62a1c91a859a | 221 | |
gke | 0:62a1c91a859a | 222 | void UpdateControls(void) { |
gke | 0:62a1c91a859a | 223 | static int16 HoldRoll, HoldPitch, RollPitchScale; |
gke | 0:62a1c91a859a | 224 | static boolean NewCh5Active; |
gke | 0:62a1c91a859a | 225 | |
gke | 0:62a1c91a859a | 226 | F.RCNewValues = false; |
gke | 0:62a1c91a859a | 227 | |
gke | 0:62a1c91a859a | 228 | MapRC(); // remap channel order for specific Tx/Rx |
gke | 0:62a1c91a859a | 229 | |
gke | 0:62a1c91a859a | 230 | StickThrottle = RC[ThrottleRC]; |
gke | 0:62a1c91a859a | 231 | |
gke | 0:62a1c91a859a | 232 | //_________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 233 | |
gke | 0:62a1c91a859a | 234 | // Navigation |
gke | 0:62a1c91a859a | 235 | |
gke | 0:62a1c91a859a | 236 | F.ReturnHome = F.Navigate = F.UsingPolar = false; |
gke | 0:62a1c91a859a | 237 | NewCh5Active = RC[RTHRC] > RC_NEUTRAL; |
gke | 0:62a1c91a859a | 238 | |
gke | 0:62a1c91a859a | 239 | if ( F.UsingPositionHoldLock ) |
gke | 0:62a1c91a859a | 240 | if ( NewCh5Active & !F.Ch5Active ) |
gke | 0:62a1c91a859a | 241 | F.AllowTurnToWP = true; |
gke | 0:62a1c91a859a | 242 | else |
gke | 0:62a1c91a859a | 243 | F.AllowTurnToWP = SaveAllowTurnToWP; |
gke | 0:62a1c91a859a | 244 | else |
gke | 0:62a1c91a859a | 245 | if ( RC[RTHRC] > ((3L*RC_MAXIMUM)/4) ) |
gke | 0:62a1c91a859a | 246 | F.ReturnHome = true; |
gke | 0:62a1c91a859a | 247 | else |
gke | 0:62a1c91a859a | 248 | if ( RC[RTHRC] > (RC_NEUTRAL/2) ) |
gke | 0:62a1c91a859a | 249 | F.Navigate = true; |
gke | 0:62a1c91a859a | 250 | |
gke | 0:62a1c91a859a | 251 | F.Ch5Active = NewCh5Active; |
gke | 0:62a1c91a859a | 252 | |
gke | 0:62a1c91a859a | 253 | #ifdef RX6CH |
gke | 0:62a1c91a859a | 254 | DesiredCamPitchTrim = RC_NEUTRAL; |
gke | 0:62a1c91a859a | 255 | // NavSensitivity set in ReadParametersPX |
gke | 0:62a1c91a859a | 256 | #else |
gke | 0:62a1c91a859a | 257 | DesiredCamPitchTrim = RC[CamPitchRC] - RC_NEUTRAL; |
gke | 0:62a1c91a859a | 258 | NavSensitivity = RC[NavGainRC]; |
gke | 0:62a1c91a859a | 259 | NavSensitivity = Limit(NavSensitivity, 0, RC_MAXIMUM); |
gke | 0:62a1c91a859a | 260 | #endif // !RX6CH |
gke | 0:62a1c91a859a | 261 | |
gke | 0:62a1c91a859a | 262 | //_________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 263 | |
gke | 0:62a1c91a859a | 264 | // Altitude Hold |
gke | 0:62a1c91a859a | 265 | |
gke | 0:62a1c91a859a | 266 | F.AltHoldEnabled = NavSensitivity > NAV_SENS_ALTHOLD_THRESHOLD; |
gke | 0:62a1c91a859a | 267 | |
gke | 0:62a1c91a859a | 268 | if ( NavState == HoldingStation ) { // Manual |
gke | 0:62a1c91a859a | 269 | if ( StickThrottle < RC_THRES_STOP ) // to deal with usual non-zero EPA |
gke | 0:62a1c91a859a | 270 | StickThrottle = 0; |
gke | 0:62a1c91a859a | 271 | } else // Autonomous |
gke | 0:62a1c91a859a | 272 | if ( F.AllowNavAltitudeHold && F.AltHoldEnabled ) |
gke | 0:62a1c91a859a | 273 | StickThrottle = CruiseThrottle; |
gke | 0:62a1c91a859a | 274 | |
gke | 0:62a1c91a859a | 275 | if ( (! F.HoldingAlt) && (!(F.Navigate || F.ReturnHome )) ) // cancel any current altitude hold setting |
gke | 0:62a1c91a859a | 276 | DesiredAltitude = Altitude; |
gke | 0:62a1c91a859a | 277 | |
gke | 0:62a1c91a859a | 278 | //_________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 279 | |
gke | 0:62a1c91a859a | 280 | // Attitude |
gke | 0:62a1c91a859a | 281 | |
gke | 0:62a1c91a859a | 282 | #ifdef ATTITUDE_NO_LIMITS |
gke | 0:62a1c91a859a | 283 | DesiredRoll = RC[RollRC] - RC_NEUTRAL; |
gke | 0:62a1c91a859a | 284 | DesiredPitch = RC[PitchRC] - RC_NEUTRAL; |
gke | 0:62a1c91a859a | 285 | #else |
gke | 0:62a1c91a859a | 286 | RollPitchScale = MAX_ROLL_PITCH - (NavSensitivity >> 2); |
gke | 0:62a1c91a859a | 287 | DesiredRoll = SRS16((RC[RollRC] - RC_NEUTRAL) * RollPitchScale, 7); |
gke | 0:62a1c91a859a | 288 | DesiredPitch = SRS16((RC[PitchRC] - RC_NEUTRAL) * RollPitchScale, 7); |
gke | 0:62a1c91a859a | 289 | #endif // ATTITUDE_NO_LIMITS |
gke | 0:62a1c91a859a | 290 | DesiredYaw = RC[YawRC] - RC_NEUTRAL; |
gke | 0:62a1c91a859a | 291 | |
gke | 0:62a1c91a859a | 292 | HoldRoll = DesiredRoll - Trim[Roll]; |
gke | 0:62a1c91a859a | 293 | HoldRoll = abs(HoldRoll); |
gke | 0:62a1c91a859a | 294 | HoldPitch = DesiredPitch - Trim[Pitch]; |
gke | 0:62a1c91a859a | 295 | HoldPitch = abs(HoldPitch); |
gke | 0:62a1c91a859a | 296 | CurrMaxRollPitch = Max(HoldRoll, HoldPitch); |
gke | 0:62a1c91a859a | 297 | |
gke | 0:62a1c91a859a | 298 | if ( CurrMaxRollPitch > ATTITUDE_HOLD_LIMIT ) |
gke | 0:62a1c91a859a | 299 | if ( AttitudeHoldResetCount > ATTITUDE_HOLD_RESET_INTERVAL ) |
gke | 0:62a1c91a859a | 300 | F.AttitudeHold = false; |
gke | 0:62a1c91a859a | 301 | else { |
gke | 0:62a1c91a859a | 302 | AttitudeHoldResetCount++; |
gke | 0:62a1c91a859a | 303 | F.AttitudeHold = true; |
gke | 0:62a1c91a859a | 304 | } |
gke | 0:62a1c91a859a | 305 | else { |
gke | 0:62a1c91a859a | 306 | F.AttitudeHold = true; |
gke | 0:62a1c91a859a | 307 | if ( AttitudeHoldResetCount > 1 ) |
gke | 0:62a1c91a859a | 308 | AttitudeHoldResetCount -= 2; // Faster decay |
gke | 0:62a1c91a859a | 309 | } |
gke | 0:62a1c91a859a | 310 | |
gke | 0:62a1c91a859a | 311 | //_________________________________________________________________________________________ |
gke | 0:62a1c91a859a | 312 | |
gke | 0:62a1c91a859a | 313 | // Rx has gone to failsafe |
gke | 0:62a1c91a859a | 314 | |
gke | 0:62a1c91a859a | 315 | //zzz CheckSticksHaveChanged(); |
gke | 0:62a1c91a859a | 316 | |
gke | 0:62a1c91a859a | 317 | F.NewCommands = true; |
gke | 0:62a1c91a859a | 318 | |
gke | 0:62a1c91a859a | 319 | } // UpdateControls |
gke | 0:62a1c91a859a | 320 | |
gke | 0:62a1c91a859a | 321 | void CaptureTrims(void) { // only used in detecting movement from neutral in hold GPS position |
gke | 0:62a1c91a859a | 322 | // Trims are invalidated if Nav sensitivity is changed - Answer do not use trims ? |
gke | 0:62a1c91a859a | 323 | Trim[Roll] = Limit(DesiredRoll, -NAV_MAX_TRIM, NAV_MAX_TRIM); |
gke | 0:62a1c91a859a | 324 | Trim[Pitch] = Limit(DesiredPitch, -NAV_MAX_TRIM, NAV_MAX_TRIM); |
gke | 0:62a1c91a859a | 325 | Trim[Yaw] = Limit(DesiredYaw, -NAV_MAX_TRIM, NAV_MAX_TRIM); |
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 |