From f5de06c59e3bc97df544dc8c67807342ea68ae49 Mon Sep 17 00:00:00 2001 From: borisbstyle Date: Wed, 24 Feb 2016 16:36:12 +0100 Subject: [PATCH] Enable Faster cycletimes (Sample Rates) on all targets // More automatic looptime calculations cleanup --- src/main/blackbox/blackbox_io.c | 2 +- src/main/config/config.c | 2 +- src/main/config/config_master.h | 2 +- src/main/flight/imu.c | 5 ++++- src/main/flight/imu.h | 3 ++- src/main/flight/mixer.c | 4 ++-- src/main/flight/pid.c | 22 +++++++++++++--------- src/main/flight/pid.h | 2 ++ src/main/io/serial_cli.c | 2 +- src/main/io/serial_msp.c | 31 ++++++++++++++++++++++++++++++- src/main/main.c | 3 +++ src/main/mw.c | 19 ++++++++++++------- 12 files changed, 72 insertions(+), 25 deletions(-) diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index d8fb0a8ec6..916b22ba25 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -599,7 +599,7 @@ bool blackboxDeviceOpen(void) * = floor((looptime_ns * 3) / 500.0) * = (looptime_ns * 3) / 500 */ - blackboxMaxHeaderBytesPerIteration = constrain((targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION); + blackboxMaxHeaderBytesPerIteration = constrain((targetPidLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION); return blackboxPort != NULL; } diff --git a/src/main/config/config.c b/src/main/config/config.c index 967f758f07..d838f5d4c6 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -414,7 +414,7 @@ static void resetConf(void) masterConfig.gyro_sync_denom = 4; masterConfig.gyro_soft_lpf_hz = 60; - masterConfig.pid_jitter_buffer = 20; + masterConfig.pid_process_denom = 1; resetAccelerometerTrims(&masterConfig.accZero); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 4da3b63dd8..1113083369 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -65,7 +65,7 @@ typedef struct master_t { uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) - uint8_t pid_jitter_buffer; // Jitter buffer time in us for pid controller for smoother motor output + uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate gyroConfig_t gyroConfig; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 29a8e4b04f..d72331ddd1 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -419,10 +419,13 @@ void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims) } } -void imuUpdateGyroAndAttitude(void) +void imuUpdateGyro(void) { gyroUpdate(); +} +void imuUpdateAttitude(void) +{ if (sensors(SENSOR_ACC) && isAccelUpdatedAtLeastOnce) { imuCalculateEstimatedAttitude(); } else { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index c896484312..3f64b5c50d 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -79,7 +79,8 @@ void imuConfigure( float getCosTiltAngle(void); void calculateEstimatedAltitude(uint32_t currentTime); void imuUpdateAccelerometer(rollAndPitchTrims_t *accelerometerTrims); -void imuUpdateGyroAndAttitude(void); +void imuUpdateGyro(void); +void imuUpdateAttitude(void); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6276752d67..212fb17238 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -960,7 +960,7 @@ bool isMixerUsingServos(void) void filterServos(void) { #ifdef USE_SERVOS - int16_t servoIdx; + static int16_t servoIdx; static bool servoFilterIsSet; static biquad_t servoFilterState[MAX_SUPPORTED_SERVOS]; @@ -971,7 +971,7 @@ void filterServos(void) if (mixerConfig->servo_lowpass_enable) { for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) { if (!servoFilterIsSet) { - BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetLooptime); + BiQuadNewLpf(mixerConfig->servo_lowpass_freq, &servoFilterState[servoIdx], targetPidLooptime); servoFilterIsSet = true; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 061287cce7..565097b1a7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -49,8 +49,8 @@ #include "config/runtime_config.h" extern uint8_t motorCount; -extern float dT; extern bool motorLimitReached; +uint32_t targetPidLooptime; int16_t axisPID[3]; @@ -77,6 +77,10 @@ typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig pidControllerFuncPtr pid_controller = pidMultiWiiRewrite; // which pid controller are we using, defaultMultiWii +void setTargetPidLooptime(uint8_t pidProcessDenom) { + targetPidLooptime = targetLooptime * pidProcessDenom; +} + void pidResetErrorAngle(void) { errorAngleI[ROLL] = 0; @@ -108,7 +112,7 @@ void scaleItermToRcInput(int axis, pidProfile_t *pidProfile) { static float iTermScaler[3] = {1.0f, 1.0f, 1.0f}; static float antiWindUpIncrement = 0; - if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetLooptime; // Calculate increment for 500ms period + if (!antiWindUpIncrement) antiWindUpIncrement = (0.001 / 500) * targetPidLooptime; // Calculate increment for 500ms period if (ABS(rcCommandReflection) > 0.7f && (!flightModeFlags)) { /* scaling should not happen in level modes */ /* Reset Iterm on high stick inputs. No scaling necessary here */ @@ -145,10 +149,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa int axis, deltaCount; float horizonLevelStrength = 1; - float dT = (float)targetLooptime * 0.000001f; + float dT = (float)targetPidLooptime * 0.000001f; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime); deltaStateIsSet = true; } @@ -284,7 +288,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control static int32_t previousAverageDelta[2]; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); + for (axis = 0; axis < 2; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime); deltaStateIsSet = true; } @@ -302,7 +306,7 @@ static void pidMultiWii23(pidProfile_t *pidProfile, controlRateConfig_t *control gyroError = gyroADC[axis] >> 2; error = rc - gyroError; - errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here + errorGyroI[axis] = constrain(errorGyroI[axis] + ((error * (uint16_t)targetPidLooptime) >> 12) , -16000, +16000); // WindUp 16 bits is ok here if (ABS(gyroADC[axis]) > (640 * 4)) { errorGyroI[axis] = 0; @@ -439,7 +443,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co int8_t horizonLevelStrength = 100; if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) { - for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetLooptime); + for (axis = 0; axis < 3; axis++) BiQuadNewLpf(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], targetPidLooptime); deltaStateIsSet = true; } @@ -502,7 +506,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used. // Time correction (to avoid different I scaling for different builds based on average cycle time) // is normalized to cycle time = 2048. - errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis]; + errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * pidProfile->I8[axis]; // limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated. // I coefficient (I8) moved before integration to make limiting independent from PID settings @@ -530,7 +534,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference // would be scaled by different dt each time. Division by dT fixes that. - delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetLooptime >> 4))) >> 6; + delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)targetPidLooptime >> 4))) >> 6; if (deltaStateIsSet) { delta = lrintf(applyBiQuadFilter((float) delta, &deltaBiQuadState[axis])) * 3; // Keep same scaling as unfiltered delta diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 98de99238a..709c0309a2 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -87,8 +87,10 @@ typedef struct pidProfile_s { extern int16_t axisPID[XYZ_AXIS_COUNT]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; bool antiWindupProtection; +extern uint32_t targetPidLooptime; void pidSetController(pidControllerType_e type); void pidResetErrorAngle(void); void pidResetErrorGyroState(uint8_t resetOption); +void setTargetPidLooptime(uint8_t pidProcessDenom); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index f05ec872ac..9b1ad502f5 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -685,7 +685,7 @@ const clivalue_t valueTable[] = { { "pid_delta_method", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.deltaMethod, .config.lookup = { TABLE_DELTA_METHOD } }, { "dterm_lpf_hz", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } }, { "dterm_average_count", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_average_count, .config.minmax = {2, 12 } }, - { "pid_jitter_buffer", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_jitter_buffer, .config.minmax = { 0, 100 } }, + { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, { "pid_controller", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.pidController, .config.lookup = { TABLE_PID_CONTROLLER } }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 9d621c8533..d9399888e8 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -106,14 +106,24 @@ void setGyroSamplingSpeed(uint16_t looptime) { uint16_t gyroSampleRate = 1000; uint8_t maxDivider = 1; - if (looptime != targetLooptime) { + if (looptime != targetLooptime || looptime == 0) { + if (looptime == 0) looptime = targetLooptime; // needed for pid controller changes #ifdef STM32F303xC if (looptime < 1000) { masterConfig.gyro_lpf = 0; gyroSampleRate = 125; maxDivider = 8; + if (looptime < 250) { + masterConfig.acc_hardware = 1; + masterConfig.baro_hardware = 1; + masterConfig.mag_hardware = 1; + masterConfig.pid_process_denom = 2; + } else if (looptime < 1000) { + masterConfig.pid_process_denom = 1; + } } else { masterConfig.gyro_lpf = 1; + masterConfig.pid_process_denom = 1; } #else if (looptime < 1000) { @@ -123,11 +133,27 @@ void setGyroSamplingSpeed(uint16_t looptime) { masterConfig.mag_hardware = 1; gyroSampleRate = 125; maxDivider = 8; + if (looptime < 250) { + masterConfig.pid_process_denom = 3; + } else if (looptime < 500) { + if (currentProfile->pidProfile.pidController == 2) { + masterConfig.pid_process_denom = 3; + } else { + masterConfig.pid_process_denom = 2; + } + } else { + if (currentProfile->pidProfile.pidController == 2) { + masterConfig.pid_process_denom = 2; + } else { + masterConfig.pid_process_denom = 1; + } + } } else { masterConfig.gyro_lpf = 1; masterConfig.acc_hardware = 0; masterConfig.baro_hardware = 0; masterConfig.mag_hardware = 0; + masterConfig.pid_process_denom = 1; } #endif masterConfig.gyro_sync_denom = constrain(looptime / gyroSampleRate, 1, maxDivider); @@ -1236,6 +1262,7 @@ static bool processInCommand(void) uint32_t i; uint16_t tmp; uint8_t rate; + uint8_t oldPid; #ifdef GPS uint8_t wp_no; int32_t lat = 0, lon = 0, alt = 0; @@ -1283,8 +1310,10 @@ static bool processInCommand(void) setGyroSamplingSpeed(read16()); break; case MSP_SET_PID_CONTROLLER: + oldPid = currentProfile->pidProfile.pidController; currentProfile->pidProfile.pidController = read8(); pidSetController(currentProfile->pidProfile.pidController); + if (oldPid != currentProfile->pidProfile.pidController) setGyroSamplingSpeed(0); // recalculate looptimes for new PID break; case MSP_SET_PID: if (IS_PID_CONTROLLER_FP_BASED(currentProfile->pidProfile.pidController)) { diff --git a/src/main/main.c b/src/main/main.c index bd06185f7a..15160b39b1 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -580,6 +580,9 @@ void init(void) afatfs_init(); #endif + if (masterConfig.gyro_lpf) masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + setTargetPidLooptime(masterConfig.pid_process_denom); // Initialize pid looptime + #ifdef BLACKBOX initBlackbox(); #endif diff --git a/src/main/mw.c b/src/main/mw.c index 7e8afbb49a..7c66721222 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -181,7 +181,7 @@ void filterRc(void){ // Set RC refresh rate for sampling and channels to filter initRxRefreshRate(&rxRefreshRate); - rcInterpolationFactor = rxRefreshRate / targetLooptime + 1; + rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1; if (isRXDataNew) { for (int channel=0; channel < 4; channel++) { @@ -644,7 +644,7 @@ static bool haveProcessedAnnexCodeOnce = false; void taskMainPidLoop(void) { - imuUpdateGyroAndAttitude(); + imuUpdateAttitude(); annexCode(); @@ -736,6 +736,9 @@ void taskMainPidLoop(void) // Function for loop trigger void taskMainPidLoopCheck(void) { static uint32_t previousTime; + static uint8_t pidUpdateCountdown; + + if (!pidUpdateCountdown) pidUpdateCountdown = masterConfig.pid_process_denom; cycleTime = micros() - previousTime; previousTime = micros(); @@ -747,14 +750,16 @@ void taskMainPidLoopCheck(void) { while (1) { if (gyroSyncCheckUpdate() || ((cycleTime + (micros() - previousTime)) >= (targetLooptime + GYRO_WATCHDOG_DELAY))) { - while (1) { - if (micros() >= masterConfig.pid_jitter_buffer + previousTime) break; + imuUpdateGyro(); + if (pidUpdateCountdown == 1) { + pidUpdateCountdown = masterConfig.pid_process_denom; + taskMainPidLoop(); + } else { + pidUpdateCountdown--; } - break; } + break; } - - taskMainPidLoop(); } void taskUpdateAccelerometer(void)