diff --git a/.travis.yml b/.travis.yml index fccc04f6e2..654f433661 100755 --- a/.travis.yml +++ b/.travis.yml @@ -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 diff --git a/docs/Cli.md b/docs/Cli.md index b3e3c76553..5c184f77be 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -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 | diff --git a/install-toolchain.sh b/install-toolchain.sh index 959b080682..ec9e4b4dd3 100755 --- a/install-toolchain.sh +++ b/install-toolchain.sh @@ -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 diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 8dcc1687d3..f5657c2e36 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -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)); diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 097b142909..835dd7d65f 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -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; } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index b551e9dc7d..251924cc68 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -497,7 +497,7 @@ void init(void) flashLedsAndBeep(); #ifdef USE_DTERM_NOTCH - pidInitFilters(pidProfile()); + pidInitFilters(); #endif imuInit(); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index b928fd10cf..a217649c2b 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 5564f58d37..aa7b75a892 100755 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -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) { diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index 78faf8217b..ce7f45b123 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -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; diff --git a/src/main/fc/serial_cli.c b/src/main/fc/serial_cli.c index 99cae1d157..7af9da5025 100644 --- a/src/main/fc/serial_cli.c +++ b/src/main/fc/serial_cli.c @@ -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) diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 6661b44483..889810eda2 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -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) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 86c7a35a34..8f35ca964c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -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 } } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index b885e7f155..abade60976 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -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); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1a9592117f..45e136261b 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -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; } diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 102cedf4d2..be4596272d 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -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 diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index 3cc1b69c66..013daf84a6 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -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; diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index eb767b680f..0c6996ebbf 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -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 diff --git a/src/main/target/common.h b/src/main/target/common.h index 90babbffd0..35aa32dffd 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -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