Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
params.c@0:62a1c91a859a, 2011-02-18 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 18 22:28:05 2011 +0000
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
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 ReadParameters(void); |
gke | 0:62a1c91a859a | 24 | void UseDefaultParameters(void); |
gke | 0:62a1c91a859a | 25 | void UpdateWhichParamSet(void); |
gke | 0:62a1c91a859a | 26 | boolean ParameterSanityCheck(void); |
gke | 0:62a1c91a859a | 27 | void InitParameters(void); |
gke | 0:62a1c91a859a | 28 | |
gke | 0:62a1c91a859a | 29 | const uint8 ESCLimits [] = { OUT_MAXIMUM, OUT_HOLGER_MAXIMUM, OUT_X3D_MAXIMUM, OUT_YGEI2C_MAXIMUM }; |
gke | 0:62a1c91a859a | 30 | |
gke | 0:62a1c91a859a | 31 | #ifdef MULTICOPTER |
gke | 0:62a1c91a859a | 32 | #include "uavx_multicopter.h" |
gke | 0:62a1c91a859a | 33 | #else |
gke | 0:62a1c91a859a | 34 | #ifdef HELICOPTER |
gke | 0:62a1c91a859a | 35 | #include "uavx_helicopter.h" |
gke | 0:62a1c91a859a | 36 | #else |
gke | 0:62a1c91a859a | 37 | #ifdef ELEVONS |
gke | 0:62a1c91a859a | 38 | #include "uavx_elevon.h" |
gke | 0:62a1c91a859a | 39 | #else |
gke | 0:62a1c91a859a | 40 | #include "uavx_aileron.h" |
gke | 0:62a1c91a859a | 41 | #endif |
gke | 0:62a1c91a859a | 42 | #endif |
gke | 0:62a1c91a859a | 43 | #endif |
gke | 0:62a1c91a859a | 44 | |
gke | 0:62a1c91a859a | 45 | uint8 ParamSet; |
gke | 0:62a1c91a859a | 46 | boolean ParametersChanged, SaveAllowTurnToWP; |
gke | 0:62a1c91a859a | 47 | |
gke | 0:62a1c91a859a | 48 | int8 P[MAX_PARAMETERS]; |
gke | 0:62a1c91a859a | 49 | real32 K[MAX_PARAMETERS]; // Arm rescaled legacy parameters as appropriate |
gke | 0:62a1c91a859a | 50 | |
gke | 0:62a1c91a859a | 51 | real32 OSin[48], OCos[48]; |
gke | 0:62a1c91a859a | 52 | |
gke | 0:62a1c91a859a | 53 | uint8 Orientation , PolarOrientation; |
gke | 0:62a1c91a859a | 54 | uint8 UAVXAirframe; |
gke | 0:62a1c91a859a | 55 | |
gke | 0:62a1c91a859a | 56 | void Legacy(void) { |
gke | 0:62a1c91a859a | 57 | static uint8 p; |
gke | 0:62a1c91a859a | 58 | |
gke | 0:62a1c91a859a | 59 | for ( p = 0; p <MAX_PARAMETERS; p++ ) // brute force |
gke | 0:62a1c91a859a | 60 | K[p] = (float)P[p]; |
gke | 0:62a1c91a859a | 61 | |
gke | 0:62a1c91a859a | 62 | // Rate Control |
gke | 0:62a1c91a859a | 63 | K[RollKp] *= 2.6; |
gke | 0:62a1c91a859a | 64 | K[RollKi] *= 20.7; |
gke | 0:62a1c91a859a | 65 | K[RollKd] = -K[RollKd] * 0.021; |
gke | 0:62a1c91a859a | 66 | K[RollIntLimit] *= DEGRAD; |
gke | 0:62a1c91a859a | 67 | |
gke | 0:62a1c91a859a | 68 | K[PitchKp] *= 2.6; |
gke | 0:62a1c91a859a | 69 | K[PitchKi] *= 20.7; |
gke | 0:62a1c91a859a | 70 | K[PitchKd] = -K[PitchKd] * 0.021; |
gke | 0:62a1c91a859a | 71 | K[PitchIntLimit] *= DEGRAD; |
gke | 0:62a1c91a859a | 72 | |
gke | 0:62a1c91a859a | 73 | K[YawKp] *= 2.6; |
gke | 0:62a1c91a859a | 74 | K[YawKi] *= 41.4; |
gke | 0:62a1c91a859a | 75 | K[YawKd] = -K[YawKd] * 0.0004; |
gke | 0:62a1c91a859a | 76 | |
gke | 0:62a1c91a859a | 77 | // Angle Control |
gke | 0:62a1c91a859a | 78 | |
gke | 0:62a1c91a859a | 79 | // not yet |
gke | 0:62a1c91a859a | 80 | |
gke | 0:62a1c91a859a | 81 | // Inertial Damping |
gke | 0:62a1c91a859a | 82 | K[VertDampKp] *= 0.1; // one click/MPS |
gke | 0:62a1c91a859a | 83 | K[HorizDampKp] *= 0.1; |
gke | 0:62a1c91a859a | 84 | K[VertDampDecay] *= 0.01; |
gke | 0:62a1c91a859a | 85 | K[HorizDampDecay] *= 0.01; |
gke | 0:62a1c91a859a | 86 | |
gke | 0:62a1c91a859a | 87 | // Altitude Hold |
gke | 0:62a1c91a859a | 88 | K[AltKp] *= 0.625; |
gke | 0:62a1c91a859a | 89 | K[AltKi] *= 25.0; |
gke | 0:62a1c91a859a | 90 | K[AltKd] *= 0.125; |
gke | 0:62a1c91a859a | 91 | |
gke | 0:62a1c91a859a | 92 | // Navigation |
gke | 0:62a1c91a859a | 93 | K[NavKi] *= 0.08; |
gke | 0:62a1c91a859a | 94 | K[NavKd] *= 0.0008; |
gke | 0:62a1c91a859a | 95 | |
gke | 0:62a1c91a859a | 96 | K[Balance] = ( 128.0 + (float)P[Balance])/128.0; |
gke | 0:62a1c91a859a | 97 | |
gke | 0:62a1c91a859a | 98 | K[CompassKp] = P[CompassKp] / 4096.0; |
gke | 0:62a1c91a859a | 99 | K[YawIntLimit] = P[YawIntLimit] * 256.0 /1000.0; |
gke | 0:62a1c91a859a | 100 | |
gke | 0:62a1c91a859a | 101 | // Camera |
gke | 0:62a1c91a859a | 102 | K[CamRollKp] *= 5.0; |
gke | 0:62a1c91a859a | 103 | K[CamPitchKp] *= 5.0; |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | // Acceleration Neutrals |
gke | 0:62a1c91a859a | 106 | K[MiddleBF] = P[MiddleBF] * 0.001; // mG |
gke | 0:62a1c91a859a | 107 | K[MiddleLR] = P[MiddleLR] * 0.001; |
gke | 0:62a1c91a859a | 108 | K[MiddleUD] = P[MiddleUD] * 0.001; |
gke | 0:62a1c91a859a | 109 | |
gke | 0:62a1c91a859a | 110 | K[LowVoltThres] *= 0.2; |
gke | 0:62a1c91a859a | 111 | |
gke | 0:62a1c91a859a | 112 | } // Legacy |
gke | 0:62a1c91a859a | 113 | |
gke | 0:62a1c91a859a | 114 | void ReadParameters(void) { |
gke | 0:62a1c91a859a | 115 | static int8 i, b, a; |
gke | 0:62a1c91a859a | 116 | |
gke | 0:62a1c91a859a | 117 | if ( ParametersChanged ) { // overkill if only a single parameter has changed but is not in flight loop |
gke | 0:62a1c91a859a | 118 | a = (ParamSet - 1)* MAX_PARAMETERS; |
gke | 0:62a1c91a859a | 119 | for ( i = 0; i < MAX_PARAMETERS; i++) |
gke | 0:62a1c91a859a | 120 | P[i] = ReadPX(a + i); |
gke | 0:62a1c91a859a | 121 | |
gke | 0:62a1c91a859a | 122 | Legacy(); |
gke | 0:62a1c91a859a | 123 | |
gke | 0:62a1c91a859a | 124 | ESCMax = ESCLimits[P[ESCType]]; |
gke | 0:62a1c91a859a | 125 | if ( P[ESCType] == ESCPPM ) |
gke | 0:62a1c91a859a | 126 | ESCMin = 1; |
gke | 0:62a1c91a859a | 127 | else { |
gke | 0:62a1c91a859a | 128 | ESCMin = 0; |
gke | 0:62a1c91a859a | 129 | for ( i = 0; i < NoOfI2CESCOutputs; i++ ) |
gke | 0:62a1c91a859a | 130 | ESCI2CFail[i] = 0; |
gke | 0:62a1c91a859a | 131 | InitI2CESCs(); |
gke | 0:62a1c91a859a | 132 | } |
gke | 0:62a1c91a859a | 133 | |
gke | 0:62a1c91a859a | 134 | b = P[ServoSense]; |
gke | 0:62a1c91a859a | 135 | for ( i = 0; i < 8; i++ ) { |
gke | 0:62a1c91a859a | 136 | if ( b & 1 ) |
gke | 0:62a1c91a859a | 137 | PWMSense[i] = -1; |
gke | 0:62a1c91a859a | 138 | else |
gke | 0:62a1c91a859a | 139 | PWMSense[i] = 1; |
gke | 0:62a1c91a859a | 140 | b >>=1; |
gke | 0:62a1c91a859a | 141 | } |
gke | 0:62a1c91a859a | 142 | |
gke | 0:62a1c91a859a | 143 | F.UsingPositionHoldLock = ( (P[ConfigBits] & UsePositionHoldLockMask ) != 0); |
gke | 0:62a1c91a859a | 144 | F.UsingPolarCoordinates = ( (P[ConfigBits] & UsePolarMask ) != 0); |
gke | 0:62a1c91a859a | 145 | |
gke | 0:62a1c91a859a | 146 | for ( i = 0; i < CONTROLS; i++) // make reverse map |
gke | 0:62a1c91a859a | 147 | RMap[Map[P[TxRxType]][i]] = i; |
gke | 0:62a1c91a859a | 148 | |
gke | 0:62a1c91a859a | 149 | IdleThrottle = Limit((int16)P[PercentIdleThr], 10, 30); // 10-30% |
gke | 0:62a1c91a859a | 150 | IdleThrottle = (IdleThrottle * OUT_MAXIMUM )/100L; |
gke | 0:62a1c91a859a | 151 | CruiseThrottle = ((int16)P[PercentCruiseThr] * OUT_MAXIMUM )/100L; |
gke | 0:62a1c91a859a | 152 | MaxCruiseThrottle = (RC_MAXIMUM * 60L * OUT_MAXIMUM)/100L; // 60% |
gke | 0:62a1c91a859a | 153 | |
gke | 0:62a1c91a859a | 154 | NavNeutralRadius = Limit((int16)P[NeutralRadius], 0, NAV_MAX_NEUTRAL_RADIUS); |
gke | 0:62a1c91a859a | 155 | NavNeutralRadius = ConvertMToGPS(NavNeutralRadius); |
gke | 0:62a1c91a859a | 156 | |
gke | 0:62a1c91a859a | 157 | NavYCorrLimit = Limit((int16)P[NavYawLimit], 5, 50); |
gke | 0:62a1c91a859a | 158 | |
gke | 0:62a1c91a859a | 159 | MagDeviation = (real32)P[NavMagVar] * DEGRAD; |
gke | 0:62a1c91a859a | 160 | CompassOffset = ((real32)((int16)P[CompassOffsetQtr] * 90L) * DEGRAD ); |
gke | 0:62a1c91a859a | 161 | InitCompass(); |
gke | 0:62a1c91a859a | 162 | |
gke | 0:62a1c91a859a | 163 | #ifdef MULTICOPTER |
gke | 0:62a1c91a859a | 164 | Orientation = P[Orient]; |
gke | 0:62a1c91a859a | 165 | if (Orientation == 0xff ) // uninitialised |
gke | 0:62a1c91a859a | 166 | Orientation = 0; |
gke | 0:62a1c91a859a | 167 | #else |
gke | 0:62a1c91a859a | 168 | Orientation = 0; |
gke | 0:62a1c91a859a | 169 | #endif // MULTICOPTER |
gke | 0:62a1c91a859a | 170 | |
gke | 0:62a1c91a859a | 171 | F.UsingSerialPPM = ( P[TxRxType] == FrSkyDJT_D8R ) || ( P[TxRxType] == ExternalDecoder ) || ( (P[ConfigBits] & RxSerialPPMMask ) != 0); |
gke | 0:62a1c91a859a | 172 | DoRxPolarity(); |
gke | 0:62a1c91a859a | 173 | PPM_Index = PrevEdge = 0; |
gke | 0:62a1c91a859a | 174 | |
gke | 0:62a1c91a859a | 175 | F.UsingPolar = ((P[ConfigBits] & UsePolarMask) != 0); |
gke | 0:62a1c91a859a | 176 | F.RFInInches = ((P[ConfigBits] & RFInchesMask) != 0); |
gke | 0:62a1c91a859a | 177 | |
gke | 0:62a1c91a859a | 178 | F.UsingTxMode2 = ((P[ConfigBits] & TxMode2Mask) != 0); |
gke | 0:62a1c91a859a | 179 | |
gke | 0:62a1c91a859a | 180 | if ( P[GyroRollPitchType] == IRSensors ) |
gke | 0:62a1c91a859a | 181 | F.UsingAngleControl = true; |
gke | 0:62a1c91a859a | 182 | else |
gke | 0:62a1c91a859a | 183 | F.UsingAngleControl = ((P[ConfigBits] & UseAngleControlMask) != 0); |
gke | 0:62a1c91a859a | 184 | |
gke | 0:62a1c91a859a | 185 | F.UsingRTHAutoDescend = ((P[ConfigBits] & UseRTHDescendMask) != 0); |
gke | 0:62a1c91a859a | 186 | NavRTHTimeoutmS = (uint24)P[DescentDelayS]*1000L; |
gke | 0:62a1c91a859a | 187 | |
gke | 0:62a1c91a859a | 188 | BatteryVolts = K[LowVoltThres]; |
gke | 0:62a1c91a859a | 189 | BatteryCurrent = 0; |
gke | 0:62a1c91a859a | 190 | |
gke | 0:62a1c91a859a | 191 | F.ParametersValid = ParameterSanityCheck(); |
gke | 0:62a1c91a859a | 192 | |
gke | 0:62a1c91a859a | 193 | ParametersChanged = false; |
gke | 0:62a1c91a859a | 194 | |
gke | 0:62a1c91a859a | 195 | } |
gke | 0:62a1c91a859a | 196 | } // ReadParameters |
gke | 0:62a1c91a859a | 197 | |
gke | 0:62a1c91a859a | 198 | void UseDefaultParameters(void) { // loads a representative set of initial parameters as a base for tuning |
gke | 0:62a1c91a859a | 199 | int8 p, d; |
gke | 0:62a1c91a859a | 200 | static int16 a; |
gke | 0:62a1c91a859a | 201 | |
gke | 0:62a1c91a859a | 202 | for ( a = 0; a < PX_LENGTH; a++ ) |
gke | 0:62a1c91a859a | 203 | PX[a] = 0xff; |
gke | 0:62a1c91a859a | 204 | |
gke | 0:62a1c91a859a | 205 | for ( p = 0; p < MAX_PARAMETERS; p++ ) { |
gke | 0:62a1c91a859a | 206 | d = DefaultParams[p][0]; |
gke | 0:62a1c91a859a | 207 | WritePX(p, d); |
gke | 0:62a1c91a859a | 208 | WritePX(p+MAX_PARAMETERS, d); |
gke | 0:62a1c91a859a | 209 | } |
gke | 0:62a1c91a859a | 210 | |
gke | 0:62a1c91a859a | 211 | WritePX(NAV_NO_WP, 0); // set NoOfWaypoints to zero |
gke | 0:62a1c91a859a | 212 | |
gke | 0:62a1c91a859a | 213 | WritePXImagefile(); |
gke | 0:62a1c91a859a | 214 | |
gke | 0:62a1c91a859a | 215 | TxString("\r\nDefault Parameters Loaded\r\n"); |
gke | 0:62a1c91a859a | 216 | TxString("Do a READ CONFIG to refresh the UAVPSet parameter display\r\n"); |
gke | 0:62a1c91a859a | 217 | } // UseDefaultParameters |
gke | 0:62a1c91a859a | 218 | |
gke | 0:62a1c91a859a | 219 | void UpdateParamSetChoice(void) { |
gke | 0:62a1c91a859a | 220 | #define STICK_WINDOW 30 |
gke | 0:62a1c91a859a | 221 | |
gke | 0:62a1c91a859a | 222 | uint8 NewParamSet, NewAllowNavAltitudeHold, NewAllowTurnToWP; |
gke | 0:62a1c91a859a | 223 | int8 Selector; |
gke | 0:62a1c91a859a | 224 | |
gke | 0:62a1c91a859a | 225 | NewParamSet = ParamSet; |
gke | 0:62a1c91a859a | 226 | NewAllowNavAltitudeHold = F.AllowNavAltitudeHold; |
gke | 0:62a1c91a859a | 227 | NewAllowTurnToWP = F.AllowTurnToWP; |
gke | 0:62a1c91a859a | 228 | |
gke | 0:62a1c91a859a | 229 | if ( F.UsingTxMode2 ) |
gke | 0:62a1c91a859a | 230 | Selector = DesiredRoll; |
gke | 0:62a1c91a859a | 231 | else |
gke | 0:62a1c91a859a | 232 | Selector = -DesiredYaw; |
gke | 0:62a1c91a859a | 233 | |
gke | 0:62a1c91a859a | 234 | if ( (abs(DesiredPitch) > STICK_WINDOW) && (abs(Selector) > STICK_WINDOW) ) { |
gke | 0:62a1c91a859a | 235 | if ( DesiredPitch > STICK_WINDOW ) { // bottom |
gke | 0:62a1c91a859a | 236 | if ( Selector < -STICK_WINDOW ) // left |
gke | 0:62a1c91a859a | 237 | { // bottom left |
gke | 0:62a1c91a859a | 238 | NewParamSet = 1; |
gke | 0:62a1c91a859a | 239 | NewAllowNavAltitudeHold = true; |
gke | 0:62a1c91a859a | 240 | } else |
gke | 0:62a1c91a859a | 241 | if ( Selector > STICK_WINDOW ) // right |
gke | 0:62a1c91a859a | 242 | { // bottom right |
gke | 0:62a1c91a859a | 243 | NewParamSet = 2; |
gke | 0:62a1c91a859a | 244 | NewAllowNavAltitudeHold = true; |
gke | 0:62a1c91a859a | 245 | } |
gke | 0:62a1c91a859a | 246 | } else |
gke | 0:62a1c91a859a | 247 | if ( DesiredPitch < -STICK_WINDOW ) { // top |
gke | 0:62a1c91a859a | 248 | if ( Selector < -STICK_WINDOW ) { // left |
gke | 0:62a1c91a859a | 249 | NewAllowNavAltitudeHold = false; |
gke | 0:62a1c91a859a | 250 | NewParamSet = 1; |
gke | 0:62a1c91a859a | 251 | } else |
gke | 0:62a1c91a859a | 252 | if ( Selector > STICK_WINDOW ) { // right |
gke | 0:62a1c91a859a | 253 | NewAllowNavAltitudeHold = false; |
gke | 0:62a1c91a859a | 254 | NewParamSet = 2; |
gke | 0:62a1c91a859a | 255 | } |
gke | 0:62a1c91a859a | 256 | } |
gke | 0:62a1c91a859a | 257 | |
gke | 0:62a1c91a859a | 258 | if ( ( NewParamSet != ParamSet ) || ( NewAllowNavAltitudeHold != F.AllowNavAltitudeHold ) ) { |
gke | 0:62a1c91a859a | 259 | ParamSet = NewParamSet; |
gke | 0:62a1c91a859a | 260 | F.AllowNavAltitudeHold = NewAllowNavAltitudeHold; |
gke | 0:62a1c91a859a | 261 | LEDBlue_ON; |
gke | 0:62a1c91a859a | 262 | DoBeep100mS(2, 2); |
gke | 0:62a1c91a859a | 263 | if ( ParamSet == (uint8)2 ) |
gke | 0:62a1c91a859a | 264 | DoBeep100mS(2, 2); |
gke | 0:62a1c91a859a | 265 | if ( F.AllowNavAltitudeHold ) |
gke | 0:62a1c91a859a | 266 | DoBeep100mS(4, 4); |
gke | 0:62a1c91a859a | 267 | ParametersChanged |= true; |
gke | 0:62a1c91a859a | 268 | Beeper_OFF; |
gke | 0:62a1c91a859a | 269 | LEDBlue_OFF; |
gke | 0:62a1c91a859a | 270 | } |
gke | 0:62a1c91a859a | 271 | } |
gke | 0:62a1c91a859a | 272 | |
gke | 0:62a1c91a859a | 273 | if ( F.UsingTxMode2 ) |
gke | 0:62a1c91a859a | 274 | Selector = -DesiredYaw; |
gke | 0:62a1c91a859a | 275 | else |
gke | 0:62a1c91a859a | 276 | Selector = DesiredRoll; |
gke | 0:62a1c91a859a | 277 | |
gke | 0:62a1c91a859a | 278 | if ( (abs(RC[ThrottleC]) < STICK_WINDOW) && (abs(Selector) > STICK_WINDOW ) ) { |
gke | 0:62a1c91a859a | 279 | if ( Selector < -STICK_WINDOW ) // left |
gke | 0:62a1c91a859a | 280 | NewAllowTurnToWP = false; |
gke | 0:62a1c91a859a | 281 | else |
gke | 0:62a1c91a859a | 282 | if ( Selector > STICK_WINDOW ) // left |
gke | 0:62a1c91a859a | 283 | NewAllowTurnToWP = true; // right |
gke | 0:62a1c91a859a | 284 | |
gke | 0:62a1c91a859a | 285 | if ( NewAllowTurnToWP != F.AllowTurnToWP ) { |
gke | 0:62a1c91a859a | 286 | F.AllowTurnToWP = NewAllowTurnToWP; |
gke | 0:62a1c91a859a | 287 | LEDBlue_ON; |
gke | 0:62a1c91a859a | 288 | // if ( F.AllowTurnToWP ) |
gke | 0:62a1c91a859a | 289 | DoBeep100mS(4, 2); |
gke | 0:62a1c91a859a | 290 | |
gke | 0:62a1c91a859a | 291 | LEDBlue_OFF; |
gke | 0:62a1c91a859a | 292 | } |
gke | 0:62a1c91a859a | 293 | } |
gke | 0:62a1c91a859a | 294 | |
gke | 0:62a1c91a859a | 295 | SaveAllowTurnToWP = F.AllowTurnToWP; |
gke | 0:62a1c91a859a | 296 | |
gke | 0:62a1c91a859a | 297 | } // UpdateParamSetChoice |
gke | 0:62a1c91a859a | 298 | |
gke | 0:62a1c91a859a | 299 | boolean ParameterSanityCheck(void) { |
gke | 0:62a1c91a859a | 300 | static boolean Fail; |
gke | 0:62a1c91a859a | 301 | |
gke | 0:62a1c91a859a | 302 | Fail = (P[RollKp] == 0) || |
gke | 0:62a1c91a859a | 303 | (P[PitchKp] == 0) || |
gke | 0:62a1c91a859a | 304 | (P[YawKp] == 0); |
gke | 0:62a1c91a859a | 305 | |
gke | 0:62a1c91a859a | 306 | return ( !Fail ); |
gke | 0:62a1c91a859a | 307 | } // ParameterSanityCheck |
gke | 0:62a1c91a859a | 308 | |
gke | 0:62a1c91a859a | 309 | void InitParameters(void) { |
gke | 0:62a1c91a859a | 310 | static int8 i; |
gke | 0:62a1c91a859a | 311 | static real32 A; |
gke | 0:62a1c91a859a | 312 | |
gke | 0:62a1c91a859a | 313 | F.ParametersValid = false; |
gke | 0:62a1c91a859a | 314 | if ( !ReadPXImagefile() ) |
gke | 0:62a1c91a859a | 315 | UseDefaultParameters(); |
gke | 0:62a1c91a859a | 316 | |
gke | 0:62a1c91a859a | 317 | UAVXAirframe = AF_TYPE; |
gke | 0:62a1c91a859a | 318 | |
gke | 0:62a1c91a859a | 319 | for (i = 0; i < 48; i++) { |
gke | 0:62a1c91a859a | 320 | A = ((real32)i * PI)/24.0; |
gke | 0:62a1c91a859a | 321 | OSin[i] = sin(A); |
gke | 0:62a1c91a859a | 322 | OCos[i] = cos(A); |
gke | 0:62a1c91a859a | 323 | } |
gke | 0:62a1c91a859a | 324 | Orientation = 0; |
gke | 0:62a1c91a859a | 325 | |
gke | 0:62a1c91a859a | 326 | ALL_LEDS_ON; |
gke | 0:62a1c91a859a | 327 | |
gke | 0:62a1c91a859a | 328 | ParametersChanged = true; |
gke | 0:62a1c91a859a | 329 | ParamSet = 1; |
gke | 0:62a1c91a859a | 330 | ReadParameters(); |
gke | 0:62a1c91a859a | 331 | |
gke | 0:62a1c91a859a | 332 | ALL_LEDS_OFF; |
gke | 0:62a1c91a859a | 333 | } // InitParameters |
gke | 0:62a1c91a859a | 334 | |
gke | 0:62a1c91a859a | 335 |