Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 16:55:29 +03:00
Code Issues Wiki Activity

Merge pull request #995 from iNavFlight/fw-pid-change

Browse source 
Fixed-wing PIFF controller, different PID banks for FW/MC
This commit is contained in:
Konstantin Sharlaimov 2017-01-24 21:21:03 +10:00 • committed by GitHub
commit 2725e6a9fc
18 changed files with 444 additions and 331 deletions
Download patch file Download diff file Expand all files Collapse all files

4
.travis.yml
View file

@ -31,7 +31,7 @@ compiler: clang
before_install: ./install-toolchain.sh
install:
- export PATH=$PATH:$PWD/gcc-arm-none-eabi-5_4-2016q2/bin
- export PATH=$PATH:$PWD/gcc-arm-none-eabi-6_2-2016q4/bin
before_script: arm-none-eabi-gcc --version
script: ./.travis.sh
@ -39,7 +39,7 @@ script: ./.travis.sh
cache:
apt: true
directories:
- $PWD/gcc-arm-none-eabi-5_4-2016q2
- $PWD/gcc-arm-none-eabi-6_2-2016q4
notifications:
#slack: inavflight:UWRoWFJ4cbbpHXT8HJJlAPXa

36
docs/Cli.md
View file

@ -287,18 +287,30 @@ Re-apply any new defaults as desired.
| default_rate_profile | 0 | Default = profile number |
| mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you aquirre valid GPS fix. |
| mag_hold_rate_limit | 90 | This setting limits yaw rotation rate that MAG_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when MAG_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. |
| p_pitch | 40 | |
| i_pitch | 30 | |
| d_pitch | 23 | |
| p_roll | 40 | |
| i_roll | 30 | |
| d_roll | 23 | |
| p_yaw | 85 | |
| i_yaw | 45 | |
| d_yaw | 0 | |
| p_level | 20 | |
| i_level | 15 | |
| d_level | 75 | |
| `mc_p_pitch` | 40 | Multicopter rate stabilisation P-gain for PITCH |
| `mc_i_pitch` | 30 | Multicopter rate stabilisation I-gain for PITCH |
| `mc_d_pitch` | 23 | Multicopter rate stabilisation D-gain for PITCH |
| `mc_p_roll` | 40 | Multicopter rate stabilisation P-gain for ROLL |
| `mc_i_roll` | 30 | Multicopter rate stabilisation I-gain for ROLL |
| `mc_d_roll` | 23 | Multicopter rate stabilisation D-gain for ROLL |
| `mc_p_yaw` | 85 | Multicopter rate stabilisation P-gain for YAW |
| `mc_i_yaw` | 45 | Multicopter rate stabilisation I-gain for YAW |
| `mc_d_yaw` | 0 | Multicopter rate stabilisation D-gain for YAW |
| `mc_p_level` | 20 | Multicopter attitude stabilisation P-gain |
| `mc_i_level` | 15 | Multicopter attitude stabilisation low-pass filter cutoff |
| `mc_d_level` | 75 | Multicopter attitude stabilisation HORIZON transition point |
| `fw_p_pitch` | 20 | Fixed-wing rate stabilisation P-gain for PITCH |
| `fw_i_pitch` | 30 | Fixed-wing rate stabilisation I-gain for PITCH |
| `fw_ff_pitch`| 10 | Fixed-wing rate stabilisation FF-gain for PITCH |
| `fw_p_roll` | 25 | Fixed-wing rate stabilisation P-gain for ROLL |
| `fw_i_roll` | 30 | Fixed-wing rate stabilisation I-gain for ROLL |
| `fw_ff_roll` | 10 | Fixed-wing rate stabilisation FF-gain for ROLL |
| `fw_p_yaw` | 50 | Fixed-wing rate stabilisation P-gain for YAW |
| `fw_i_yaw` | 45 | Fixed-wing rate stabilisation I-gain for YAW |
| `fw_ff_yaw` | 0 | Fixed-wing rate stabilisation FF-gain for YAW |
| `fw_p_level` | 20 | Fixed-wing attitude stabilisation P-gain |
| `fw_i_level` | 15 | Fixed-wing attitude stabilisation low-pass filter cutoff |
| `fw_d_level` | 75 | Fixed-wing attitude stabilisation HORIZON transition point |
| max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° |
| max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° |
| gyro_soft_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. Default 60Hz |

4
install-toolchain.sh
View file

@ -1,5 +1,5 @@
#!/bin/bash
if [ ! -d $PWD/gcc-arm-none-eabi-5_4-2016q2/bin ] ; then
curl --retry 10 --retry-max-time 120 -L "https://launchpad.net/gcc-arm-embedded/5.0/5-2016-q2-update/+download/gcc-arm-none-eabi-5_4-2016q2-20160622-linux.tar.bz2" | tar xfj -
if [ ! -d $PWD/gcc-arm-none-eabi-6_2-2016q4/bin ] ; then
curl --retry 10 --retry-max-time 120 -L "https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/6-2016q4/gcc-arm-none-eabi-6_2-2016q4-20161216-linux.tar.bz2" | tar xfj -
fi

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

@ -486,7 +486,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1:
case FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_2:
return pidProfile()->D8[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0] != 0;
return pidBank()->pid[condition - FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0].D != 0;
case FLIGHT_LOG_FIELD_CONDITION_MAG:
#ifdef MAG
@ -1375,34 +1375,31 @@ static bool blackboxWriteSysinfo()
BLACKBOX_PRINT_HEADER_LINE("rates:%d,%d,%d", currentControlRateProfile->rates[ROLL],
currentControlRateProfile->rates[PITCH],
currentControlRateProfile->rates[YAW]);
BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", pidProfile()->P8[PIDROLL],
pidProfile()->I8[PIDROLL],
pidProfile()->D8[PIDROLL]);
BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", pidProfile()->P8[PIDPITCH],
pidProfile()->I8[PIDPITCH],
pidProfile()->D8[PIDPITCH]);
BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", pidProfile()->P8[PIDYAW],
pidProfile()->I8[PIDYAW],
pidProfile()->D8[PIDYAW]);
BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", pidProfile()->P8[PIDALT],
pidProfile()->I8[PIDALT],
pidProfile()->D8[PIDALT]);
BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", pidProfile()->P8[PIDPOS],
pidProfile()->I8[PIDPOS],
pidProfile()->D8[PIDPOS]);
BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", pidProfile()->P8[PIDPOSR],
pidProfile()->I8[PIDPOSR],
pidProfile()->D8[PIDPOSR]);
BLACKBOX_PRINT_HEADER_LINE("navrPID:%d,%d,%d", pidProfile()->P8[PIDNAVR],
pidProfile()->I8[PIDNAVR],
pidProfile()->D8[PIDNAVR]);
BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", pidProfile()->P8[PIDLEVEL],
pidProfile()->I8[PIDLEVEL],
pidProfile()->D8[PIDLEVEL]);
BLACKBOX_PRINT_HEADER_LINE("magPID:%d", pidProfile()->P8[PIDMAG]);
BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", pidProfile()->P8[PIDVEL],
pidProfile()->I8[PIDVEL],
pidProfile()->D8[PIDVEL]);
BLACKBOX_PRINT_HEADER_LINE("rollPID:%d,%d,%d", pidBank()->pid[PID_ROLL].P,
pidBank()->pid[PID_ROLL].I,
pidBank()->pid[PID_ROLL].D);
BLACKBOX_PRINT_HEADER_LINE("pitchPID:%d,%d,%d", pidBank()->pid[PID_PITCH].P,
pidBank()->pid[PID_PITCH].I,
pidBank()->pid[PID_PITCH].D);
BLACKBOX_PRINT_HEADER_LINE("yawPID:%d,%d,%d", pidBank()->pid[PID_YAW].P,
pidBank()->pid[PID_YAW].I,
pidBank()->pid[PID_YAW].D);
BLACKBOX_PRINT_HEADER_LINE("altPID:%d,%d,%d", pidBank()->pid[PID_POS_Z].P,
pidBank()->pid[PID_POS_Z].I,
pidBank()->pid[PID_POS_Z].D);
BLACKBOX_PRINT_HEADER_LINE("posPID:%d,%d,%d", pidBank()->pid[PID_POS_XY].P,
pidBank()->pid[PID_POS_XY].I,
pidBank()->pid[PID_POS_XY].D);
BLACKBOX_PRINT_HEADER_LINE("posrPID:%d,%d,%d", pidBank()->pid[PID_VEL_XY].P,
pidBank()->pid[PID_VEL_XY].I,
pidBank()->pid[PID_VEL_XY].D);
BLACKBOX_PRINT_HEADER_LINE("levelPID:%d,%d,%d", pidBank()->pid[PID_LEVEL].P,
pidBank()->pid[PID_LEVEL].I,
pidBank()->pid[PID_LEVEL].D);
BLACKBOX_PRINT_HEADER_LINE("magPID:%d", pidBank()->pid[PID_HEADING].P);
BLACKBOX_PRINT_HEADER_LINE("velPID:%d,%d,%d", pidBank()->pid[PID_VEL_Z].P,
pidBank()->pid[PID_VEL_Z].I,
pidBank()->pid[PID_VEL_Z].D);
BLACKBOX_PRINT_HEADER_LINE("yaw_p_limit:%d", pidProfile()->yaw_p_limit);
BLACKBOX_PRINT_HEADER_LINE("yaw_lpf_hz:%d", (int)(pidProfile()->yaw_lpf_hz * 100.0f));
BLACKBOX_PRINT_HEADER_LINE("dterm_lpf_hz:%d", (int)(pidProfile()->dterm_lpf_hz * 100.0f));

96
src/main/cms/cms_menu_imu.c
View file

@ -53,16 +53,16 @@ static char profileIndexString[] = " p";
static void cmsx_ReadPidToArray(uint8_t *dst, int pidIndex)
{
dst[0] = pidProfile()->P8[pidIndex];
dst[1] = pidProfile()->I8[pidIndex];
dst[2] = pidProfile()->D8[pidIndex];
dst[0] = pidBank()->pid[pidIndex].P;
dst[1] = pidBank()->pid[pidIndex].I;
dst[2] = pidBank()->pid[pidIndex].D;
}
static void cmsx_WritebackPidFromArray(uint8_t *src, int pidIndex)
{
pidProfileMutable()->P8[pidIndex] = src[0];
pidProfileMutable()->I8[pidIndex] = src[1];
pidProfileMutable()->D8[pidIndex] = src[2];
pidBankMutable()->pid[pidIndex].P = src[0];
pidBankMutable()->pid[pidIndex].I = src[1];
pidBankMutable()->pid[pidIndex].D = src[2];
}
static long cmsx_menuImu_onEnter(void)
@ -100,9 +100,9 @@ static uint8_t cmsx_pidYaw[3];
static long cmsx_PidRead(void)
{
cmsx_ReadPidToArray(cmsx_pidRoll, PIDROLL);
cmsx_ReadPidToArray(cmsx_pidPitch, PIDPITCH);
cmsx_ReadPidToArray(cmsx_pidYaw, PIDYAW);
cmsx_ReadPidToArray(cmsx_pidRoll, PID_ROLL);
cmsx_ReadPidToArray(cmsx_pidPitch, PID_PITCH);
cmsx_ReadPidToArray(cmsx_pidYaw, PID_YAW);
return 0;
}
@ -119,9 +119,9 @@ static long cmsx_PidWriteback(const OSD_Entry *self)
{
UNUSED(self);
cmsx_WritebackPidFromArray(cmsx_pidRoll, PIDROLL);
cmsx_WritebackPidFromArray(cmsx_pidPitch, PIDPITCH);
cmsx_WritebackPidFromArray(cmsx_pidYaw, PIDYAW);
cmsx_WritebackPidFromArray(cmsx_pidRoll, PID_ROLL);
cmsx_WritebackPidFromArray(cmsx_pidPitch, PID_PITCH);
cmsx_WritebackPidFromArray(cmsx_pidYaw, PID_YAW);
schedulePidGainsUpdate();
@ -157,15 +157,15 @@ static CMS_Menu cmsx_menuPid = {
.entries = cmsx_menuPidEntries
};
static uint8_t cmsx_pidAlt[3];
static uint8_t cmsx_pidVel[3];
static uint8_t cmsx_pidMag[3];
static uint8_t cmsx_pidPosZ[3];
static uint8_t cmsx_pidVelZ[3];
static uint8_t cmsx_pidHead[3];
static long cmsx_menuPidAltMag_onEnter(void)
{
cmsx_ReadPidToArray(cmsx_pidAlt, PIDALT);
cmsx_ReadPidToArray(cmsx_pidVel, PIDVEL);
cmsx_pidMag[0] = pidProfile()->P8[PIDMAG];
cmsx_ReadPidToArray(cmsx_pidPosZ, PID_POS_Z);
cmsx_ReadPidToArray(cmsx_pidVelZ, PID_VEL_Z);
cmsx_pidHead[0] = pidBank()->pid[PID_HEADING].P;
return 0;
}
@ -174,9 +174,9 @@ static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self)
{
UNUSED(self);
cmsx_WritebackPidFromArray(cmsx_pidAlt, PIDALT);
cmsx_WritebackPidFromArray(cmsx_pidVel, PIDVEL);
pidProfileMutable()->P8[PIDMAG] = cmsx_pidMag[0];
cmsx_WritebackPidFromArray(cmsx_pidPosZ, PID_POS_Z);
cmsx_WritebackPidFromArray(cmsx_pidVelZ, PID_VEL_Z);
pidBankMutable()->pid[PID_HEADING].P = cmsx_pidHead[0];
navigationUsePIDs();
@ -186,13 +186,13 @@ static long cmsx_menuPidAltMag_onExit(const OSD_Entry *self)
static OSD_Entry cmsx_menuPidAltMagEntries[] = {
{ "-- ALT&MAG --", OME_Label, NULL, profileIndexString, 0},
{ "ALT P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidAlt[0], 0, 255, 1 }, 0 },
{ "ALT I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidAlt[1], 0, 255, 1 }, 0 },
{ "ALT D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidAlt[2], 0, 255, 1 }, 0 },
{ "VEL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVel[0], 0, 255, 1 }, 0 },
{ "VEL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVel[1], 0, 255, 1 }, 0 },
{ "VEL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVel[2], 0, 255, 1 }, 0 },
{ "MAG P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidMag[0], 0, 255, 1 }, 0 },
{ "ALT P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosZ[0], 0, 255, 1 }, 0 },
{ "ALT I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosZ[1], 0, 255, 1 }, 0 },
{ "ALT D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosZ[2], 0, 255, 1 }, 0 },
{ "VEL P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVelZ[0], 0, 255, 1 }, 0 },
{ "VEL I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVelZ[1], 0, 255, 1 }, 0 },
{ "VEL D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVelZ[2], 0, 255, 1 }, 0 },
{ "MAG P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidHead[0], 0, 255, 1 }, 0 },
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
@ -207,15 +207,13 @@ static CMS_Menu cmsx_menuPidAltMag = {
.entries = cmsx_menuPidAltMagEntries,
};
static uint8_t cmsx_pidPos[3];
static uint8_t cmsx_pidPosR[3];
static uint8_t cmsx_pidNavR[3];
static uint8_t cmsx_pidPosXY[3];
static uint8_t cmsx_pidVelXY[3];
static long cmsx_menuPidGpsnav_onEnter(void)
{
cmsx_ReadPidToArray(cmsx_pidPos, PIDPOS);
cmsx_ReadPidToArray(cmsx_pidPosR, PIDPOSR);
cmsx_ReadPidToArray(cmsx_pidNavR, PIDNAVR);
cmsx_ReadPidToArray(cmsx_pidPosXY, PID_POS_XY);
cmsx_ReadPidToArray(cmsx_pidVelXY, PID_VEL_XY);
return 0;
}
@ -224,9 +222,8 @@ static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self)
{
UNUSED(self);
cmsx_WritebackPidFromArray(cmsx_pidPos, PIDPOS);
cmsx_WritebackPidFromArray(cmsx_pidPosR, PIDPOSR);
cmsx_WritebackPidFromArray(cmsx_pidNavR, PIDNAVR);
cmsx_WritebackPidFromArray(cmsx_pidPosXY, PID_POS_XY);
cmsx_WritebackPidFromArray(cmsx_pidVelXY, PID_VEL_XY);
navigationUsePIDs();
@ -236,14 +233,11 @@ static long cmsx_menuPidGpsnav_onExit(const OSD_Entry *self)
static OSD_Entry cmsx_menuPidGpsnavEntries[] = {
{ "-- GPSNAV --", OME_Label, NULL, profileIndexString, 0},
{ "POS P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPos[0], 0, 255, 1 }, 0 },
{ "POS I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPos[1], 0, 255, 1 }, 0 },
{ "POSR P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosR[0], 0, 255, 1 }, 0 },
{ "POSR I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosR[1], 0, 255, 1 }, 0 },
{ "POSR D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosR[2], 0, 255, 1 }, 0 },
{ "NAVR P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidNavR[0], 0, 255, 1 }, 0 },
{ "NAVR I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidNavR[1], 0, 255, 1 }, 0 },
{ "NAVR D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidNavR[2], 0, 255, 1 }, 0 },
{ "POS P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosXY[0], 0, 255, 1 }, 0 },
{ "POS I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidPosXY[1], 0, 255, 1 }, 0 },
{ "POSR P", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVelXY[0], 0, 255, 1 }, 0 },
{ "POSR I", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVelXY[1], 0, 255, 1 }, 0 },
{ "POSR D", OME_UINT8, NULL, &(OSD_UINT8_t){ &cmsx_pidVelXY[2], 0, 255, 1 }, 0 },
{"BACK", OME_Back, NULL, NULL, 0},
{NULL, OME_END, NULL, NULL, 0}
@ -346,9 +340,9 @@ static long cmsx_profileOtherOnEnter(void)
cmsx_dtermSetpointWeight = pidProfile()->dtermSetpointWeight;
cmsx_setpointRelaxRatio = pidProfile()->setpointRelaxRatio;
cmsx_angleStrength = pidProfile()->P8[PIDLEVEL];
cmsx_horizonStrength = pidProfile()->I8[PIDLEVEL];
cmsx_horizonTransition = pidProfile()->D8[PIDLEVEL];
cmsx_angleStrength = pidProfile()[PIDLEVEL].P;
cmsx_horizonStrength = pidProfile()[PIDLEVEL].I;
cmsx_horizonTransition = pidProfile()[PIDLEVEL].D;
return 0;
}
@ -360,9 +354,9 @@ static long cmsx_profileOtherOnExit(const OSD_Entry *self)
pidProfileMutable()->dtermSetpointWeight = cmsx_dtermSetpointWeight;
pidProfileMutable()->setpointRelaxRatio = cmsx_setpointRelaxRatio;
pidProfileMutable()->P8[PIDLEVEL] = cmsx_angleStrength;
pidProfileMutable()->I8[PIDLEVEL] = cmsx_horizonStrength;
pidProfileMutable()->D8[PIDLEVEL] = cmsx_horizonTransition;
pidProfileMutable()[PIDLEVEL].P = cmsx_angleStrength;
pidProfileMutable()[PIDLEVEL].I = cmsx_horizonStrength;
pidProfileMutable()[PIDLEVEL].D = cmsx_horizonTransition;
return 0;
}

2
src/main/fc/fc_init.c
View file

@ -497,7 +497,7 @@ void init(void)
flashLedsAndBeep();
#ifdef USE_DTERM_NOTCH
pidInitFilters(pidProfile());
pidInitFilters();
#endif
imuInit();

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

@ -695,9 +695,9 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn
case MSP_PID:
for (int i = 0; i < PID_ITEM_COUNT; i++) {
sbufWriteU8(dst, pidProfile()->P8[i]);
sbufWriteU8(dst, pidProfile()->I8[i]);
sbufWriteU8(dst, pidProfile()->D8[i]);
sbufWriteU8(dst, pidBank()->pid[i].P);
sbufWriteU8(dst, pidBank()->pid[i].I);
sbufWriteU8(dst, pidBank()->pid[i].D);
}
break;
@ -1342,9 +1342,9 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
case MSP_SET_PID:
for (int i = 0; i < PID_ITEM_COUNT; i++) {
pidProfileMutable()->P8[i] = sbufReadU8(src);
pidProfileMutable()->I8[i] = sbufReadU8(src);
pidProfileMutable()->D8[i] = sbufReadU8(src);
pidBankMutable()->pid[i].P = sbufReadU8(src);
pidBankMutable()->pid[i].I = sbufReadU8(src);
pidBankMutable()->pid[i].D = sbufReadU8(src);
}
schedulePidGainsUpdate();
#if defined(NAV)
@ -1558,7 +1558,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src)
#ifdef USE_DTERM_NOTCH
pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
pidInitFilters(pidProfile());
pidInitFilters();
#endif
#ifdef USE_GYRO_NOTCH_2
gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);

4
src/main/fc/mw.c
View file

@ -669,10 +669,10 @@ void taskMainPidLoop(timeUs_t currentTimeUs)
}
// Update PID coefficients
updatePIDCoefficients(pidProfile(), currentControlRateProfile, motorConfig());
updatePIDCoefficients();
// Calculate stabilisation
pidController(pidProfile(), currentControlRateProfile, rxConfig());
pidController();
#ifdef HIL
if (hilActive) {

36
src/main/fc/rc_adjustments.c
View file

@ -238,8 +238,8 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
break;
case ADJUSTMENT_PITCH_ROLL_P:
case ADJUSTMENT_PITCH_P:
newValue = constrain((int)pidProfile()->P8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->P8[PIDPITCH] = newValue;
newValue = constrain((int)pidBank()->pid[PID_PITCH].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_PITCH].P = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_P, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_P) {
schedulePidGainsUpdate();
@ -247,15 +247,15 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_P
case ADJUSTMENT_ROLL_P:
newValue = constrain((int)pidProfile()->P8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->P8[PIDROLL] = newValue;
newValue = constrain((int)pidBank()->pid[PID_ROLL].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_ROLL].P = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_P, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_I:
case ADJUSTMENT_PITCH_I:
newValue = constrain((int)pidProfile()->I8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->I8[PIDPITCH] = newValue;
newValue = constrain((int)pidBank()->pid[PID_PITCH].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_PITCH].I = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_I, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_I) {
schedulePidGainsUpdate();
@ -263,15 +263,15 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_I
case ADJUSTMENT_ROLL_I:
newValue = constrain((int)pidProfile()->I8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->I8[PIDROLL] = newValue;
newValue = constrain((int)pidBank()->pid[PID_ROLL].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_ROLL].I = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_I, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_PITCH_ROLL_D:
case ADJUSTMENT_PITCH_D:
newValue = constrain((int)pidProfile()->D8[PIDPITCH] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->D8[PIDPITCH] = newValue;
newValue = constrain((int)pidBank()->pid[PID_PITCH].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_PITCH].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_PITCH_D, newValue);
if (adjustmentFunction == ADJUSTMENT_PITCH_D) {
schedulePidGainsUpdate();
@ -279,26 +279,26 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t
}
// follow though for combined ADJUSTMENT_PITCH_ROLL_D
case ADJUSTMENT_ROLL_D:
newValue = constrain((int)pidProfile()->D8[PIDROLL] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->D8[PIDROLL] = newValue;
newValue = constrain((int)pidBank()->pid[PID_ROLL].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_ROLL].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_ROLL_D, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_P:
newValue = constrain((int)pidProfile()->P8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->P8[PIDYAW] = newValue;
newValue = constrain((int)pidBank()->pid[PID_YAW].P + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_YAW].P = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_P, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_I:
newValue = constrain((int)pidProfile()->I8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->I8[PIDYAW] = newValue;
newValue = constrain((int)pidBank()->pid[PID_YAW].I + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_YAW].I = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_I, newValue);
schedulePidGainsUpdate();
break;
case ADJUSTMENT_YAW_D:
newValue = constrain((int)pidProfile()->D8[PIDYAW] + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidProfileMutable()->D8[PIDYAW] = newValue;
newValue = constrain((int)pidBank()->pid[PID_YAW].D + delta, 0, 200); // FIXME magic numbers repeated in serial_cli.c
pidBankMutable()->pid[PID_YAW].D = newValue;
blackboxLogInflightAdjustmentEvent(ADJUSTMENT_YAW_D, newValue);
schedulePidGainsUpdate();
break;

73
src/main/fc/serial_cli.c
View file

@ -681,19 +681,33 @@ static const clivalue_t valueTable[] = {
// PG_PID_PROFILE
// { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 }, PG_PID_CONFIG, offsetof(pidProfile_t, defaultRateProfileIndex) },
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PITCH]) },
{ "i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PITCH]) },
{ "d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PITCH]) },
{ "p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[ROLL]) },
{ "i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[ROLL]) },
{ "d_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[ROLL]) },
{ "p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[YAW]) },
{ "i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[YAW]) },
{ "d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[YAW]) },
{ "mc_p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_PITCH].P) },
{ "mc_i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_PITCH].I) },
{ "mc_d_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_PITCH].D) },
{ "mc_p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_ROLL].P) },
{ "mc_i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_ROLL].I) },
{ "mc_d_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_ROLL].D) },
{ "mc_p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_YAW].P) },
{ "mc_i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_YAW].I) },
{ "mc_d_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_YAW].D) },
{ "p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDLEVEL]) },
{ "i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDLEVEL]) },
{ "d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDLEVEL]) },
{ "mc_p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_LEVEL].P) },
{ "mc_i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_LEVEL].I) },
{ "mc_d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_LEVEL].D) },
{ "fw_p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_PITCH].P) },
{ "fw_i_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_PITCH].I) },
{ "fw_ff_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_PITCH].D) },
{ "fw_p_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_ROLL].P) },
{ "fw_i_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_ROLL].I) },
{ "fw_ff_roll", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_ROLL].D) },
{ "fw_p_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_YAW].P) },
{ "fw_i_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_YAW].I) },
{ "fw_ff_yaw", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_YAW].D) },
{ "fw_p_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_LEVEL].P) },
{ "fw_i_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_LEVEL].I) },
{ "fw_d_level", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_LEVEL].D) },
{ "max_angle_inclination_rll", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 100, 900 }, PG_PID_PROFILE, offsetof(pidProfile_t, max_angle_inclination[FD_ROLL]) },
{ "max_angle_inclination_pit", VAR_INT16 | PROFILE_VALUE, .config.minmax = { 100, 900 }, PG_PID_PROFILE, offsetof(pidProfile_t, max_angle_inclination[FD_PITCH]) },
@ -719,23 +733,28 @@ static const clivalue_t valueTable[] = {
{ "rate_accel_limit_roll_pitch",VAR_UINT32 | PROFILE_VALUE | MODE_MAX, .config.max = { 500000 }, PG_PID_PROFILE, offsetof(pidProfile_t, axisAccelerationLimitRollPitch) },
{ "rate_accel_limit_yaw", VAR_UINT32 | PROFILE_VALUE | MODE_MAX, .config.max = { 500000 }, PG_PID_PROFILE, offsetof(pidProfile_t, axisAccelerationLimitYaw) },
#ifdef NAV
{ "nav_alt_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDALT]) },
{ "nav_alt_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDALT]) },
{ "nav_alt_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDALT]) },
{ "nav_mc_pos_z_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_Z].P) },
{ "nav_mc_pos_z_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_Z].I) },
{ "nav_mc_pos_z_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_Z].D) },
{ "nav_vel_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDVEL]) },
{ "nav_vel_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDVEL]) },
{ "nav_vel_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDVEL]) },
{ "nav_mc_vel_z_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_VEL_Z].P) },
{ "nav_mc_vel_z_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_VEL_Z].I) },
{ "nav_mc_vel_z_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_VEL_Z].D) },
{ "nav_pos_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDPOS]) },
{ "nav_pos_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDPOS]) },
{ "nav_pos_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDPOS]) },
{ "nav_posr_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDPOSR]) },
{ "nav_posr_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDPOSR]) },
{ "nav_posr_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDPOSR]) },
{ "nav_navr_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, P8[PIDNAVR]) },
{ "nav_navr_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, I8[PIDNAVR]) },
{ "nav_navr_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, D8[PIDNAVR]) },
{ "nav_mc_pos_xy_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_XY].P) },
{ "nav_mc_pos_xy_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_XY].I) },
{ "nav_mc_pos_xy_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_POS_XY].D) },
{ "nav_mc_vel_xy_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_VEL_XY].P) },
{ "nav_mc_vel_xy_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_VEL_XY].I) },
{ "nav_mc_vel_xy_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_mc.pid[PID_VEL_XY].D) },
{ "nav_fw_pos_z_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_POS_Z].P) },
{ "nav_fw_pos_z_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_POS_Z].I) },
{ "nav_fw_pos_z_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_POS_Z].D) },
{ "nav_fw_pos_xy_p", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_POS_XY].P) },
{ "nav_fw_pos_xy_i", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_POS_XY].I) },
{ "nav_fw_pos_xy_d", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, bank_fw.pid[PID_POS_XY].D) },
// PG_POSITION_ESTIMATION_CONFIG
#if defined(NAV_AUTO_MAG_DECLINATION)

34
src/main/flight/navigation_rewrite.c
View file

@ -2557,41 +2557,43 @@ void updateWaypointsAndNavigationMode(void)
*-----------------------------------------------------------*/
void navigationUsePIDs(void)
{
/** Multicopter PIDs */
// Brake time parameter
posControl.posDecelerationTime = (float)pidProfile()->I8[PIDPOS] / 100.0f;
posControl.posDecelerationTime = (float)pidProfile()->bank_mc.pid[PID_POS_XY].I / 100.0f;
// Position controller expo (taret vel expo for MC)
posControl.posResponseExpo = constrainf((float)pidProfile()->D8[PIDPOS] / 100.0f, 0.0f, 1.0f);
posControl.posResponseExpo = constrainf((float)pidProfile()->bank_mc.pid[PID_POS_XY].D / 100.0f, 0.0f, 1.0f);
// Initialize position hold P-controller
for (int axis = 0; axis < 2; axis++) {
navPInit(&posControl.pids.pos[axis], (float)pidProfile()->P8[PIDPOS] / 100.0f);
navPInit(&posControl.pids.pos[axis], (float)pidProfile()->bank_mc.pid[PID_POS_XY].P / 100.0f);
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->P8[PIDPOSR] / 100.0f,
(float)pidProfile()->I8[PIDPOSR] / 100.0f,
(float)pidProfile()->D8[PIDPOSR] / 100.0f);
navPidInit(&posControl.pids.vel[axis], (float)pidProfile()->bank_mc.pid[PID_VEL_XY].P / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].I / 100.0f,
(float)pidProfile()->bank_mc.pid[PID_VEL_XY].D / 100.0f);
}
// Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z
navPInit(&posControl.pids.pos[Z], (float)pidProfile()->P8[PIDALT] / 100.0f);
navPInit(&posControl.pids.pos[Z], (float)pidProfile()->bank_mc.pid[PID_POS_Z].P / 100.0f);
navPidInit(&posControl.pids.vel[Z], (float)pidProfile()->P8[PIDVEL] / 66.7f,
(float)pidProfile()->I8[PIDVEL] / 20.0f,
(float)pidProfile()->D8[PIDVEL] / 100.0f);
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);
// Initialize surface tracking PID
navPidInit(&posControl.pids.surface, 2.0f,
1.0f,
0.0f);
/** Airplane PIDs */
// Initialize fixed wing PID controllers
navPidInit(&posControl.pids.fw_nav, (float)pidProfile()->P8[PIDNAVR] / 100.0f,
(float)pidProfile()->I8[PIDNAVR] / 100.0f,
(float)pidProfile()->D8[PIDNAVR] / 100.0f);
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);
navPidInit(&posControl.pids.fw_alt, (float)pidProfile()->P8[PIDALT] / 100.0f,
(float)pidProfile()->I8[PIDALT] / 100.0f,
(float)pidProfile()->D8[PIDALT] / 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);
}
void navigationInit(void)

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

@ -52,10 +52,11 @@
typedef struct {
float kP;
float kI;
float kD;
float kT;
float kP; // Proportional gain
float kI; // Integral gain
float kD; // Derivative gain
float kFF; // Feed-forward gain
float kT; // Back-calculation tracking gain
float gyroRate;
float rateTarget;
@ -99,7 +100,6 @@ static pt1Filter_t magHoldRateFilter;
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
static bool pidGainsUpdateRequired = false;
static float tpaFactor;
int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT];
#ifdef BLACKBOX
@ -111,34 +111,63 @@ static pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT];
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 0);
PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.P8[ROLL] = 40,
.I8[ROLL] = 30,
.D8[ROLL] = 23,
.P8[PITCH] = 40,
.I8[PITCH] = 30,
.D8[PITCH] = 23,
.P8[YAW] = 85,
.I8[YAW] = 45,
.D8[YAW] = 0, // not used
.P8[PIDALT] = 50, // NAV_POS_Z_P * 100
.I8[PIDALT] = 0, // not used
.D8[PIDALT] = 0, // not used
.P8[PIDPOS] = 65, // NAV_POS_XY_P * 100
.I8[PIDPOS] = 120, // posDecelerationTime * 100
.D8[PIDPOS] = 10, // posResponseExpo * 100
.P8[PIDPOSR] = 180, // NAV_VEL_XY_P * 100
.I8[PIDPOSR] = 15, // NAV_VEL_XY_I * 100
.D8[PIDPOSR] = 100, // NAV_VEL_XY_D * 100
.P8[PIDNAVR] = 10, // FW_NAV_P * 100
.I8[PIDNAVR] = 5, // FW_NAV_I * 100
.D8[PIDNAVR] = 8, // FW_NAV_D * 100
.P8[PIDLEVEL] = 20, // Self-level strength
.I8[PIDLEVEL] = 15, // Self-leveing low-pass frequency (0 - disabled)
.D8[PIDLEVEL] = 75, // 75% horizon strength
.P8[PIDMAG] = 60,
.P8[PIDVEL] = 100, // NAV_VEL_Z_P * 100
.I8[PIDVEL] = 50, // NAV_VEL_Z_I * 100
.D8[PIDVEL] = 10, // NAV_VEL_Z_D * 100
.bank_mc = {
.pid = {
[PID_ROLL] = { 40, 30, 23 },
[PID_PITCH] = { 40, 30, 23 },
[PID_YAW] = { 85, 45, 0 },
[PID_LEVEL] = {
.P = 20, // Self-level strength
.I = 15, // Self-leveing low-pass frequency (0 - disabled)
.D = 75, // 75% horizon strength
},
[PID_HEADING] = { 60, 0, 0 },
[PID_POS_XY] = {
.P = 65, // NAV_POS_XY_P * 100
.I = 120, // posDecelerationTime * 100
.D = 10, // posResponseExpo * 100
},
[PID_VEL_XY] = {
.P = 180, // NAV_VEL_XY_P * 100
.I = 15, // NAV_VEL_XY_I * 100
.D = 100, // NAV_VEL_XY_D * 100
},
[PID_POS_Z] = {
.P = 50, // NAV_POS_Z_P * 100
.I = 0, // not used
.D = 0, // not used
},
[PID_VEL_Z] = {
.P = 100, // NAV_VEL_Z_P * 100
.I = 50, // NAV_VEL_Z_I * 100
.D = 10, // NAV_VEL_Z_D * 100
}
}
},
.bank_fw = {
.pid = {
[PID_ROLL] = { 25, 35, 10 },
[PID_PITCH] = { 20, 35, 10 },
[PID_YAW] = { 50, 45, 0 },
[PID_LEVEL] = {
.P = 20, // Self-level strength
.I = 15, // Self-leveing low-pass frequency (0 - disabled)
.D = 75, // 75% horizon strength
},
[PID_HEADING] = { 60, 0, 0 },
[PID_POS_Z] = {
.P = 50, // FW_POS_Z_P * 100
.I = 0, // not used
.D = 0, // not used
},
[PID_POS_XY] = {
.P = 75, // FW_NAV_P * 100
.I = 5, // FW_NAV_I * 100
.D = 8, // FW_NAV_D * 100
}
}
},
.acc_soft_lpf_hz = 15,
.dterm_soft_notch_hz = 0,
@ -173,7 +202,7 @@ void pidInit(void)
}
#ifdef USE_DTERM_NOTCH
bool pidInitFilters(const pidProfile_t *pidProfile)
bool pidInitFilters(void)
{
uint32_t refreshRate;
#ifdef ASYNC_GYRO_PROCESSING
@ -182,10 +211,10 @@ bool pidInitFilters(const pidProfile_t *pidProfile)
refreshRate= gyro.targetLooptime;
#endif
notchFilterApplyFn = nullFilterApply;
if(refreshRate != 0 && pidProfile->dterm_soft_notch_hz != 0){
if(refreshRate != 0 && pidProfile()->dterm_soft_notch_hz != 0){
notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; ++ axis) {
biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile->dterm_soft_notch_hz, pidProfile->dterm_soft_notch_cutoff);
biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
}
return true;
}
@ -237,18 +266,63 @@ float pidRcCommandToRate(int16_t stick, uint8_t rate)
/*
FP-PID has been rescaled to match LuxFloat (and MWRewrite) from Cleanflight 1.13
*/
#define FP_PID_RATE_FF_MULTIPLIER 31.0f
#define FP_PID_RATE_P_MULTIPLIER 31.0f
#define FP_PID_RATE_I_MULTIPLIER 4.0f
#define FP_PID_RATE_D_MULTIPLIER 1905.0f
#define FP_PID_LEVEL_P_MULTIPLIER 65.6f
#define FP_PID_YAWHOLD_P_MULTIPLIER 80.0f
static float calculateFixedWingTPAFactor(void)
{
float tpaFactor;
// tpa_rate is amount of curve TPA applied to PIDs
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
if (currentControlRateProfile->dynThrPID != 0 && currentControlRateProfile->tpa_breakpoint > motorConfig()->minthrottle) {
if (rcCommand[THROTTLE] > motorConfig()->minthrottle) {
// Calculate TPA according to throttle
tpaFactor = 0.5f + ((float)(currentControlRateProfile->tpa_breakpoint - motorConfig()->minthrottle) / (rcCommand[THROTTLE] - motorConfig()->minthrottle) / 2.0f);
// Limit to [0.5; 2] range
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
}
else {
tpaFactor = 2.0f;
}
// Attenuate TPA curve according to configured amount
tpaFactor = 1.0f + (tpaFactor - 1.0f) * (currentControlRateProfile->dynThrPID / 100.0f);
}
else {
tpaFactor = 1.0f;
}
return tpaFactor;
}
static float calculateMultirotorTPAFactor(void)
{
float tpaFactor;
// TPA should be updated only when TPA is actually set
if (currentControlRateProfile->dynThrPID == 0 || rcCommand[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {
tpaFactor = 1.0f;
} else if (rcCommand[THROTTLE] < motorConfig()->maxthrottle) {
tpaFactor = (100 - (uint16_t)currentControlRateProfile->dynThrPID * (rcCommand[THROTTLE] - currentControlRateProfile->tpa_breakpoint) / (motorConfig()->maxthrottle - currentControlRateProfile->tpa_breakpoint)) / 100.0f;
} else {
tpaFactor = (100 - currentControlRateProfile->dynThrPID) / 100.0f;
}
return tpaFactor;
}
void schedulePidGainsUpdate(void)
{
pidGainsUpdateRequired = true;
}
void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const motorConfig_t *motorConfig)
void updatePIDCoefficients(void)
{
static uint16_t prevThrottle = 0;
@ -263,66 +337,32 @@ void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConf
return;
}
// Calculate TPA factor - different logic for airplanes and multirotors
if (STATE(FIXED_WING)) {
// tpa_rate is amount of curve TPA applied to PIDs
// tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned)
if (controlRateConfig->dynThrPID != 0 && controlRateConfig->tpa_breakpoint > motorConfig->minthrottle) {
if (rcCommand[THROTTLE] > motorConfig->minthrottle) {
// Calculate TPA according to throttle
tpaFactor = 0.5f + ((float)(controlRateConfig->tpa_breakpoint - motorConfig->minthrottle) / (rcCommand[THROTTLE] - motorConfig->minthrottle) / 2.0f);
// Limit to [0.5; 2] range
tpaFactor = constrainf(tpaFactor, 0.5f, 2.0f);
}
else {
tpaFactor = 2.0f;
}
// Attenuate TPA curve according to configured amount
tpaFactor = 1.0f + (tpaFactor - 1.0f) * (controlRateConfig->dynThrPID / 100.0f);
}
else {
tpaFactor = 1.0f;
}
}
else {
// TPA should be updated only when TPA is actually set
if (controlRateConfig->dynThrPID == 0 || rcCommand[THROTTLE] < controlRateConfig->tpa_breakpoint) {
tpaFactor = 1.0f;
} else if (rcCommand[THROTTLE] < motorConfig->maxthrottle) {
tpaFactor = (100 - (uint16_t)controlRateConfig->dynThrPID * (rcCommand[THROTTLE] - controlRateConfig->tpa_breakpoint) / (motorConfig->maxthrottle - controlRateConfig->tpa_breakpoint)) / 100.0f;
} else {
tpaFactor = (100 - controlRateConfig->dynThrPID) / 100.0f;
}
}
const float tpaFactor = STATE(FIXED_WING) ? calculateFixedWingTPAFactor() : calculateMultirotorTPAFactor();
// PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
//TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
for (int axis = 0; axis < 3; axis++) {
pidState[axis].kP = pidProfile->P8[axis] / FP_PID_RATE_P_MULTIPLIER;
pidState[axis].kI = pidProfile->I8[axis] / FP_PID_RATE_I_MULTIPLIER;
pidState[axis].kD = pidProfile->D8[axis] / FP_PID_RATE_D_MULTIPLIER;
// Apply TPA
if (STATE(FIXED_WING)) {
// Airplanes - scale all PIDs according to TPA
pidState[axis].kP *= tpaFactor;
pidState[axis].kI *= tpaFactor;
pidState[axis].kD *= tpaFactor * tpaFactor; // acceleration scales with speed^2
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor * tpaFactor;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
pidState[axis].kD = 0.0f;
pidState[axis].kFF = pidBank()->pid[axis].D / FP_PID_RATE_FF_MULTIPLIER * tpaFactor;
pidState[axis].kT = 0.0f;
}
else {
// Multicopter - scale roll/pitch PIDs according to TPA
if (axis != FD_YAW) {
pidState[axis].kP *= tpaFactor;
pidState[axis].kD *= tpaFactor;
}
}
const float axisTPA = (axis == FD_YAW) ? 1.0f : tpaFactor;
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER;
pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA;
pidState[axis].kFF = 0.0f;
if ((pidProfile->P8[axis] != 0) && (pidProfile->I8[axis] != 0)) {
pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
} else {
pidState[axis].kT = 0;
// Tracking anti-windup requires P/I/D to be all defined which is only true for MC
if ((pidBank()->pid[axis].P != 0) && (pidBank()->pid[axis].I != 0)) {
pidState[axis].kT = 2.0f / ((pidState[axis].kP / pidState[axis].kI) + (pidState[axis].kD / pidState[axis].kP));
} else {
pidState[axis].kT = 0;
}
}
}
@ -330,7 +370,7 @@ void updatePIDCoefficients(const pidProfile_t *pidProfile, const controlRateConf
}
#ifdef USE_FLM_HEADLOCK
static void pidApplyHeadingLock(const pidProfile_t *pidProfile, pidState_t *pidState)
static void pidApplyHeadingLock(pidState_t *pidState)
{
// Heading lock mode is different from Heading hold using compass.
// Heading lock attempts to keep heading at current value even if there is an external disturbance.
@ -342,18 +382,18 @@ static void pidApplyHeadingLock(const pidProfile_t *pidProfile, pidState_t *pidS
} else {
pidState->axisLockAccum += (pidState->rateTarget - pidState->gyroRate) * dT;
pidState->axisLockAccum = constrainf(pidState->axisLockAccum, -45, 45);
pidState->rateTarget = pidState->axisLockAccum * (pidProfile->P8[PIDMAG] / FP_PID_YAWHOLD_P_MULTIPLIER);
pidState->rateTarget = pidState->axisLockAccum * (pidBank()->pid[PID_HEADING].P / FP_PID_YAWHOLD_P_MULTIPLIER);
}
}
#endif
static float calcHorizonRateMagnitude(const pidProfile_t *pidProfile, const rxConfig_t *rxConfig)
static float calcHorizonRateMagnitude(void)
{
// Figure out the raw stick positions
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig()->midrc));
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig()->midrc));
const float mostDeflectedStickPos = constrain(MAX(stickPosAil, stickPosEle), 0, 500) / 500.0f;
const float modeTransitionStickPos = constrain(pidProfile->D8[PIDLEVEL], 0, 100) / 100.0f;
const float modeTransitionStickPos = constrain(pidBank()->pid[PID_LEVEL].D, 0, 100) / 100.0f;
float horizonRateMagnitude;
@ -368,13 +408,13 @@ static float calcHorizonRateMagnitude(const pidProfile_t *pidProfile, const rxCo
return horizonRateMagnitude;
}
static void pidLevel(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude)
static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float horizonRateMagnitude)
{
// This is ROLL/PITCH, run ANGLE/HORIZON controllers
const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile->max_angle_inclination[axis]);
const float angleTarget = pidRcCommandToAngle(rcCommand[axis], pidProfile()->max_angle_inclination[axis]);
const float angleError = angleTarget - attitude.raw[axis];
float angleRateTarget = constrainf(angleError * (pidProfile->P8[PIDLEVEL] / FP_PID_LEVEL_P_MULTIPLIER), -controlRateConfig->rates[axis] * 10.0f, controlRateConfig->rates[axis] * 10.0f);
float angleRateTarget = constrainf(angleError * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->rates[axis] * 10.0f, currentControlRateProfile->rates[axis] * 10.0f);
// Apply simple LPF to angleRateTarget to make response less jerky
// Ideas behind this:
@ -387,9 +427,9 @@ static void pidLevel(const pidProfile_t *pidProfile, const controlRateConfig_t *
// compensate for each slightest change
// 5) (2) and (4) lead to a simple idea of adding a low-pass filter on rateTarget for ANGLE mode damping
// response to rapid attitude changes and smoothing out self-leveling reaction
if (pidProfile->I8[PIDLEVEL]) {
if (pidBank()->pid[PID_LEVEL].I) {
// I8[PIDLEVEL] is filter cutoff frequency (Hz). Practical values of filtering frequency is 5-10 Hz
angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, pidProfile->I8[PIDLEVEL], dT);
angleRateTarget = pt1FilterApply4(&pidState->angleFilterState, angleRateTarget, pidBank()->pid[PID_LEVEL].I, dT);
}
// P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes)
@ -401,43 +441,81 @@ static void pidLevel(const pidProfile_t *pidProfile, const controlRateConfig_t *
}
/* Apply angular acceleration limit to rate target to limit extreme stick inputs to respect physical capabilities of the machine */
static void pidApplySetpointRateLimiting(const pidProfile_t *pidProfile, pidState_t *pidState, flight_dynamics_index_t axis)
static void pidApplySetpointRateLimiting(pidState_t *pidState, flight_dynamics_index_t axis)
{
const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile->axisAccelerationLimitYaw : pidProfile->axisAccelerationLimitRollPitch;
const uint32_t axisAccelLimit = (axis == FD_YAW) ? pidProfile()->axisAccelerationLimitYaw : pidProfile()->axisAccelerationLimitRollPitch;
if (axisAccelLimit > AXIS_ACCEL_MIN_LIMIT) {
pidState->rateTarget = rateLimitFilterApply4(&pidState->axisAccelFilter, pidState->rateTarget, (float)axisAccelLimit, dT);
}
}
static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *pidState, flight_dynamics_index_t axis)
#ifdef USE_SERVOS
static void pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
// Calculate new P-term and FF-term
float newPTerm = rateError * pidState->kP;
float newFFTerm = pidState->rateTarget * pidState->kFF;
// Additional P-term LPF on YAW axis
if (axis == FD_YAW && pidProfile()->yaw_lpf_hz) {
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile()->yaw_lpf_hz, dT);
}
// Calculate integral
pidState->errorGyroIf += rateError * pidState->kI * dT;
if (STATE(ANTI_WINDUP)) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf);
}
if (STATE(FIXED_WING) && pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
}
axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit);
#ifdef BLACKBOX
axisPID_P[axis] = newPTerm;
axisPID_I[axis] = pidState->errorGyroIf;
axisPID_D[axis] = newFFTerm;
axisPID_Setpoint[axis] = pidState->rateTarget;
#endif
}
#endif
static void pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
// Calculate new P-term
float newPTerm = rateError * pidState->kP;
// Constrain YAW by yaw_p_limit value if not servo driven (in that case servo limits apply)
if (axis == FD_YAW && (motorCount >= 4 && pidProfile->yaw_p_limit)) {
newPTerm = constrain(newPTerm, -pidProfile->yaw_p_limit, pidProfile->yaw_p_limit);
if (axis == FD_YAW && (motorCount >= 4 && pidProfile()->yaw_p_limit)) {
newPTerm = constrain(newPTerm, -pidProfile()->yaw_p_limit, pidProfile()->yaw_p_limit);
}
// Additional P-term LPF on YAW axis
if (axis == FD_YAW && pidProfile->yaw_lpf_hz) {
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile->yaw_lpf_hz, dT);
if (axis == FD_YAW && pidProfile()->yaw_lpf_hz) {
newPTerm = pt1FilterApply4(&pidState->ptermLpfState, newPTerm, pidProfile()->yaw_lpf_hz, dT);
}
// Calculate new D-term
float newDTerm;
if (pidProfile->D8[axis] == 0) {
if (pidBank()->pid[axis].D == 0) {
// optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0;
} else {
firFilterUpdate(&pidState->gyroRateFilter, pidProfile->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate);
firFilterUpdate(&pidState->gyroRateFilter, pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate);
newDTerm = firFilterApply(&pidState->gyroRateFilter) * (pidState->kD / dT);
// Apply additional lowpass
if (pidProfile->dterm_lpf_hz) {
newDTerm = pt1FilterApply4(&pidState->deltaLpfState, newDTerm, pidProfile->dterm_lpf_hz, dT);
if (pidProfile()->dterm_lpf_hz) {
newDTerm = pt1FilterApply4(&pidState->deltaLpfState, newDTerm, pidProfile()->dterm_lpf_hz, dT);
}
#ifdef USE_DTERM_NOTCH
@ -450,10 +528,10 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
// TODO: Get feedback from mixer on available correction range for each axis
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
const float newOutputLimited = constrainf(newOutput, -pidProfile->pidSumLimit, +pidProfile->pidSumLimit);
const float newOutputLimited = constrainf(newOutput, -pidProfile()->pidSumLimit, +pidProfile()->pidSumLimit);
// Prevent strong Iterm accumulation during stick inputs
const float integratorThreshold = (axis == FD_YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
const float integratorThreshold = (axis == FD_YAW) ? pidProfile()->yawItermIgnoreRate : pidProfile()->rollPitchItermIgnoreRate;
const float antiWindupScaler = constrainf(1.0f - (ABS(pidState->rateTarget) / integratorThreshold), 0.0f, 1.0f);
pidState->errorGyroIf += (rateError * pidState->kI * antiWindupScaler * dT) + ((newOutputLimited - newOutput) * pidState->kT * dT);
@ -465,12 +543,6 @@ static void pidApplyRateController(const pidProfile_t *pidProfile, pidState_t *p
pidState->errorGyroIfLimit = ABS(pidState->errorGyroIf);
}
#ifdef USE_SERVOS
if (STATE(FIXED_WING) && pidProfile->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile->fixedWingItermThrowLimit, pidProfile->fixedWingItermThrowLimit);
}
#endif
axisPID[axis] = newOutputLimited;
#ifdef BLACKBOX
@ -530,7 +602,7 @@ uint8_t getMagHoldState()
/*
* MAG_HOLD P Controller returns desired rotation rate in dps to be fed to Rate controller
*/
float pidMagHold(const pidProfile_t *pidProfile)
float pidMagHold(void)
{
float magHoldRate;
@ -577,7 +649,7 @@ float pidMagHold(const pidProfile_t *pidProfile)
New controller for 2deg error requires 2,6dps. 4dps for 3deg and so on up until mag_hold_rate_limit is reached.
*/
magHoldRate = error * pidProfile->P8[PIDMAG] / 30;
magHoldRate = error * pidBank()->pid[PID_HEADING].P / 30;
magHoldRate = constrainf(magHoldRate, -compassConfig()->mag_hold_rate_limit, compassConfig()->mag_hold_rate_limit);
magHoldRate = pt1FilterApply4(&magHoldRateFilter, magHoldRate, MAG_HOLD_ERROR_LPF_FREQ, dT);
@ -606,9 +678,8 @@ static void pidTurnAssistant(pidState_t *pidState)
}
#endif
void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig, const rxConfig_t *rxConfig)
void pidController(void)
{
uint8_t magHoldState = getMagHoldState();
if (magHoldState == MAG_HOLD_UPDATE_HEADING) {
@ -623,9 +694,9 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
float rateTarget;
if (axis == FD_YAW && magHoldState == MAG_HOLD_ENABLED) {
rateTarget = pidMagHold(pidProfile);
rateTarget = pidMagHold();
} else {
rateTarget = pidRcCommandToRate(rcCommand[axis], controlRateConfig->rates[axis]);
rateTarget = pidRcCommandToRate(rcCommand[axis], currentControlRateProfile->rates[axis]);
}
// Limit desired rate to something gyro can measure reliably
@ -634,14 +705,14 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
// Step 3: Run control for ANGLE_MODE, HORIZON_MODE, and HEADING_LOCK
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
const float horizonRateMagnitude = calcHorizonRateMagnitude(pidProfile, rxConfig);
pidLevel(pidProfile, controlRateConfig, &pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude);
pidLevel(pidProfile, controlRateConfig, &pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude);
const float horizonRateMagnitude = calcHorizonRateMagnitude();
pidLevel(&pidState[FD_ROLL], FD_ROLL, horizonRateMagnitude);
pidLevel(&pidState[FD_PITCH], FD_PITCH, horizonRateMagnitude);
}
#ifdef USE_FLM_HEADLOCK
if (FLIGHT_MODE(HEADING_LOCK) && magHoldState != MAG_HOLD_ENABLED) {
pidApplyHeadingLock(pidProfile, &pidState[FD_YAW]);
pidApplyHeadingLock(&pidState[FD_YAW]);
}
#endif
@ -653,12 +724,21 @@ void pidController(const pidProfile_t *pidProfile, const controlRateConfig_t *co
// Apply setpoint rate of change limits
for (int axis = 0; axis < 3; axis++) {
pidApplySetpointRateLimiting(pidProfile, &pidState[axis], axis);
pidApplySetpointRateLimiting(&pidState[axis], axis);
}
// Step 4: Run gyro-driven control
for (int axis = 0; axis < 3; axis++) {
// Apply PID setpoint controller
pidApplyRateController(pidProfile, &pidState[axis], axis); // scale gyro rate to DPS
#ifdef USE_SERVOS
if (STATE(FIXED_WING)) {
pidApplyFixedWingRateController(&pidState[axis], axis);
}
else {
pidApplyMulticopterRateController(&pidState[axis], axis);
}
#else
pidApplyMulticopterRateController(&pidState[axis], axis);
#endif
}
}

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

@ -18,6 +18,7 @@
#pragma once
#include "config/parameter_group.h"
#include "fc/runtime_config.h"
#define GYRO_SATURATION_LIMIT 1800 // 1800dps
#define PID_SUM_LIMIT_MIN 100
@ -36,23 +37,33 @@
#define MAG_HOLD_ERROR_LPF_FREQ 2
typedef enum {
PIDROLL,
PIDPITCH,
PIDYAW,
PIDALT,
PIDPOS,
PIDPOSR,
PIDNAVR,
PIDLEVEL,
PIDMAG,
PIDVEL,
/* PID MC FW */
PID_ROLL, // + +
PID_PITCH, // + +
PID_YAW, // + +
PID_POS_Z, // + +
PID_POS_XY, // + +
PID_VEL_XY, // + n/a
PID_SURFACE, // n/a n/a
PID_LEVEL, // + +
PID_HEADING, // + +
PID_VEL_Z, // + n/a
PID_ITEM_COUNT
} pidIndex_e;
typedef struct pid8_s {
uint8_t P;
uint8_t I;
uint8_t D;
} pid8_t;
typedef struct pidBank_s {
pid8_t pid[PID_ITEM_COUNT];
} pidBank_t;
typedef struct pidProfile_s {
uint8_t P8[PID_ITEM_COUNT];
uint8_t I8[PID_ITEM_COUNT];
uint8_t D8[PID_ITEM_COUNT];
pidBank_t bank_fw;
pidBank_t bank_mc;
uint16_t dterm_soft_notch_hz; // Dterm Notch frequency
uint16_t dterm_soft_notch_cutoff; // Dterm Notch Cutoff frequency
@ -79,13 +90,16 @@ typedef struct pidProfile_s {
PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
static inline const pidBank_t * pidBank() { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
static inline pidBank_t * pidBankMutable() { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
extern int16_t axisPID[];
extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
void pidInit(void);
#ifdef USE_DTERM_NOTCH
bool pidInitFilters(const pidProfile_t *pidProfile);
bool pidInitFilters(void);
#endif
void pidResetErrorAccumulators(void);
@ -95,8 +109,8 @@ struct motorConfig_s;
struct rxConfig_s;
void schedulePidGainsUpdate(void);
void updatePIDCoefficients(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig, const struct motorConfig_s *motorConfig);
void pidController(const pidProfile_t *pidProfile, const struct controlRateConfig_s *controlRateConfig, const struct rxConfig_s *rxConfig);
void updatePIDCoefficients(void);
void pidController(void);
float pidRateToRcCommand(float rateDPS, uint8_t rate);
int16_t pidAngleToRcCommand(float angleDeciDegrees, int16_t maxInclination);

6
src/main/io/osd.c
View file

@ -359,19 +359,19 @@ static void osdDrawSingleElement(uint8_t item)
case OSD_ROLL_PIDS:
{
sprintf(buff, "ROL %3d %3d %3d", pidProfile()->P8[PIDROLL], pidProfile()->I8[PIDROLL], pidProfile()->D8[PIDROLL]);
sprintf(buff, "ROL %3d %3d %3d", pidBank()->pid[PID_ROLL].P, pidBank()->pid[PID_ROLL].I, pidBank()->pid[PID_ROLL].D);
break;
}
case OSD_PITCH_PIDS:
{
sprintf(buff, "PIT %3d %3d %3d", pidProfile()->P8[PIDPITCH], pidProfile()->I8[PIDPITCH], pidProfile()->D8[PIDPITCH]);
sprintf(buff, "PIT %3d %3d %3d", pidBank()->pid[PID_PITCH].P, pidBank()->pid[PID_PITCH].I, pidBank()->pid[PID_PITCH].D);
break;
}
case OSD_YAW_PIDS:
{
sprintf(buff, "YAW %3d %3d %3d", pidProfile()->P8[PIDYAW], pidProfile()->I8[PIDYAW], pidProfile()->D8[PIDYAW]);
sprintf(buff, "YAW %3d %3d %3d", pidBank()->pid[PID_YAW].P, pidBank()->pid[PID_YAW].I, pidBank()->pid[PID_YAW].D);
break;
}

5
src/main/scheduler/scheduler.c
View file

@ -297,11 +297,10 @@ void scheduler(void)
// Execute task
const timeUs_t currentTimeBeforeTaskCall = micros();
selectedTask->taskFunc(currentTimeBeforeTaskCall);
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
#ifndef SKIP_TASK_STATISTICS
const timeUs_t taskExecutionTime = micros() - currentTimeBeforeTaskCall;
selectedTask->movingSumExecutionTime += taskExecutionTime - selectedTask->movingSumExecutionTime / TASK_MOVING_SUM_COUNT;
selectedTask->totalExecutionTime += taskExecutionTime; // time consumed by scheduler + task
selectedTask->maxExecutionTime = MAX(selectedTask->maxExecutionTime, taskExecutionTime);
#endif

4
src/main/target/ALIENFLIGHTF3/config.c
View file

@ -44,8 +44,8 @@ void targetConfiguration(void)
motorConfigMutable()->minthrottle = 1000;
motorConfigMutable()->maxthrottle = 2000;
motorConfigMutable()->motorPwmRate = 32000;
pidProfileMutable()->P8[ROLL] = 36;
pidProfileMutable()->P8[PITCH] = 36;
pidProfileMutable()->bank_mc.pid[ROLL].P = 36;
pidProfileMutable()->bank_mc.pid[PITCH].P = 36;
failsafeConfigMutable()->failsafe_delay = 2;
failsafeConfigMutable()->failsafe_off_delay = 0;
controlRateProfilesMutable(0)->rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT;

5
src/main/target/NAZE/target.h
View file

@ -60,7 +60,7 @@
#define UART3_RX_PIN PB11
#define UART3_TX_PIN PB10
#define SERIAL_PORT_COUNT 5
#define SERIAL_PORT_COUNT 4
#define MPU6500_CS_GPIO_CLK_PERIPHERAL NAZE_CS_GPIO_CLK_PERIPHERAL
#define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO
@ -74,7 +74,7 @@
#define M25P16_CS_PIN NAZE_SPI_CS_PIN
#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE
#define SERIAL_PORT_COUNT 4
#define SERIAL_PORT_COUNT 5
#define USE_FLASHFS
#define USE_FLASH_M25P16
@ -184,7 +184,6 @@
//#define USE_SERIAL_4WAY_BLHELI_INTERFACE
#define TARGET_MOTOR_COUNT 6
#define DISABLE_UNCOMMON_MIXERS
#define DEFAULT_FEATURES FEATURE_VBAT
#define DEFAULT_RX_FEATURE FEATURE_RX_PPM

11
src/main/target/common.h
View file

@ -32,8 +32,6 @@
#define USE_SERIALRX_IBUS // Cheap FlySky & Turnigy receivers
#if (FLASH_SIZE > 64)
#define ASYNC_GYRO_PROCESSING
#define BOOTLOG
#define BLACKBOX
#define GPS
#define GPS_PROTO_UBLOX
@ -43,10 +41,6 @@
#define USE_FLM_HEADLOCK
#define USE_FLM_TURN_ASSIST
#define TELEMETRY_FRSKY
#else
#define SKIP_TASK_STATISTICS
#define SKIP_CLI_COMMAND_HELP
#define SKIP_CLI_RESOURCES
#endif
#if defined(STM_FAST_TARGET)
@ -56,6 +50,9 @@
#endif
#if (FLASH_SIZE > 128)
#define ASYNC_GYRO_PROCESSING
#define BOOTLOG
#define BOOTLOG_DESCRIPTIONS
#define USE_64BIT_TIME
#define USE_GYRO_NOTCH_1
#define USE_GYRO_NOTCH_2
@ -63,7 +60,6 @@
#define CMS
#define USE_DASHBOARD
#define USE_MSP_DISPLAYPORT
#define BOOTLOG_DESCRIPTIONS
#define DASHBOARD_ARMED_BITMAP
#define GPS_PROTO_NMEA
#define GPS_PROTO_I2C_NAV
@ -88,6 +84,7 @@
#define MAX_BOOTLOG_ENTRIES 64
#else
#define CLI_MINIMAL_VERBOSITY
#define SKIP_TASK_STATISTICS
#define SKIP_CLI_COMMAND_HELP
#define SKIP_CLI_RESOURCES
#define DISABLE_UNCOMMON_MIXERS