diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0077be706e..b84c32dc43 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1330,13 +1330,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) { @@ -1350,19 +1350,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 @@ -1378,6 +1378,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; @@ -2605,9 +2611,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, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].I, - (float)pidProfile()->bank_fw.pid[PID_POS_Z].D); + 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) diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index cc6faca017..ad46b9a738 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -110,22 +110,24 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros) 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 wSKE = speedWeight; - const float wSPE = 1.0f - speedWeight; - const float demSEB = demSPE * wSPE - demSKE * wSKE; - const float estSEB = estSPE * wSPE - estSKE * wSKE; + 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 - const float pitchGainInv = GRAVITY_MSS; // GRAVITY_MSS * airspeed; + // 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 = navPidApply2(&posControl.pids.fw_alt, demSEB, estSEB, US2S(deltaMicros), minDiveDeciDeg, maxClimbDeciDeg, 0) / pitchGainInv; + 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)); // Reconstrain pitch angle ( >0 - climb, <0 - dive) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index bce5111c03..9024ccf2b7 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -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);