Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-22 07:45:24 +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

10
src/main/blackbox/blackbox.c
View file

@ -217,6 +217,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"mcVelAxisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisFF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisFF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisFF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisOut",0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisOut",1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisOut",2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
@ -396,7 +399,7 @@ typedef struct blackboxMainState_s {
int32_t axisPID_Setpoint[XYZ_AXIS_COUNT];
int32_t mcPosAxisP[XYZ_AXIS_COUNT];
int32_t mcVelAxisPID[3][XYZ_AXIS_COUNT];
int32_t mcVelAxisPID[4][XYZ_AXIS_COUNT];
int32_t mcVelAxisOutput[XYZ_AXIS_COUNT];
int32_t mcSurfacePID[3];
@ -706,7 +709,7 @@ static void writeIntraframe(void)
blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT);
for (int i = 0; i < 3; i++) {
for (int i = 0; i < 4; i++) {
blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT);
}
@ -906,7 +909,7 @@ static void writeInterframe(void)
arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
for (int i = 0; i < 4; i++) {
arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
}
@ -1338,6 +1341,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional);
blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral);
blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative);
blackboxCurrent->mcVelAxisPID[3][i] = lrintf(nav_pids->vel[i].feedForward);
blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
}
#endif

25
src/main/fc/fc_msp.c
View file

@ -627,6 +627,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
}
break;
case MSP2_PID:
for (int i = 0; i < PID_ITEM_COUNT; i++) {
sbufWriteU8(dst, pidBank()->pid[i].P);
sbufWriteU8(dst, pidBank()->pid[i].I);
sbufWriteU8(dst, pidBank()->pid[i].D);
sbufWriteU8(dst, pidBank()->pid[i].FF);
}
break;
case MSP_PIDNAMES:
for (const char *c = pidnames; *c; c++) {
sbufWriteU8(dst, *c);
@ -1571,6 +1580,22 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
return MSP_RESULT_ERROR;
break;
case MSP2_SET_PID:
if (dataSize >= PID_ITEM_COUNT * 4) {
for (int i = 0; i < PID_ITEM_COUNT; i++) {
pidBankMutable()->pid[i].P = sbufReadU8(src);
pidBankMutable()->pid[i].I = sbufReadU8(src);
pidBankMutable()->pid[i].D = sbufReadU8(src);
pidBankMutable()->pid[i].FF = sbufReadU8(src);
}
schedulePidGainsUpdate();
#if defined(USE_NAV)
navigationUsePIDs();
#endif
} else
return MSP_RESULT_ERROR;
break;
case MSP_SET_MODE_RANGE:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize >= 5) && (tmp_u8 < MAX_MODE_ACTIVATION_CONDITION_COUNT)) {

5
src/main/fc/settings.yaml
View file

@ -1073,6 +1073,11 @@ groups:
condition: USE_NAV
min: 0
max: 255
- name: nav_mc_vel_xy_ff
field: bank_mc.pid[PID_VEL_XY].FF
condition: USE_NAV
min: 0
max: 255
- name: nav_mc_heading_p
field: bank_mc.pid[PID_HEADING].P
condition: USE_NAV

24
src/main/flight/pid.c
View file

@ -114,58 +114,66 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.bank_mc = {
.pid = {
[PID_ROLL] = { 40, 30, 23 },
[PID_PITCH] = { 40, 30, 23 },
[PID_YAW] = { 85, 45, 0 },
[PID_ROLL] = { 40, 30, 23, 0 },
[PID_PITCH] = { 40, 30, 23, 0 },
[PID_YAW] = { 85, 45, 0, 0 },
[PID_LEVEL] = {
.P = 20, // Self-level strength
.I = 15, // Self-leveing low-pass frequency (0 - disabled)
.D = 75, // 75% horizon strength
.FF = 0,
},
[PID_HEADING] = { 60, 0, 0 },
[PID_HEADING] = { 60, 0, 0, 0 },
[PID_POS_XY] = {
.P = 65, // NAV_POS_XY_P * 100
.I = 120, // posDecelerationTime * 100
.D = 10, // posResponseExpo * 100
.FF = 0,
},
[PID_VEL_XY] = {
.P = 40, // NAV_VEL_XY_P * 20
.I = 15, // NAV_VEL_XY_I * 100
.D = 100, // NAV_VEL_XY_D * 100
.FF = 40, // NAV_VEL_XY_D * 100
},
[PID_POS_Z] = {
.P = 50, // NAV_POS_Z_P * 100
.I = 0, // not used
.D = 0, // not used
.FF = 0,
},
[PID_VEL_Z] = {
.P = 100, // NAV_VEL_Z_P * 66.7
.I = 50, // NAV_VEL_Z_I * 20
.D = 10, // NAV_VEL_Z_D * 100
.FF = 0,
}
}
},
.bank_fw = {
.pid = {
[PID_ROLL] = { 5, 7, 50 },
[PID_PITCH] = { 5, 7, 50 },
[PID_YAW] = { 6, 10, 60 },
[PID_ROLL] = { 5, 7, 50, 0 },
[PID_PITCH] = { 5, 7, 50, 0 },
[PID_YAW] = { 6, 10, 60, 0 },
[PID_LEVEL] = {
.P = 20, // Self-level strength
.I = 5, // Self-leveing low-pass frequency (0 - disabled)
.D = 75, // 75% horizon strength
.FF = 0,
},
[PID_HEADING] = { 60, 0, 0 },
[PID_HEADING] = { 60, 0, 0, 0 },
[PID_POS_Z] = {
.P = 40, // FW_POS_Z_P * 10
.I = 5, // FW_POS_Z_I * 10
.D = 10, // FW_POS_Z_D * 10
.FF = 0,
},
[PID_POS_XY] = {
.P = 75, // FW_POS_XY_P * 100
.I = 5, // FW_POS_XY_I * 100
.D = 8, // FW_POS_XY_D * 100
.FF = 0,
}
}
},

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

@ -69,6 +69,7 @@ typedef struct pid8_s {
uint8_t P;
uint8_t I;
uint8_t D;
uint8_t FF;
} pid8_t;
typedef struct pidBank_s {

3
src/main/msp/msp_protocol_v2_inav.h
View file

@ -57,3 +57,6 @@
#define MSP2_INAV_SET_SERVO_MIXER 0x2021
#define MSP2_INAV_LOGIC_CONDITIONS 0x2022
#define MSP2_INAV_SET_LOGIC_CONDITIONS 0x2023
#define MSP2_PID 0x2030
#define MSP2_SET_PID 0x2031

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)

2
src/main/navigation/navigation.h
View file

@ -240,6 +240,7 @@ typedef struct {
float kI;
float kD;
float kT; // Tracking gain (anti-windup)
float kFF; // FeedForward Component
} pidControllerParam_t;
typedef struct {
@ -256,6 +257,7 @@ typedef struct {
float integral; // used integral value in output
float proportional; // used proportional value in output
float derivative; // used derivative value in output
float feedForward; // used FeedForward value in output
float output_constrained; // controller output constrained
} pidController_t;

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

@ -353,7 +353,7 @@ const navEstimatedPosVel_t * navGetCurrentActualPositionAndVelocity(void);
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 navPidInit(pidController_t *pid, float _kP, float _kI, float _kD, float _kFF);
void navPInit(pController_t *p, float _kP);
bool isThrustFacingDownwards(void);