Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 08:15:26 +03:00
Code Issues Wiki Activity

Merge pull request #1978 from iNavFlight/de_fw_tecs_1

Browse source 
Fixed wing TECS controller preparation (stage 1)
This commit is contained in:
Konstantin Sharlaimov 2017-09-06 10:08:39 +10:00 • committed by GitHub
commit d40fb2bdf4
4 changed files with 44 additions and 16 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/flight/imu.h
View file

@ -22,6 +22,7 @@
#include "config/parameter_group.h"
#define GRAVITY_CMSS 980.665f
#define GRAVITY_MSS 9.80665f
extern int16_t throttleAngleCorrection;
extern int16_t smallAngle;

22
src/main/navigation/navigation.c
View file

@ -1331,13 +1331,13 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
// Implementation of PID with back-calculation I-term anti-windup
// Control System Design, Lecture Notes for ME 155A by Karl Johan Åström (p.228)
// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler)
{
float newProportional, newDerivative;
float error = setpoint - measurement;
/* P-term */
newProportional = error * pid->param.kP;
newProportional = error * pid->param.kP * gainScaler;
/* D-term */
if (pidFlags & PID_DTERM_FROM_ERROR) {
@ -1351,19 +1351,19 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
pid->last_input = measurement;
}
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt);
newDerivative = pid->param.kD * pt1FilterApply4(&pid->dterm_filter_state, newDerivative, NAV_DTERM_CUT_HZ, dt) * gainScaler;
if (pidFlags & PID_ZERO_INTEGRATOR) {
pid->integrator = 0.0f;
}
/* Pre-calculate output and limit it if actuator is saturating */
const float outVal = newProportional + pid->integrator + newDerivative;
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
const float outValConstrained = constrainf(outVal, outMin, outMax);
/* Update I-term */
if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
if (pidFlags & PID_SHRINK_INTEGRATOR) {
// Only allow integrator to shrink
@ -1379,6 +1379,12 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
return outValConstrained;
}
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags)
{
return navPidApply3(pid, setpoint, measurement, dt, outMin, outMax, pidFlags, 1.0f);
}
void navPidReset(pidController_t *pid)
{
pid->integrator = 0.0f;
@ -2606,9 +2612,9 @@ void navigationUsePIDs(void)
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 100.0f);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 9.80665f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 9.80665f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 9.80665f);
}
void navigationInit(void)

36
src/main/navigation/navigation_fixedwing.c
View file

@ -103,19 +103,39 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
// On a fixed wing we might not have a reliable climb rate source (if no BARO available), so we can't apply PID controller to
// velocity error. We use PID controller on altitude error and calculate desired pitch angle
// Here we use negative values for dive for better clarity
int maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
int minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// Update energies
const float demSPE = (posControl.desiredState.pos.V.Z / 100.0f) * GRAVITY_MSS;
const float demSKE = 0.0f;
float targetPitchAngle = navPidApply2(&posControl.pids.fw_alt, posControl.desiredState.pos.V.Z, posControl.actualState.pos.V.Z, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0);
const float estSPE = (posControl.actualState.pos.V.Z / 100.0f) * GRAVITY_MSS;
const float estSKE = 0.0f;
// speedWeight controls balance between potential and kinetic energy used for pitch controller
// speedWeight = 1.0 : pitch will only control airspeed and won't control altitude
// speedWeight = 0.5 : pitch will be used to control both airspeed and altitude
// speedWeight = 0.0 : pitch will only control altitude
const float speedWeight = 0.0f; // no speed sensing for now
const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight;
const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight;
// SEB to pitch angle gain to account for airspeed (with respect to specified reference (tuning) speed
const float pitchGainInv = 1.0f / 1.0f;
// Here we use negative values for dive for better clarity
const float maxClimbDeciDeg = DEGREES_TO_DECIDEGREES(navConfig()->fw.max_climb_angle);
const float minDiveDeciDeg = -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_dive_angle);
// PID controller to translate energy balance error [J] into pitch angle [decideg]
float targetPitchAngle = navPidApply3(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0, pitchGainInv);
targetPitchAngle = pt1FilterApply4(&velzFilterState, targetPitchAngle, NAV_FW_PITCH_CUTOFF_FREQENCY_HZ, US2S(deltaMicros));
// Calculate climb angle ( >0 - climb, <0 - dive)
// Reconstrain pitch angle ( >0 - climb, <0 - dive)
targetPitchAngle = constrainf(targetPitchAngle, minDiveDeciDeg, maxClimbDeciDeg);
posControl.rcAdjustment[PITCH] = targetPitchAngle;
}
void applyFixedWingAltitudeController(timeUs_t currentTimeUs)
void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs)
{
static timeUs_t previousTimePositionUpdate; // Occurs @ altitude sensor update rate (max MAX_ALTITUDE_UPDATE_RATE_HZ)
static timeUs_t previousTimeUpdate; // Occurs @ looptime rate
@ -534,7 +554,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
// Don't apply anything if ground speed is too low (<3m/s)
if (posControl.actualState.velXY > 300) {
if (navStateFlags & NAV_CTL_ALT)
applyFixedWingAltitudeController(currentTimeUs);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTimeUs);
@ -545,7 +565,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags,
}
#else
if (navStateFlags & NAV_CTL_ALT)
applyFixedWingAltitudeController(currentTimeUs);
applyFixedWingAltitudeAndThrottleController(currentTimeUs);
if (navStateFlags & NAV_CTL_POS)
applyFixedWingPositionController(currentTimeUs);

1
src/main/navigation/navigation_private.h
View file

@ -296,6 +296,7 @@ extern navigationPosControl_t posControl;
/* Internally used functions */
float navPidApply2(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags);
float navPidApply3(pidController_t *pid, const float setpoint, const float measurement, const float dt, const float outMin, const float outMax, const pidControllerFlags_e pidFlags, const float gainScaler);
void navPidReset(pidController_t *pid);
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD);
void navPInit(pController_t *p, float _kP);