Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-22 07:45:29 +03:00
Code Issues Wiki Activity

Harakiri PID controller make hardcoded parameters configurable

Browse source
This commit is contained in:
Michael Jakob 2015-02-26 20:40:28 +01:00
parent 9f95334347
commit b20dc77a74
4 changed files with 14 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

3
src/main/config/config.c
View file

@ -187,6 +187,9 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->A_level = 5.0f; pidProfile->A_level = 5.0f;
pidProfile->H_level = 3.0f; pidProfile->H_level = 3.0f;
pidProfile->H_sensitivity = 75; pidProfile->H_sensitivity = 75;
pidProfile->pid5_oldyw = 0;
pidProfile->pid5_maincuthz = 12;
} }
#ifdef GPS #ifdef GPS

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

@ -577,7 +577,11 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
uint8_t axis; uint8_t axis;
float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale; float ACCDeltaTimeINS, FLOATcycleTime, Mwii3msTimescale;
MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // maincuthz (default 0 (disabled), Range 1-50Hz) if (pidProfile->dterm_cut_hz) {
MainDptCut = RCconstPI / constrain(pidProfile->dterm_cut_hz, 1, 50); // dterm_cut_hz (default 0, Range 1-50Hz)
} else {
MainDptCut = RCconstPI / 12.0f; // default is 12Hz to maintain initial behavior of PID5
}
FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms FLOATcycleTime = (float)constrain(cycleTime, 1, 100000); // 1us - 100ms
ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now ACCDeltaTimeINS = FLOATcycleTime * 0.000001f; // ACCDeltaTimeINS is in seconds now
RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element RCfactor = ACCDeltaTimeINS / (MainDptCut + ACCDeltaTimeINS); // used for pt1 element
@ -589,7 +593,7 @@ rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
for (axis = 0; axis < 2; axis++) { for (axis = 0; axis < 2; axis++) {
int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea int32_t tmp = (int32_t)((float)gyroADC[axis] * 0.3125f); // Multiwii masks out the last 2 bits, this has the same idea
gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight gyroADCQuant = (float)tmp * 3.2f; // but delivers more accuracy and also reduces jittery flight
rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers rcCommandAxis = (float)rcCommand[axis]; // Calculate common values for pid controllers
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) { if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
#ifdef GPS #ifdef GPS
error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis]; error = constrain(2.0f * rcCommandAxis + GPS_angle[axis], -((int) max_angle_inclination), +max_angle_inclination) - inclination.raw[axis] + angleTrim->raw[axis];

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

@ -61,7 +61,9 @@ typedef struct pidProfile_s {
float A_level; float A_level;
float H_level; float H_level;
uint8_t H_sensitivity; uint8_t H_sensitivity;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300) uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint8_t pid5_oldyw; // Old yaw behavior for PID5
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5 uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames uint8_t pterm_cut_hz; // Used for fitlering Pterm noise on noisy frames
uint8_t gyro_cut_hz; // Used for soft gyro filtering uint8_t gyro_cut_hz; // Used for soft gyro filtering

3
src/main/io/serial_cli.c
View file

@ -514,6 +514,9 @@ const clivalue_t valueTable[] = {
{ "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 }, { "pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pterm_cut_hz, 0, 200 },
{ "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 }, { "gyro_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_cut_hz, 0, 200 },
{ "pid5_oldyw", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_oldyw, 0, 1 },
{ "pid5_maincuthz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pid5_maincuthz, 1, 50 },
#ifdef BLACKBOX #ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 },
{ "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 },