diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index f60f48a64d8..57e98ba7e2c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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 diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index c582688e5e6..d752795c2fc 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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)) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ad6b9f54a15..bc44d68a576 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -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 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 06d0fa8747f..46614a40aaa 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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, } } }, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index a0943320890..cf5c05c2838 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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 { diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 83b9bf30265..4973f691cff 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -56,4 +56,7 @@ #define MSP2_INAV_SERVO_MIXER 0x2020 #define MSP2_INAV_SET_SERVO_MIXER 0x2021 #define MSP2_INAV_LOGIC_CONDITIONS 0x2022 -#define MSP2_INAV_SET_LOGIC_CONDITIONS 0x2023 \ No newline at end of file +#define MSP2_INAV_SET_LOGIC_CONDITIONS 0x2023 + +#define MSP2_PID 0x2030 +#define MSP2_SET_PID 0x2031 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5575dc8348a..fe6c4566f9a 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -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) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 6a7c9d19e99..421f6af6e74 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -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; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 29dc704ca3b..519607855ad 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -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);