Merge pull request #1835 from martinbudden/bf_config_pid

Separated out pidConfig_t items

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

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
 

src/main/fc/config.c

@@ -614,14 +614,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;
@@ -641,7 +642,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;
 
@@ -1051,7 +1051,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;
     }
 }

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

src/main/fc/mw.c

@@ -679,7 +679,7 @@ void subTaskPidController(void)
     // PID - note this is function pointer set by setPIDController()
     pidController(
         &currentProfile->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;
     }

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,
+void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, uint16_t midrc)
-         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),
+            errorAngle += GPS_angle[axis];
-                    +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
 #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;

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,
+void pidController(const pidProfile_t *pidProfile, uint16_t max_angle_inclination, const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
-        const union rollAndPitchTrims_u *angleTrim, uint16_t midrc);
 
 extern float axisPIDf[3];
 extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

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;
+                taskFrequency = subTaskFrequency / pidConfig()->pid_process_denom;
-                if (masterConfig.pid_process_denom > 1) {
+                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);
             }
         }

src/main/main.c

@@ -415,7 +415,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(&currentProfile->pidProfile);
     pidInitConfig(&currentProfile->pidProfile);
 

src/main/target/ALIENFLIGHTF3/config.c

@@ -82,7 +82,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;

src/main/target/ALIENFLIGHTF4/config.c

@@ -63,7 +63,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) {

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

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;