Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-19 22:35:19 +03:00
Code Issues Wiki Activity

Convert XY VEL TO ACC controller to use feed forward component (#4326)

Browse source 
* modify navPidApply3 to use linear FF component

* store FF component in BB log

* Make VEL_XY FF component configurable

* change scaling for PID_VEL_XY FF component

* MSP2_PID frame to setup FF component

* Set default VEL XY FF to 40
This commit is contained in:
Paweł Spychalski 2019-03-09 09:22:50 +01:00 • committed by GitHub
parent 359385d28c
commit c64a63b504
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
9 changed files with 82 additions and 20 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1632,7 +1632,7 @@ static void navProcessFSMEvents(navigationFSMEvent_t injectedEvent)
// http://www.cds.caltech.edu/~murray/courses/cds101/fa02/caltech/astrom-ch6.pdf
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 newProportional, newDerivative, newFeedForward;
float error = setpoint - measurement;
/* P-term */
@ -1660,13 +1660,19 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
pid->integrator = 0.0f;
}
/*
* Compute FeedForward parameter
*/
newFeedForward = setpoint * pid->param.kFF * gainScaler;
/* Pre-calculate output and limit it if actuator is saturating */
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative + newFeedForward;
const float outValConstrained = constrainf(outVal, outMin, outMax);
pid->proportional = newProportional;
pid->integral = pid->integrator;
pid->derivative = newDerivative;
pid->feedForward = newFeedForward;
pid->output_constrained = outValConstrained;
/* Update I-term */
@ -1701,16 +1707,18 @@ void navPidReset(pidController_t *pid)
pid->derivative = 0.0f;
pid->integrator = 0.0f;
pid->last_input = 0.0f;
pid->feedForward = 0.0f;
pid->dterm_filter_state.state = 0.0f;
pid->dterm_filter_state.RC = 0.0f;
pid->output_constrained = 0.0f;
}
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF)
{
pid->param.kP = _kP;
pid->param.kI = _kI;
pid->param.kD = _kD;
pid->param.kFF = _kFF;
if (_kI > 1e-6f && _kP > 1e-6f) {
float Ti = _kP / _kI;
@ -3028,7 +3036,9 @@ void navigationUsePIDs(void)
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 20.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f);
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].FF / 100.0f
);
}
// Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
@ -3036,10 +3046,12 @@ void navigationUsePIDs(void)
navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->bank_mc.pid[PID_VEL_Z].P / 66.7f,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].I / 20.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f);
(float)pidProfile()->bank_mc.pid[PID_VEL_Z].D / 100.0f,
0.0f);
// Initialize surface tracking PID
navPidInit(&posControl.pids.surface, 2.0f,
0.0f,
0.0f,
0.0f);
@ -3047,11 +3059,13 @@ void navigationUsePIDs(void)
// Initialize fixed wing PID controllers
navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->bank_fw.pid[PID_POS_XY].P / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].I / 100.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f);
(float)pidProfile()->bank_fw.pid[PID_POS_XY].D / 100.0f,
0.0f);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->bank_fw.pid[PID_POS_Z].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f);
(float)pidProfile()->bank_fw.pid[PID_POS_Z].D / 10.0f,
0.0f);
}
void navigationInit(void)