Prof Greg Egan
UAVX Multicopter Flight Controller.
Diff: control.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
--- a/control.c Fri Feb 25 01:35:24 2011 +0000
+++ b/control.c Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
// = UAVXArm Quadrocopter Controller =
// = Copyright (c) 2008 by Prof. Greg Egan =
// = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
-// = http://code.google.com/p/uavp-mods/ http://uavp.ch =
+// = http://code.google.com/p/uavp-mods/ =
// ===============================================================================================
// This is part of UAVXArm.
@@ -20,12 +20,10 @@
#include "UAVXArm.h"
-real32 PTerm, ITerm, DTerm;
-
void DoAltitudeHold(void);
void UpdateAltitudeSource(void);
+real32 AltitudeCF( real32, real32);
void AltitudeHold(void);
-void InertialDamping(void);
void DoOrientationTransform(void);
void DoControl(void);
@@ -33,20 +31,20 @@
void LightsAndSirens(void);
void InitControl(void);
-real32 Angle[3], Anglep[3], Rate[3], Ratep[3]; // Milliradians
-real32 Comp[4];
+real32 Angle[3], Anglep[3], Rate[3], Ratep[3], YawRateIntE;
+real32 AccAltComp, AltComp;
real32 DescentComp;
-real32 AngleE[3], AngleIntE[3];
-
real32 GS;
real32 Rl, Pl, Yl, Ylp;
int16 HoldYaw;
int16 CruiseThrottle, MaxCruiseThrottle, DesiredThrottle, IdleThrottle, InitialThrottle, StickThrottle;
-int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredHeading, DesiredCamPitchTrim;
-real32 ControlRoll, ControlPitch, ControlRollP, ControlPitchP;
+int16 DesiredRoll, DesiredPitch, DesiredYaw, DesiredCamPitchTrim;
+real32 DesiredHeading;
+real32 ControlRoll, ControlPitch;
real32 CameraRollAngle, CameraPitchAngle;
int16 CurrMaxRollPitch;
+int16 Trim[3];
int16 AttitudeHoldResetCount;
real32 AltDiffSum, AltD, AltDSum;
@@ -55,12 +53,47 @@
uint32 AltuSp;
int16 DescentLimiter;
+uint32 ControlUpdateTimeuS;
real32 GRollKp, GRollKi, GRollKd, GPitchKp, GPitchKi, GPitchKd;
-boolean FirstPass;
+boolean FirstPass;
int8 BeepTick = 0;
+
+
+real32 AltCF = 0.0;
+real32 AltF[3] = { 0.0, 0.0, 0.0};
+
+real32 AltitudeCF( real32 AltE, real32 dT ) {
+
+ // Complementary Filter originally authored by RoyLB originally for attitude estimation
+ // http://www.rcgroups.com/forums/showpost.php?p=12082524&postcount=1286
+
+ // Using acceleration as an alias for vertical displacement to avoid integration "issues"
+
+ static real32 TauCF;
+
+ TauCF = K[VertDamp];
+
+ if ( F.AccelerationsValid && F.NearLevel ) {
+
+ AltF[0] = (AltE - AltCF) * Sqr(TauCF);
+ AltF[2] += AltF[0] * dT;
+
+ AltF[1] = AltF[2] + (AltE - AltCF) * 2.0 * TauCF + ( 0.5 * Acc[UD] * Sqr(dT) ); // ??? some scaling on Acc perhaps
+ AltCF += AltF[1] * dT;
+
+ } else
+ AltCF = AltE;
+
+ AccAltComp = AltCF - AltE;
+
+ return( AltCF );
+
+} // AltitudeCF
+
+
void DoAltitudeHold(void) { // Syncronised to baro intervals independant of active altitude source
static int16 NewAltComp;
@@ -75,36 +108,32 @@
AltdTR = 1.0 / AltdT;
AltuSp = Now;
- AltE = DesiredAltitude - Altitude;
- LimAltE = Limit(AltE, -ALT_BAND_M, ALT_BAND_M);
+ AltE = AltitudeCF( DesiredAltitude - Altitude, AltdT );
+ LimAltE = Limit1(AltE, ALT_BAND_M);
AltP = LimAltE * K[AltKp];
- AltP = Limit(AltP, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
+ AltP = Limit1(AltP, ALT_MAX_THR_COMP);
AltDiffSum += LimAltE;
- AltDiffSum = Limit(AltDiffSum, -ALT_INT_WINDUP_LIMIT, ALT_INT_WINDUP_LIMIT);
+ AltDiffSum = Limit1(AltDiffSum, ALT_INT_WINDUP_LIMIT);
AltI = AltDiffSum * K[AltKi] * AltdT;
- AltI = Limit(AltDiffSum, -K[AltIntLimit], K[AltIntLimit]);
+ AltI = Limit1(AltDiffSum, K[AltIntLimit]);
ROC = ( Altitude - AltitudeP ) * AltdTR; // may neeed filtering - noisy
AltitudeP = Altitude;
AltD = ROC * K[AltKd];
- AltD = Limit(AltD, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
+ AltD = Limit1(AltD, ALT_MAX_THR_COMP);
if ( ROC < ( -K[MaxDescentRateDmpS] * 10.0 ) ) {
DescentLimiter += 1;
DescentLimiter = Limit(DescentLimiter, 0, ALT_MAX_THR_COMP * 2.0);
-
} else
DescentLimiter = DecayX(DescentLimiter, 1);
NewAltComp = AltP + AltI + AltD + AltDSum + DescentLimiter;
-
- NewAltComp = Limit(NewAltComp, -ALT_MAX_THR_COMP, ALT_MAX_THR_COMP);
-
- Comp[Alt] = SlewLimit(Comp[Alt], NewAltComp, 1.0);
-
+ NewAltComp = Limit1(NewAltComp, ALT_MAX_THR_COMP);
+ AltComp = SlewLimit(AltComp, NewAltComp, 1.0);
if ( ROC > Stats[MaxROCS] )
Stats[MaxROCS] = ROC;
@@ -142,11 +171,11 @@
if ( F.ThrottleMoving ) {
F.HoldingAlt = false;
DesiredAltitude = Altitude;
- Comp[Alt] = Decay1(Comp[Alt]);
+ AltComp = Decay1(AltComp);
} else {
F.HoldingAlt = true;
if ( fabs(ROC) < ALT_HOLD_MAX_ROC_MPS ) {
- NewCruiseThrottle = DesiredThrottle + Comp[Alt];
+ NewCruiseThrottle = DesiredThrottle + AltComp;
CruiseThrottle = HardFilter(CruiseThrottle, NewCruiseThrottle);
CruiseThrottle = Limit( CruiseThrottle , IdleThrottle, MaxCruiseThrottle );
}
@@ -154,54 +183,12 @@
}
}
} else {
- Comp[Alt] = Decay1(Comp[Alt]);
+ AltComp = Decay1(AltComp);
ROC = 0.0;
F.HoldingAlt = false;
}
} // AltitudeHold
-void InertialDamping(void) { // Uses accelerometer to damp disturbances while holding altitude
- static uint8 i;
-
- if ( F.AccelerationsValid && F.NearLevel ) {
- // Up - Down
-
- Vel[UD] += Acc[UD] * dT;
- Comp[UD] = Vel[UD] * K[VertDampKp];
- Comp[UD] = Limit(Comp[UD], DAMP_VERT_LIMIT_LOW, DAMP_VERT_LIMIT_HIGH);
-
- Vel[UD] = DecayX(Vel[UD], K[VertDampDecay]);
-
- // Lateral compensation only when holding altitude
- if ( F.HoldingAlt && F.AttitudeHold ) {
- if ( F.WayPointCentred ) {
- // Left - Right
- Vel[LR] += Acc[LR] * dT;
- Comp[LR] = Vel[LR] * K[HorizDampKp];
- Comp[LR] = Limit(Comp[LR], -DAMP_HORIZ_LIMIT, DAMP_HORIZ_LIMIT);
- Vel[LR] = DecayX(Vel[LR], K[HorizDampDecay]);
-
- // Back - Front
- Vel[BF] += Acc[BF] * dT;
- Comp[BF] = Vel[BF] * K[HorizDampKp];
- Comp[BF] = Limit(Comp[BF], -DAMP_HORIZ_LIMIT, DAMP_HORIZ_LIMIT);
- Vel[BF] = DecayX(Vel[BF], K[HorizDampDecay]);
- } else {
- Vel[LR] = Vel[BF] = 0;
- Comp[LR] = Decay1(Comp[LR]);
- Comp[BF] = Decay1(Comp[BF]);
- }
- } else {
- Vel[LR] = Vel[BF] = 0;
-
- Comp[LR] = Decay1(Comp[LR]);
- Comp[BF] = Decay1(Comp[BF]);
- }
- } else
- for ( i = 0; i < (uint8)3; i++ )
- Comp[i] = Vel[i] = 0.0;
-
-} // InertialDamping
void DoOrientationTransform(void) {
static real32 OSO, OCO;
@@ -222,13 +209,13 @@
// PC+RS
ControlPitch = (int16)( DesiredPitch * OCO + DesiredRoll * OSO );
-
+
CameraRollAngle = Angle[Pitch] * OSO + Angle[Roll] * OCO;
CameraPitchAngle = Angle[Pitch] * OCO - Angle[Roll] * OSO;
} // DoOrientationTransform
-void GainSchedule(boolean UseAngle) {
+void GainSchedule(void) {
/*
// rudimentary gain scheduling (linear)
@@ -258,21 +245,64 @@
GS = 1.0; // Temp
- GRollKp = K[RollKp];
- GRollKi = K[RollKi];
- GRollKd = K[RollKd];
- GPitchKp = K[PitchKp];
- GPitchKi = K[PitchKi];
- GPitchKd = K[PitchKd];
} // GainSchedule
+const real32 RelayKcu = ( 4.0 * 10 )/ ( PI * 0.8235 ); // stimulus 10 => Kcu 15.5
+const real32 RelayPd = 2.0 * 1.285;
+const real32 RelayStim = 3.0;
+
+real32 RelayA = 0.0;
+real32 RelayTau = 0.0;
+uint32 RelayIteration = 0;
+real32 RelayP, RelayW;
+
+void DoRelayTuning(void) {
+
+ static real32 Temp;
+
+ Temp = fabs(Angle[Roll]);
+ if ( Temp > RelayA ) RelayA = Temp;
+
+ if ( ( RelayP < 0.0 ) && ( Angle[Roll] >= 0.0 ) ) {
+
+ RelayTau = RelayIteration * dT;
+
+ RelayP = - (PI * RelayA) / (4.0 * RelayStim);
+ RelayW = (2.0 * PI) / RelayTau;
+
+#ifndef PID_RAW
+ SendPIDTuning();
+#endif // PID_RAW
+
+ RelayA = 0.0;
+ RelayIteration = 0;
+ }
+
+ RelayIteration++;
+ RelayP = Angle[Roll];
+
+} // DoRelayTuning
+
+void Relay(void) {
+
+ if ( Angle[Roll] < 0.0 )
+ Rl = -RelayStim;
+ else
+ Rl = +RelayStim;
+
+ DesiredRoll = Rl;
+
+} // Relay
+
void DoControl(void) {
+ static real32 RateE;
+
+
GetAttitude();
AltitudeHold();
- InertialDamping();
#ifdef SIMULATE
@@ -290,78 +320,70 @@
DoOrientationTransform();
- GainSchedule(F.UsingAngleControl);
+ GainSchedule();
#ifdef DISABLE_EXTRAS
// for commissioning
- Comp[BF] = Comp[LR] = Comp[UD] = Comp[Alt] = 0;
+ AccAltComp = AltComp = 0.0;
NavCorr[Roll] = NavCorr[Pitch] = NavCorr[Yaw] = 0;
- F.UsingAngleControl = false;
#endif // DISABLE_EXTRAS
- if ( F.UsingAngleControl ) {
- // Roll
-
- AngleE[Roll] = ( ControlRoll * ATTITUDE_SCALE ) - Angle[Roll];
- AngleIntE[Roll] += AngleE[Roll] * dT;
- AngleIntE[Roll] = Limit(AngleIntE[Roll], -K[RollIntLimit], K[RollIntLimit]);
- Rl = -(AngleE[Roll] * GRollKp + AngleIntE[Roll] * GRollKi + Rate[Roll] * GRollKd * dTR);
- Rl -= NavCorr[Roll] - Comp[LR];
+ // Roll
- // Pitch
+#ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle
+ RateE = ( Angle[Roll] - Anglep[Roll] ) * dTR;
+#else
+ RateE = Rate[Roll];
+#endif // USE_ANGLE_DERIVED_RATE
+ Rl = RateE * GRollKp + Angle[Roll] * GRollKi + ( RateE - Ratep[Roll] ) * GRollKd * dTR;
+ Rl += ControlRoll + NavCorr[Roll];
- AngleE[Pitch] = ( ControlPitch * ATTITUDE_SCALE ) - Angle[Pitch];
- AngleIntE[Pitch] += AngleE[Pitch] * dT;
- AngleIntE[Pitch] = Limit(AngleIntE[Pitch], -K[PitchIntLimit], K[PitchIntLimit]);
- Pl = -(AngleE[Pitch] * GPitchKp + AngleIntE[Pitch] * GPitchKi + Rate[Pitch] * GPitchKd * dTR);
- Pl -= NavCorr[Pitch] - Comp[BF];
-
- } else {
- // Roll
+#ifdef USE_ANGLE_DERIVED_RATE
+ Ratep[Roll] = RateE;
+ Anglep[Roll] = Angle[Roll];
+#else
+ Ratep[Roll] = Rate[Roll];
+#endif // USE_ANGLE_DERIVED_RATE
- AngleE[Roll] = Limit(Angle[Roll], -K[RollIntLimit], K[RollIntLimit]);
- Rl = Rate[Roll] * GRollKp + AngleE[Roll] * GRollKi + (Rate[Roll]-Ratep[Roll]) * GRollKd * dTR;
- Rl -= NavCorr[Roll] - Comp[LR];
-
- Rl *= GS;
-
- Rl -= ControlRoll;
-
- ControlRollP = ControlRoll;
- Ratep[Roll] = Rate[Roll];
+ // Pitch
- // Pitch
+#ifdef USE_ANGLE_DERIVED_RATE // Gyro rate noisy so compute from angle
+ RateE = ( Angle[Pitch] - Anglep[Pitch] ) * dTR;
+#else
+ RateE = Rate[Pitch];
+#endif // USE_ANGLE_DERIVED_RATE
+ Pl = RateE * GPitchKp + Angle[Pitch] * GPitchKi + ( RateE - Ratep[Pitch] ) * GPitchKd * dTR;
+ Pl += ControlPitch + NavCorr[Pitch];
- AngleE[Pitch] = Limit(Angle[Pitch], -K[PitchIntLimit], K[PitchIntLimit]);
- Pl = Rate[Pitch] * GPitchKp + AngleE[Pitch] * GPitchKi + (Rate[Pitch]-Ratep[Pitch]) * GPitchKd * dTR;
- Pl -= NavCorr[Pitch] - Comp[BF];
-
- Pl *= GS;
-
- Pl -= ControlPitch;
-
- ControlPitchP = ControlPitch;
- Ratep[Pitch] = Rate[Pitch];
- }
+#ifdef USE_ANGLE_DERIVED_RATE
+ Ratep[Pitch] = RateE;
+ Anglep[Pitch] = Angle[Pitch];
+#else
+ Ratep[Pitch] = Rate[Pitch];
+#endif // USE_ANGLE_DERIVED_RATE
// Yaw
- Rate[Yaw] -= NavCorr[Yaw];
- if ( abs(DesiredYaw) > 5 )
- Rate[Yaw] -= DesiredYaw;
+#define MAX_YAW_RATE (HALFPI / RC_NEUTRAL) // Radians/Sec e.g. HalfPI is 90deg/sec
+
+ DoLegacyYawComp(AttitudeMethod); // returns Angle as heading error along with compensated rate
- Yl = Rate[Yaw] * K[YawKp] + Angle[Yaw] * K[YawKi] + (Rate[Yaw]-Ratep[Yaw]) * K[YawKd] * dTR;
- Ratep[Yaw] = Rate[Yaw];
+ RateE = Rate[Yaw] + ( DesiredYaw + NavCorr[Yaw] ) * MAX_YAW_RATE;
+
+ YawRateIntE += RateE;
+ YawRateIntE = Limit1(YawRateIntE, YawIntLimit);
+
+ Yl = RateE * K[YawKp] + YawRateIntE * K[YawKi];
#ifdef TRICOPTER
Yl = SlewLimit(Ylp, Yl, 2.0);
Ylp = Yl;
- Yl = Limit(Yl, -K[YawLimit] * 4.0, K[YawLimit] * 4.0);
+ Yl = Limit1(Yl, K[YawLimit] * 4.0);
#else
- Yl = Limit(Yl, -K[YawLimit], K[YawLimit]);
+ Yl = Limit1(Yl, K[YawLimit]); // currently 25 default
#endif // TRICOPTER
-#endif // SIMULATE
+#endif // SIMULATE
} // DoControl
@@ -435,8 +457,14 @@
AltuSp = DescentLimiter = 0;
for ( i = 0; i < (uint8)3; i++ )
- AngleE[i] = AngleIntE[i] = Angle[i] = Anglep[i] = Rate[i] = Trim[i] = Vel[i] = Comp[i] = 0.0;
+ Angle[i] = Anglep[i] = Rate[i] = Vel[i] = 0.0;
+
+ AccAltComp = AltComp = 0.0;
- Comp[Alt] = AltSum = Ylp = ControlRollP = ControlPitchP = AltitudeP = 0.0;
+ YawRateIntE = 0.0;
+
+ AltSum = Ylp = AltitudeP = 0.0;
+ ControlUpdateTimeuS = 0;
} // InitControl
+