rc.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 void DoRxPolarity(void);
00024 void InitRC(void);
00025 void MapRC(void);
00026 void CheckSticksHaveChanged(void);
00027 void UpdateControls(void);
00028 void CaptureTrims(void);
00029 void CheckThrottleMoved(void);
00030 void ReceiverTest(void);
00031 
00032 const uint8 Map[CustomTxRx+1][CONTROLS] = {
00033     { 2,0,1,3,4,5,6 },     // Futaba Thr 3 Throttle
00034     { 1,0,3,2,4,5,6 },    // Futaba Thr 2 Throttle
00035     { 4,2,1,0,5,3,6 },    // Futaba 9C Spektrum DM8/AR7000
00036     { 0,1,2,3,4,5,6 },    // JR XP8103/PPM
00037     { 6,0,3,5,2,4,1 },    // JR 9XII Spektrum DM9 ?
00038 
00039     { 5,0,3,6,2,1,4 },    // JR DXS12 
00040     { 5,0,3,6,2,1,4 },    // Spektrum DX7/AR7000
00041     { 4,0,3,5,2,1,6 },    // Spektrum DX7/AR6200
00042 
00043     { 2,0,1,3,4,6,5 },     // Futaba Thr 3 Sw 6/7
00044     { 0,1,2,3,4,5,6 },    // Spektrum DX7/AR6000
00045     { 0,1,2,3,4,5,6 },    // Graupner MX16S
00046     { 4,0,2,3,1,5,6 },    // Spektrum DX6i/AR6200
00047     { 2,0,1,3,4,5,6 },    // Futaba Th 3/R617FS
00048     { 4,0,2,3,5,1,6 },    // Spektrum DX7a/AR7000
00049     { 2,0,1,3,4,6,5 },     // External decoder (Futaba Thr 3 6/7 swap)
00050     { 0,1,2,3,4,5,6 },    // FrSky DJT/D8R-SP
00051     { 5,0,3,6,2,1,4 },    // UNDEFINED Spektrum DX7/AR7000
00052     { 2,0,1,3,4,5,6 }    // Custom
00053 //{ 4,0,2,1,3,5,6 }    // Custom
00054     };
00055 
00056 // Rx signalling polarity used only for serial PPM frames usually
00057 // by tapping internal Rx circuitry.
00058 const boolean PPMPosPolarity[CustomTxRx+1] =
00059     {
00060         false,     // Futaba Ch3 Throttle
00061         false,    // Futaba Ch2 Throttle
00062         true,    // Futaba 9C Spektrum DM8/AR7000
00063         true,    // JR XP8103/PPM
00064         true,    // JR 9XII Spektrum DM9/AR7000
00065 
00066         true,    // JR DXS12
00067         true,    // Spektrum DX7/AR7000
00068         true,    // Spektrum DX7/AR6200
00069         false,    // Futaba Thr 3 Sw 6/7
00070         true,    // Spektrum DX7/AR6000
00071         true,    // Graupner MX16S
00072         true,    // Graupner DX6i/AR6200
00073         true,    // Futaba Thr 3/R617FS
00074         true,     // Spektrum DX7a/AR7000
00075         false,     // External decoder (Futaba Ch3 Throttle)
00076         true,    // FrSky DJT/D8R-SP
00077         true,    // UNKNOWN using Spektrum DX7/AR7000
00078         true    // custom Tx/Rx combination
00079     };
00080 
00081 // Reference Internal Quadrocopter Channel Order
00082 // 0 Throttle
00083 // 1 Aileron
00084 // 2 Elevator
00085 // 3 Rudder
00086 // 4 Gear
00087 // 5 Aux1
00088 // 6 Aux2
00089 
00090 int8 RMap[CONTROLS];
00091 
00092 int16x8x4Q PPMQ;
00093 int16 PPMQSum[CONTROLS];
00094 int16 RC[CONTROLS], RCp[CONTROLS];
00095 int16 ThrLow, ThrNeutral, ThrHigh;
00096 
00097 boolean RCPositiveEdge;
00098 
00099 void DoRxPolarity(void) {
00100 
00101     RCInterrupt.rise(&RCNullISR);
00102     RCInterrupt.fall(&RCNullISR);
00103 
00104     if ( F.UsingSerialPPM  ) // serial PPM frame from within an Rx
00105         if  ( PPMPosPolarity[P[TxRxType]] )
00106             RCInterrupt.rise(&RCISR);
00107         else
00108             RCInterrupt.fall(&RCISR);
00109     else {
00110         RCInterrupt.rise(&RCISR);
00111         RCInterrupt.fall(&RCISR);
00112     }
00113 
00114 }  // DoRxPolarity
00115 
00116 void InitRC(void) {
00117     static int8 c, q;
00118 
00119     DoRxPolarity();
00120 
00121     SignalCount = -RC_GOOD_BUCKET_MAX;
00122     F.Signal = F.RCNewValues = false;
00123 
00124     for (c = 0; c < RC_CONTROLS; c++) {
00125         PPM[c] = 0;
00126         RC[c] = RCp[c] = RC_NEUTRAL;
00127 
00128         for (q = 0; q <= PPMQMASK; q++)
00129             PPMQ.B[q][c] = RC_NEUTRAL;
00130         PPMQSum[c] = RC_NEUTRAL * 4;
00131     }
00132     PPMQ.Head = 0;
00133 
00134     DesiredRoll = DesiredPitch = DesiredYaw = DesiredThrottle = StickThrottle = 0;
00135     Trim[Roll] = Trim[Pitch] = Trim[Yaw] = 0;
00136 
00137     PPM_Index = PrevEdge = RCGlitches = 0;
00138 } // InitRC
00139 
00140 void MapRC(void) { // re-maps captured PPM to Rx channel sequence
00141     static int8 c;
00142     static int16 LastThrottle, i;
00143     static uint16 Sum;
00144 
00145     LastThrottle = RC[ThrottleC];
00146 
00147     for ( c = 0 ; c < RC_CONTROLS ; c++ ) {
00148         Sum = PPM[c];
00149         PPMQSum[c] -= PPMQ.B[PPMQ.Head][c];
00150         PPMQ.B[PPMQ.Head][c] = Sum;
00151         PPMQSum[c] += Sum;
00152         PPMQ.Head = (PPMQ.Head + 1) & PPMQMASK;
00153     }
00154 
00155     for ( c = 0 ; c < RC_CONTROLS ; c++ ) {
00156         i = Map[P[TxRxType]][c];
00157         RC[c] = ( PPMQSum[i] * RC_MAXIMUM ) / 4000; // needs rethink - throwing away precision
00158     }
00159 
00160     if ( THROTTLE_SLEW_LIMIT > 0 )
00161         RC[ThrottleRC] = SlewLimit(LastThrottle, RC[ThrottleRC], THROTTLE_SLEW_LIMIT);
00162 
00163 } // MapRC
00164 
00165 void CheckSticksHaveChanged(void) {
00166     static boolean Change;
00167     static uint8 c;
00168 
00169     if ( F.ReturnHome || F.Navigate  ) {
00170         if ( mSClock() > mS[StickChangeUpdate] ) {
00171             mS[StickChangeUpdate] = mSClock() + 500;
00172             if ( !F.ForceFailsafe && ( State == InFlight )) {
00173                 Stats[RCFailsafesS]++;
00174                 mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
00175                 mS[DescentUpdate]  = mSClock() + ALT_DESCENT_UPDATE_MS;
00176                 DescentComp = 0; // for no Baro case
00177             }
00178 
00179             F.ForceFailsafe = State == InFlight; // abort if not navigating
00180         }
00181     } else {
00182         if ( mSClock() > mS[StickChangeUpdate] ) {
00183             mS[StickChangeUpdate] = mSClock() + 500;
00184 
00185             Change = false;
00186             for ( c = ThrottleC; c <= (uint8)RTHC; c++ ) {
00187                 Change |= abs( RC[c] - RCp[c]) > RC_STICK_MOVEMENT;
00188                 RCp[c] = RC[c];
00189             }
00190         }
00191 
00192         if ( Change ) {
00193             mS[RxFailsafeTimeout] = mSClock() + RC_NO_CHANGE_TIMEOUT_MS;
00194             mS[NavStateTimeout] = mSClock();
00195             F.ForceFailsafe = false;
00196             if ( FailState == MonitoringRx ) {
00197                 if ( F.LostModel ) {
00198                     Beeper_OFF;
00199                     F.LostModel = false;
00200                     DescentComp = 0;
00201                 }
00202             }
00203         } else
00204             if ( mSClock() > mS[RxFailsafeTimeout] ) {
00205                 if ( !F.ForceFailsafe && ( ( State == InFlight ) || ( ( mSClock() - mS[RxFailsafeTimeout])  > 120000 ) ) ) {
00206                     Stats[RCFailsafesS]++;
00207                     mS[NavStateTimeout] = mSClock() + NAV_RTH_LAND_TIMEOUT_MS;
00208                     mS[DescentUpdate]  = mSClock() + ALT_DESCENT_UPDATE_MS;
00209                     DescentComp = 0; // for no Baro case
00210                     F.ForceFailsafe = true;
00211                 }
00212 
00213                 //    F.ForceFailsafe = State == InFlight; // abort if not navigating
00214             }
00215     }
00216 
00217 } // CheckSticksHaveChanged
00218 
00219 void UpdateControls(void) {
00220     static int16 HoldRoll, HoldPitch, RollPitchScale;
00221     static boolean NewCh5Active;
00222 
00223     F.RCNewValues = false;
00224 
00225     MapRC();                                // remap channel order for specific Tx/Rx
00226 
00227     StickThrottle = RC[ThrottleRC];
00228 
00229     //_________________________________________________________________________________________
00230 
00231     // Navigation
00232 
00233     F.ReturnHome = F.Navigate = F.UsingPolar = false;
00234     NewCh5Active = RC[RTHRC] > RC_NEUTRAL;
00235 
00236     if ( F.UsingPositionHoldLock )
00237         if ( NewCh5Active & !F.Ch5Active )
00238             F.AllowTurnToWP = true;
00239         else
00240             F.AllowTurnToWP = SaveAllowTurnToWP;
00241     else
00242         if ( RC[RTHRC] > ((3L*RC_MAXIMUM)/4) )
00243             F.ReturnHome = true;
00244         else
00245             if ( RC[RTHRC] > (RC_NEUTRAL/2) )
00246                 F.Navigate = true;
00247 
00248     F.Ch5Active = NewCh5Active;
00249 
00250 #ifdef RX6CH
00251     DesiredCamPitchTrim = RC_NEUTRAL;
00252     // NavSensitivity set in ReadParametersPX
00253 #else
00254     DesiredCamPitchTrim = RC[CamPitchRC] - RC_NEUTRAL;
00255     NavSensitivity = RC[NavGainRC];
00256     NavSensitivity = Limit(NavSensitivity, 0, RC_MAXIMUM);
00257 #endif // !RX6CH
00258 
00259     //_________________________________________________________________________________________
00260 
00261     // Altitude Hold
00262 
00263     F.AltHoldEnabled = NavSensitivity > NAV_SENS_ALTHOLD_THRESHOLD;
00264 
00265     if ( NavState == HoldingStation ) { // Manual
00266         if ( StickThrottle < RC_THRES_STOP )    // to deal with usual non-zero EPA
00267             StickThrottle = 0;
00268     } else // Autonomous
00269         if ( F.AllowNavAltitudeHold &&  F.AltHoldEnabled )
00270             StickThrottle = CruiseThrottle;
00271 
00272     if ( (! F.HoldingAlt) && (!(F.Navigate || F.ReturnHome )) ) // cancel any current altitude hold setting
00273         DesiredAltitude = Altitude;
00274 
00275     //_________________________________________________________________________________________
00276 
00277     // Attitude
00278 
00279 #ifdef ATTITUDE_NO_LIMITS
00280     DesiredRoll = RC[RollRC] - RC_NEUTRAL;
00281     DesiredPitch = RC[PitchRC] - RC_NEUTRAL;
00282 #else
00283     RollPitchScale = MAX_ROLL_PITCH - (NavSensitivity >> 2);
00284     DesiredRoll = SRS16((RC[RollRC] - RC_NEUTRAL) * RollPitchScale, 7);
00285     DesiredPitch = SRS16((RC[PitchRC] - RC_NEUTRAL) * RollPitchScale, 7);
00286 #endif // ATTITUDE_NO_LIMITS
00287     DesiredYaw = RC[YawRC] - RC_NEUTRAL;
00288     
00289     AdaptiveYawLPFreq();
00290 
00291     HoldRoll = abs(DesiredRoll - Trim[Roll]);
00292     HoldPitch = abs(DesiredPitch - Trim[Pitch]);
00293     CurrMaxRollPitch = Max(HoldRoll, HoldPitch);
00294 
00295     if ( CurrMaxRollPitch > ATTITUDE_HOLD_LIMIT )
00296         if ( AttitudeHoldResetCount > ATTITUDE_HOLD_RESET_INTERVAL )
00297             F.AttitudeHold = false;
00298         else {
00299             AttitudeHoldResetCount++;
00300             F.AttitudeHold = true;
00301         }
00302     else {
00303         F.AttitudeHold = true;
00304         if ( AttitudeHoldResetCount > 1 )
00305             AttitudeHoldResetCount -= 2;        // Faster decay
00306     }
00307 
00308     //_________________________________________________________________________________________
00309 
00310     // Rx has gone to failsafe
00311 
00312     //zzz  CheckSticksHaveChanged();
00313 
00314     F.NewCommands = true;
00315 
00316 } // UpdateControls
00317 
00318 void CaptureTrims(void)
00319 {     // only used in detecting movement from neutral in hold GPS position
00320     // Trims are invalidated if Nav sensitivity is changed - Answer do not use trims ?
00321     #ifndef TESTING
00322     Trim[Roll] = Limit1(DesiredRoll, NAV_MAX_TRIM);
00323     Trim[Yaw] = Limit1(DesiredPitch, NAV_MAX_TRIM);
00324     Trim[Yaw] = Limit1(DesiredYaw, NAV_MAX_TRIM);
00325     #endif // TESTING
00326 
00327     HoldYaw = 0;
00328 } // CaptureTrims
00329 
00330 void CheckThrottleMoved(void) {
00331     if ( mSClock() < mS[ThrottleUpdate] )
00332         ThrNeutral = DesiredThrottle;
00333     else {
00334         ThrLow = ThrNeutral - THROTTLE_MIDDLE;
00335         ThrLow = Max(ThrLow, THROTTLE_MIN_ALT_HOLD);
00336         ThrHigh = ThrNeutral + THROTTLE_MIDDLE;
00337         if ( ( DesiredThrottle <= ThrLow ) || ( DesiredThrottle >= ThrHigh ) ) {
00338             mS[ThrottleUpdate] = mSClock() + THROTTLE_UPDATE_MS;
00339             F.ThrottleMoving = true;
00340         } else
00341             F.ThrottleMoving = false;
00342     }
00343 } // CheckThrottleMoved
00344 
00345 void ReceiverTest(void) {
00346     static int8 s;
00347 
00348     TxString("\r\nRx Test \r\n");
00349 
00350     TxString("\r\nRx: ");
00351     ShowRxSetup();
00352     TxString("\r\n");
00353 
00354     TxString("RAW Rx frame values - neutrals NOT applied\r\n");
00355     TxString("Channel order is: ");
00356     for ( s = 0; s < RC_CONTROLS; s++)
00357         TxChar(RxChMnem[RMap[s]]);
00358 
00359     if ( F.Signal )
00360         TxString("\r\nSignal OK ");
00361     else
00362         TxString("\r\nSignal FAIL ");
00363     TxVal32(mSClock() - mS[LastValidRx], 0, 0);
00364     TxString(" mS ago\r\n");
00365 
00366     // Be wary as new RC frames are being received as this
00367     // is being displayed so data may be from overlapping frames
00368 
00369     for ( s = 0; s < RC_CONTROLS ; s++ ) {
00370         TxChar(s+'1');
00371         TxString(": ");
00372         TxChar(RxChMnem[RMap[s]]);
00373         TxString(":\t");
00374 
00375         TxVal32((int32)PPM[s] + 1000, 3, 0);
00376         TxChar(HT);
00377         TxVal32(PPM[s] / 10, 0, '%');
00378         if ( ( PPM[s] < -200 ) || ( PPM[s] > 1200 ) )
00379             TxString(" FAIL");
00380 
00381         TxNextLine();
00382     }
00383 
00384     TxString("Glitches:\t");
00385     TxVal32(RCGlitches,0,0);
00386     TxNextLine();
00387 
00388 } // ReceiverTest
00389 
00390 
00391