Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

params.c@1:1e3318a30ddd, 2011-02-25 (annotated)

Committer:
gke
Date:
Fri Feb 25 01:35:24 2011 +0000
Revision:
1:1e3318a30ddd
Parent:
0:62a1c91a859a
Child:
2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.

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