From d72b4e96c937c586c4853c13a4bbc9bf83752918 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Wed, 7 Dec 2016 16:32:33 +0000 Subject: [PATCH] Separated out pidConfig_t items --- src/main/blackbox/blackbox.c | 2 +- src/main/config/config_master.h | 4 ++-- src/main/fc/config.c | 10 +++++----- src/main/fc/fc_msp.c | 4 ++-- src/main/fc/mw.c | 4 ++-- src/main/flight/pid.c | 12 +++++------- src/main/flight/pid.h | 8 ++++++-- src/main/io/serial_cli.c | 10 +++++----- src/main/main.c | 2 +- src/main/target/ALIENFLIGHTF3/config.c | 2 +- src/main/target/ALIENFLIGHTF4/config.c | 2 +- src/main/target/MOTOLAB/config.c | 2 +- src/main/target/MULTIFLITEPICO/config.c | 2 +- 13 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index dcae20923..9f74d479c 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -1201,7 +1201,7 @@ static bool blackboxWriteSysinfo() BLACKBOX_PRINT_HEADER_LINE("looptime:%d", gyro.targetLooptime); BLACKBOX_PRINT_HEADER_LINE("gyro_sync_denom:%d", gyroConfig()->gyro_sync_denom); - BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", masterConfig.pid_process_denom); + BLACKBOX_PRINT_HEADER_LINE("pid_process_denom:%d", pidConfig()->pid_process_denom); BLACKBOX_PRINT_HEADER_LINE("rcRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcRate8); BLACKBOX_PRINT_HEADER_LINE("rcExpo:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcExpo8); BLACKBOX_PRINT_HEADER_LINE("rcYawRate:%d", masterConfig.profile[masterConfig.current_profile_index].controlRateProfile[masterConfig.profile[masterConfig.current_profile_index].activeRateProfile].rcYawRate8); diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 338f56df8..cb5e74281 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -98,6 +98,7 @@ #define sdcardConfig(x) (&masterConfig.sdcardConfig) #define blackboxConfig(x) (&masterConfig.blackboxConfig) #define flashConfig(x) (&masterConfig.flashConfig) +#define pidConfig(x) (&masterConfig.pidConfig) // System-wide @@ -128,9 +129,8 @@ typedef struct master_s { imuConfig_t imuConfig; rollAndPitchTrims_t accelerometerTrims; // accelerometer trim - uint16_t max_angle_inclination; // max inclination allowed in angle (level) mode. default 500 (50 degrees). - uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate + pidConfig_t pidConfig; uint8_t debug_mode; // Processing denominator for PID controller vs gyro sampling rate diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 4359fcd2b..a4c6da878 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -613,14 +613,15 @@ void createDefaultConfig(master_t *config) config->gyroConfig.gyro_lpf = GYRO_LPF_256HZ; // 256HZ default #ifdef STM32F10X config->gyroConfig.gyro_sync_denom = 8; - config->pid_process_denom = 1; + config->pidConfig.pid_process_denom = 1; #elif defined(USE_GYRO_SPI_MPU6000) || defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_ICM20689) config->gyroConfig.gyro_sync_denom = 1; - config->pid_process_denom = 4; + config->pidConfig.pid_process_denom = 4; #else config->gyroConfig.gyro_sync_denom = 4; - config->pid_process_denom = 2; + config->pidConfig.pid_process_denom = 2; #endif + config->pidConfig.max_angle_inclination = 700; // 70 degrees config->gyroConfig.gyro_soft_lpf_type = FILTER_PT1; config->gyroConfig.gyro_soft_lpf_hz = 90; config->gyroConfig.gyro_soft_notch_hz_1 = 400; @@ -640,7 +641,6 @@ void createDefaultConfig(master_t *config) config->boardAlignment.pitchDegrees = 0; config->boardAlignment.yawDegrees = 0; config->accelerometerConfig.acc_hardware = ACC_DEFAULT; // default/autodetect - config->max_angle_inclination = 700; // 70 degrees config->rcControlsConfig.yaw_control_direction = 1; config->gyroConfig.gyroMovementCalibrationThreshold = 32; @@ -1050,7 +1050,7 @@ void validateAndFixGyroConfig(void) } if (gyroConfig()->gyro_lpf != GYRO_LPF_256HZ && gyroConfig()->gyro_lpf != GYRO_LPF_NONE) { - masterConfig.pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed + pidConfig()->pid_process_denom = 1; // When gyro set to 1khz always set pid speed 1:1 to sampling speed gyroConfig()->gyro_sync_denom = 1; } } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index bbc065145..50d9e9e45 100755 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1093,7 +1093,7 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn sbufWriteU8(dst, 1); } else { sbufWriteU8(dst, gyroConfig()->gyro_sync_denom); - sbufWriteU8(dst, masterConfig.pid_process_denom); + sbufWriteU8(dst, pidConfig()->pid_process_denom); } sbufWriteU8(dst, motorConfig()->useUnsyncedPwm); sbufWriteU8(dst, motorConfig()->motorPwmProtocol); @@ -1442,7 +1442,7 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) case MSP_SET_ADVANCED_CONFIG: gyroConfig()->gyro_sync_denom = sbufReadU8(src); - masterConfig.pid_process_denom = sbufReadU8(src); + pidConfig()->pid_process_denom = sbufReadU8(src); motorConfig()->useUnsyncedPwm = sbufReadU8(src); #ifdef USE_DSHOT motorConfig()->motorPwmProtocol = constrain(sbufReadU8(src), 0, PWM_TYPE_MAX - 1); diff --git a/src/main/fc/mw.c b/src/main/fc/mw.c index 0743b6cff..0f775b79b 100644 --- a/src/main/fc/mw.c +++ b/src/main/fc/mw.c @@ -679,7 +679,7 @@ void subTaskPidController(void) // PID - note this is function pointer set by setPIDController() pidController( ¤tProfile->pidProfile, - masterConfig.max_angle_inclination, + pidConfig()->max_angle_inclination, &masterConfig.accelerometerTrims, rxConfig()->midrc ); @@ -792,7 +792,7 @@ void subTaskMotorUpdate(void) uint8_t setPidUpdateCountDown(void) { if (gyroConfig()->gyro_soft_lpf_hz) { - return masterConfig.pid_process_denom - 1; + return pidConfig()->pid_process_denom - 1; } else { return 1; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 80bbf9b2f..26dd218fe 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -163,8 +163,7 @@ void pidInitConfig(const pidProfile_t *pidProfile) { // Betaflight pid controller, which will be maintained in the future with additional features specialised for current (mini) multirotor usage. // Based on 2DOF reference design (matlab) -void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const rollAndPitchTrims_t *angleTrim, uint16_t midrc) +void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, uint16_t midrc) { static float previousRateError[2]; static float previousSetpoint[3]; @@ -217,13 +216,12 @@ void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclinatio // Yaw control is GYRO based, direct sticks control is applied to rate PID if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && axis != YAW) { // calculate error angle and limit the angle to the max inclination + float errorAngle = pidProfile->levelSensitivity * rcCommand[axis]; #ifdef GPS - const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis] + GPS_angle[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here -#else - const float errorAngle = (constrainf(pidProfile->levelSensitivity * rcCommand[axis], -((int) max_angle_inclination), - +max_angle_inclination) - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; // 16 bits is ok here + errorAngle += GPS_angle[axis]; #endif + errorAngle = constrainf(errorAngle, -max_angle_inclination, max_angle_inclination); + errorAngle = (errorAngle - attitude.raw[axis] + angleTrim->raw[axis]) / 10.0f; if (FLIGHT_MODE(ANGLE_MODE)) { // ANGLE mode - control is angle based, so control loop is needed setpointRate[axis] = errorAngle * pidProfile->P8[PIDLEVEL] / 10.0f; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8ecd833e0..27387afbb 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -90,9 +90,13 @@ typedef struct pidProfile_s { #endif } pidProfile_t; +typedef struct pidConfig_s { + uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate + uint16_t max_angle_inclination; +} pidConfig_t; + union rollAndPitchTrims_u; -void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, - const union rollAndPitchTrims_u *angleTrim, uint16_t midrc); +void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, uint16_t midrc); extern float axisPIDf[3]; extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 57a85a6e0..40132fb2d 100755 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -818,7 +818,6 @@ const clivalue_t valueTable[] = { { "align_board_pitch", VAR_INT16 | MASTER_VALUE, &boardAlignment()->pitchDegrees, .config.minmax = { -180, 360 } }, { "align_board_yaw", VAR_INT16 | MASTER_VALUE, &boardAlignment()->yawDegrees, .config.minmax = { -180, 360 } }, - { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &masterConfig.max_angle_inclination, .config.minmax = { 100, 900 } }, { "gyro_lpf", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_lpf, .config.lookup = { TABLE_GYRO_LPF } }, { "gyro_sync_denom", VAR_UINT8 | MASTER_VALUE, &gyroConfig()->gyro_sync_denom, .config.minmax = { 1, 8 } }, { "gyro_lowpass_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &gyroConfig()->gyro_soft_lpf_type, .config.lookup = { TABLE_LOWPASS_TYPE } }, @@ -844,6 +843,7 @@ const clivalue_t valueTable[] = { { "yaw_motor_direction", VAR_INT8 | MASTER_VALUE, &mixerConfig()->yaw_motor_direction, .config.minmax = { -1, 1 } }, { "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } }, { "pidsum_limit", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.pidSumLimit, .config.minmax = { 0.1, 1.0 } }, + { "max_angle_inclination", VAR_UINT16 | MASTER_VALUE, &pidConfig()->max_angle_inclination, .config.minmax = { 100, 900 } }, #ifdef USE_SERVOS { "servo_center_pulse", VAR_UINT16 | MASTER_VALUE, &servoConfig()->servoCenterPulse, .config.minmax = { PWM_RANGE_ZERO, PWM_RANGE_MAX } }, { "tri_unarmed_servo", VAR_INT8 | MASTER_VALUE | MODE_LOOKUP, &servoMixerConfig()->tri_unarmed_servo, .config.lookup = { TABLE_OFF_ON } }, @@ -911,7 +911,7 @@ const clivalue_t valueTable[] = { { "accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_accum_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {15, 1000 } }, { "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } }, - { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } }, + { "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &pidConfig()->pid_process_denom, .config.minmax = { 1, 8 } }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PITCH], .config.minmax = { 0, 200 } }, { "i_pitch", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PITCH], .config.minmax = { 0, 200 } }, @@ -3658,8 +3658,8 @@ static void cliTasks(char *cmdline) int subTaskFrequency; if (taskId == TASK_GYROPID) { subTaskFrequency = (int)(1000000.0f / ((float)cycleTime)); - taskFrequency = subTaskFrequency / masterConfig.pid_process_denom; - if (masterConfig.pid_process_denom > 1) { + taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom; + if (pidConfig()->pid_process_denom > 1) { cliPrintf("%02d - (%13s) ", taskId, taskInfo.taskName); } else { taskFrequency = subTaskFrequency; @@ -3678,7 +3678,7 @@ static void cliTasks(char *cmdline) cliPrintf("%6d %7d %7d %4d.%1d%% %4d.%1d%% %9d\r\n", taskFrequency, taskInfo.maxExecutionTime, taskInfo.averageExecutionTime, maxLoad/10, maxLoad%10, averageLoad/10, averageLoad%10, taskInfo.totalExecutionTime / 1000); - if (taskId == TASK_GYROPID && masterConfig.pid_process_denom > 1) { + if (taskId == TASK_GYROPID && pidConfig()->pid_process_denom > 1) { cliPrintf(" - (%13s) %6d\r\n", taskInfo.subTaskName, subTaskFrequency); } } diff --git a/src/main/main.c b/src/main/main.c index 2bf3e063a..b7ea9df25 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -414,7 +414,7 @@ void init(void) LED1_OFF; // gyro.targetLooptime set in sensorsAutodetect(), so we are ready to call pidSetTargetLooptime() - pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * masterConfig.pid_process_denom); // Initialize pid looptime + pidSetTargetLooptime((gyro.targetLooptime + LOOPTIME_SUSPEND_TIME) * pidConfig()->pid_process_denom); // Initialize pid looptime pidInitFilters(¤tProfile->pidProfile); pidInitConfig(¤tProfile->pidProfile); diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index c14ed47be..6628575bc 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -81,7 +81,7 @@ void targetConfiguration(master_t *config) if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - config->pid_process_denom = 2; + config->pidConfig.pid_process_denom = 2; } config->profile[0].pidProfile.P8[ROLL] = 90; diff --git a/src/main/target/ALIENFLIGHTF4/config.c b/src/main/target/ALIENFLIGHTF4/config.c index 7a9b8bc54..e67d717c8 100644 --- a/src/main/target/ALIENFLIGHTF4/config.c +++ b/src/main/target/ALIENFLIGHTF4/config.c @@ -62,7 +62,7 @@ void targetConfiguration(master_t *config) if (hardwareMotorType == MOTOR_BRUSHED) { config->motorConfig.motorPwmRate = BRUSHED_MOTORS_PWM_RATE; - config->pid_process_denom = 1; + config->pidConfig.pid_process_denom = 1; } if (hardwareRevision == AFF4_REV_1) { diff --git a/src/main/target/MOTOLAB/config.c b/src/main/target/MOTOLAB/config.c index 94a217a36..daee3f7f3 100755 --- a/src/main/target/MOTOLAB/config.c +++ b/src/main/target/MOTOLAB/config.c @@ -27,5 +27,5 @@ void targetConfiguration(master_t *config) { config->gyroConfig.gyro_sync_denom = 4; - config->pid_process_denom = 1; + config->pidConfig.pid_process_denom = 1; } diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index dfbe0a8fe..54a609b48 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -72,7 +72,7 @@ void targetConfiguration(master_t *config) config->motorConfig.motorPwmRate = 17000; config->gyroConfig.gyro_sync_denom = 4; - config->pid_process_denom = 1; + config->pidConfig.pid_process_denom = 1; config->profile[0].pidProfile.P8[ROLL] = 70; config->profile[0].pidProfile.I8[ROLL] = 62;