rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Enable Faster cycletimes (Sample Rates) on all targets // More automatic looptime calculations 

cleanup
This commit is contained in:
borisbstyle 2016-02-24 16:36:12 +01:00
parent 981bddf182
commit f5de06c59e
12 changed files with 72 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main/blackbox/blackbox_io.c
View File

@ -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;
}

2
src/main/config/config.c
View File

@ -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);

2
src/main/config/config_master.h
View File

@ -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;

5
src/main/flight/imu.c
View File

@ -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 {

3
src/main/flight/imu.h
View File

@ -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);

4
src/main/flight/mixer.c
View File

@ -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;
}

22
src/main/flight/pid.c
View File

@ -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

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

@ -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);

2
src/main/io/serial_cli.c
View File

@ -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 } },

31
src/main/io/serial_msp.c
View File

@ -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)) {

3
src/main/main.c
View File

@ -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

19
src/main/mw.c
View File

@ -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)