params.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 ReadParameters(void);
00024 void UseDefaultParameters(void);
00025 void UpdateWhichParamSet(void);
00026 boolean ParameterSanityCheck(void);
00027 void InitParameters(void);
00028 
00029 const uint8 ESCLimits [] = { OUT_MAXIMUM, OUT_HOLGER_MAXIMUM, OUT_X3D_MAXIMUM, OUT_YGEI2C_MAXIMUM };
00030 
00031 #ifdef MULTICOPTER
00032 #include "uavx_multicopter.h"
00033 #else
00034 #ifdef HELICOPTER
00035 #include "uavx_helicopter.h"
00036 #else
00037 #ifdef ELEVONS
00038 #include "uavx_elevon.h"
00039 #else
00040 #include "uavx_aileron.h"
00041 #endif
00042 #endif
00043 #endif
00044 
00045 uint8 ParamSet;
00046 boolean ParametersChanged, SaveAllowTurnToWP;
00047 
00048 int8 P[MAX_PARAMETERS];
00049 real32 K[MAX_PARAMETERS]; // Arm rescaled legacy parameters as appropriate
00050 
00051 real32 OSin[48], OCos[48];
00052 
00053 uint8 Orientation , PolarOrientation;
00054 uint8 UAVXAirframe;
00055 
00056 void Legacy(void) {
00057     static uint8 p;
00058     
00059     for ( p = 0; p < MAX_PARAMETERS; p++ ) // brute force
00060         K[p] = (float)P[p];
00061         
00062     GRollKp = K[RollKp];
00063     GRollKi = K[RollKi];
00064     GRollKd = K[RollKd];
00065 
00066     GPitchKp = K[PitchKp];
00067     GPitchKi = K[PitchKi];
00068     GPitchKd = K[PitchKd];
00069 
00070     K[RollIntLimit] *= (DEGRAD * 10.0);
00071     K[PitchIntLimit] *= (DEGRAD * 10.0);
00072 
00073     // Altitude Hold
00074     K[AltKp] *= 0.625;
00075     K[AltKi] *= 25.0;
00076     K[AltKd] *= 0.125;
00077 
00078     // Navigation
00079     K[NavKi] *= 0.08;
00080     K[NavKd] *= 0.0008;
00081     
00082     K[Balance] = ( 128.0 + (float)P[Balance])/128.0;
00083     
00084     K[CompassKp] = P[CompassKp] * 0.01;
00085     
00086     K[YawKp] *= 2.6;
00087     K[YawKi] *= 4.14; // was 41.4
00088     
00089     K[YawIntLimit] = P[YawIntLimit] * 256.0 /1000.0;
00090 
00091     // Camera
00092     K[CamRollKp] *= 5.0;
00093     K[CamPitchKp] *= 5.0;
00094 
00095     // Acceleration Neutrals
00096     K[MiddleBF] = P[MiddleBF] * 0.001; // mG
00097     K[MiddleLR] = P[MiddleLR] * 0.001;
00098     K[MiddleUD] = P[MiddleUD] * 0.001;
00099 
00100     K[LowVoltThres] *= 0.2;
00101 
00102 } // Legacy
00103 
00104 void ReadParameters(void) {
00105     static int8 i, b, a;
00106 
00107     if ( ParametersChanged ) {  // overkill if only a single parameter has changed but is not in flight loop
00108         a = (ParamSet - 1)* MAX_PARAMETERS;
00109         for ( i = 0; i < MAX_PARAMETERS; i++)
00110             P[i] = ReadPX(a + i);
00111 
00112         Legacy();
00113 
00114         ESCMax = ESCLimits[P[ESCType]];
00115         if ( P[ESCType] == ESCPPM )
00116             ESCMin = 1;
00117         else {
00118             ESCMin = 0;
00119             for ( i = 0; i < NoOfI2CESCOutputs; i++ )
00120                 ESCI2CFail[i] = 0;
00121             InitI2CESCs();
00122         }
00123 
00124         b = P[ServoSense];
00125         for ( i = 0; i < 8; i++ ) {
00126             if ( b & 1 )
00127                 PWMSense[i] = -1;
00128             else
00129                 PWMSense[i] = 1;
00130             b >>=1;
00131         }
00132 
00133         F.UsingPositionHoldLock = ( (P[ConfigBits] & UsePositionHoldLockMask ) != 0);
00134         F.UsingPolarCoordinates = ( (P[ConfigBits] & UsePolarMask ) != 0);
00135 
00136         for ( i = 0; i < CONTROLS; i++) // make reverse map
00137             RMap[Map[P[TxRxType]][i]] = i;
00138 
00139         IdleThrottle = Limit((int16)P[PercentIdleThr], 10, 30); // 10-30%
00140         IdleThrottle = (IdleThrottle * OUT_MAXIMUM )/100L;
00141         CruiseThrottle = ((int16)P[PercentCruiseThr] * OUT_MAXIMUM )/100L;
00142         MaxCruiseThrottle = (RC_MAXIMUM * 60L * OUT_MAXIMUM)/100L; // 60%
00143 
00144         NavNeutralRadius = Limit((int16)P[NeutralRadius], 0, NAV_MAX_NEUTRAL_RADIUS);
00145         NavNeutralRadius = ConvertMToGPS(NavNeutralRadius);
00146 
00147         NavYCorrLimit = Limit((int16)P[NavYawLimit], 5, 50);
00148 
00149         MagDeviation = (real32)P[NavMagVar] * DEGRAD;
00150         CompassOffset = ((real32)((int16)P[CompassOffsetQtr] * 90L) * DEGRAD );
00151         InitCompass();
00152 
00153 #ifdef MULTICOPTER
00154         Orientation = P[Orient];
00155         if (Orientation == 0xff ) // uninitialised
00156             Orientation = 0;
00157 #else
00158         Orientation = 0;
00159 #endif // MULTICOPTER
00160 
00161         F.UsingSerialPPM = ( P[TxRxType] == FrSkyDJT_D8R ) || ( P[TxRxType] == ExternalDecoder ) || ( (P[ConfigBits] & RxSerialPPMMask ) != 0);
00162         DoRxPolarity();
00163         PPM_Index = PrevEdge = 0;
00164 
00165         F.UsingPolar = ((P[ConfigBits] & UsePolarMask) != 0);
00166         F.RFInInches = ((P[ConfigBits] & RFInchesMask) != 0);
00167 
00168         F.UsingTxMode2 = ((P[ConfigBits] & TxMode2Mask) != 0);
00169 
00170         if ( P[GyroRollPitchType] == IRSensors )
00171             F.UsingAngleControl = true;
00172         else
00173             F.UsingAngleControl = ((P[ConfigBits] & UseAngleControlMask) != 0);
00174 
00175         F.UsingRTHAutoDescend = ((P[ConfigBits] & UseRTHDescendMask) != 0);
00176         NavRTHTimeoutmS = (uint24)P[DescentDelayS]*1000L;
00177 
00178         BatteryVolts = K[LowVoltThres];
00179         BatteryCurrent = 0;
00180 
00181         F.ParametersValid = ParameterSanityCheck();
00182 
00183         ParametersChanged = false;
00184 
00185     }
00186 } // ReadParameters
00187 
00188 void UseDefaultParameters(void) { // loads a representative set of initial parameters as a base for tuning
00189     int8 p, d;
00190     static int16 a;
00191 
00192     for ( a = 0; a < PX_LENGTH; a++ )
00193         PX[a] = 0xff;
00194 
00195     for ( p = 0; p < MAX_PARAMETERS; p++ ) {
00196         d = DefaultParams[p][0];
00197         WritePX(p, d);
00198         WritePX(p+MAX_PARAMETERS, d);
00199     }
00200 
00201     WritePX(NAV_NO_WP, 0); // set NoOfWaypoints to zero
00202 
00203     WritePXImagefile();
00204 
00205     TxString("\r\nDefault Parameters Loaded\r\n");
00206     TxString("Do a READ CONFIG to refresh the UAVPSet parameter display\r\n");
00207 } // UseDefaultParameters
00208 
00209 void UpdateParamSetChoice(void) {
00210 #define STICK_WINDOW 30
00211 
00212     uint8 NewParamSet, NewAllowNavAltitudeHold, NewAllowTurnToWP;
00213     int8 Selector;
00214 
00215     NewParamSet = ParamSet;
00216     NewAllowNavAltitudeHold = F.AllowNavAltitudeHold;
00217     NewAllowTurnToWP = F.AllowTurnToWP;
00218 
00219     if ( F.UsingTxMode2 )
00220         Selector = DesiredRoll;
00221     else
00222         Selector = -DesiredYaw;
00223 
00224     if ( (abs(DesiredPitch) > STICK_WINDOW) && (abs(Selector) > STICK_WINDOW) ) {
00225         if ( DesiredPitch > STICK_WINDOW ) { // bottom
00226             if ( Selector < -STICK_WINDOW ) // left
00227             { // bottom left
00228                 NewParamSet = 1;
00229                 NewAllowNavAltitudeHold = true;
00230             } else
00231                 if ( Selector > STICK_WINDOW ) // right
00232                 { // bottom right
00233                     NewParamSet = 2;
00234                     NewAllowNavAltitudeHold = true;
00235                 }
00236         } else
00237             if ( DesiredPitch < -STICK_WINDOW ) { // top
00238                 if ( Selector < -STICK_WINDOW ) { // left
00239                     NewAllowNavAltitudeHold = false;
00240                     NewParamSet = 1;
00241                 } else
00242                     if ( Selector > STICK_WINDOW ) { // right
00243                         NewAllowNavAltitudeHold = false;
00244                         NewParamSet = 2;
00245                     }
00246             }
00247 
00248         if ( ( NewParamSet != ParamSet ) || ( NewAllowNavAltitudeHold != F.AllowNavAltitudeHold ) ) {
00249             ParamSet = NewParamSet;
00250             F.AllowNavAltitudeHold = NewAllowNavAltitudeHold;
00251             LEDBlue_ON;
00252             DoBeep100mS(2, 2);
00253             if ( ParamSet == (uint8)2 )
00254                 DoBeep100mS(2, 2);
00255             if ( F.AllowNavAltitudeHold )
00256                 DoBeep100mS(4, 4);
00257             ParametersChanged |= true;
00258        //zzz     Beeper_OFF;
00259             LEDBlue_OFF;
00260         }
00261     }
00262 
00263     if ( F.UsingTxMode2 )
00264         Selector = -DesiredYaw;
00265     else
00266         Selector = DesiredRoll;
00267 
00268     if ( (abs(RC[ThrottleC]) < STICK_WINDOW) && (abs(Selector) > STICK_WINDOW ) ) {
00269         if ( Selector < -STICK_WINDOW ) // left
00270             NewAllowTurnToWP = false;
00271         else
00272             if ( Selector > STICK_WINDOW ) // left
00273                 NewAllowTurnToWP = true; // right
00274 
00275         if ( NewAllowTurnToWP != F.AllowTurnToWP ) {
00276             F.AllowTurnToWP = NewAllowTurnToWP;
00277             LEDBlue_ON;
00278             //    if ( F.AllowTurnToWP )
00279             DoBeep100mS(4, 2);
00280 
00281             LEDBlue_OFF;
00282         }
00283     }
00284 
00285     SaveAllowTurnToWP = F.AllowTurnToWP;
00286 
00287 } // UpdateParamSetChoice
00288 
00289 boolean ParameterSanityCheck(void) {
00290     static boolean Fail;
00291 
00292     Fail = (P[RollKp] == 0) ||
00293            (P[PitchKp] == 0) ||
00294            (P[YawKp] == 0);
00295 
00296     return ( !Fail );
00297 } // ParameterSanityCheck
00298 
00299 void InitParameters(void) {
00300     static int8 i;
00301     static real32 A;
00302 
00303     F.ParametersValid = false;
00304     if ( !ReadPXImagefile() )
00305         UseDefaultParameters();
00306 
00307     UAVXAirframe = AF_TYPE;
00308 
00309     for (i = 0; i < 48; i++) {
00310         A = ((real32)i * PI)/24.0;
00311         OSin[i] = sin(A);
00312         OCos[i] = cos(A);
00313     }
00314     Orientation = 0;
00315 
00316     ALL_LEDS_ON;
00317 
00318     ParametersChanged = true;
00319     ParamSet = 1;
00320     ReadParameters();
00321 
00322     ALL_LEDS_OFF;
00323 } // InitParameters
00324 
00325