diff --git a/src/main/config/config.c b/src/main/config/config.c index ac2300ca2e..0a654e2edf 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -70,7 +70,7 @@ void setPIDController(int type); // FIXME PID code needs to be in flight_pid.c/h void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfig, gimbalConfig_t *gimbalConfigToUse); -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse); +void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse); #define FLASH_TO_RESERVE_FOR_CONFIG 0x800 @@ -433,7 +433,7 @@ void activateConfig(void) generatePitchRollCurve(¤tProfile->controlRateConfig); generateThrottleCurve(¤tProfile->controlRateConfig, &masterConfig.escAndServoConfig); - useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig); + useRcControlsConfig(currentProfile->modeActivationConditions, &masterConfig.escAndServoConfig, ¤tProfile->pidProfile); useGyroConfig(&masterConfig.gyroConfig); #ifdef TELEMETRY diff --git a/src/main/io/rc_controls.c b/src/main/io/rc_controls.c index 479f88d1c5..225fb5ef3b 100644 --- a/src/main/io/rc_controls.c +++ b/src/main/io/rc_controls.c @@ -47,6 +47,7 @@ #include "io/rc_curves.h" static escAndServoConfig_t *escAndServoConfig; +static pidProfile_t *pidProfile; static bool isUsingSticksToArm = true; @@ -289,8 +290,31 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU { .adjustmentFunction = ADJUSTMENT_YAW_RATE, .step = 1 + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_P, + .step = 1 + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_I, + .step = 1 + }, + { + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .step = 1 + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_P, + .step = 1 + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_I, + .step = 1 + }, + { + .adjustmentFunction = ADJUSTMENT_YAW_D, + .step = 1 } - }; #define ADJUSTMENT_FUNCTION_CONFIG_INDEX_OFFSET 1 @@ -352,6 +376,36 @@ void applyAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentF newValue = (int)controlRateConfig->yawRate + delta; controlRateConfig->yawRate = constrain(newValue, 0, 100); // FIXME magic numbers repeated in serial_cli.c break; + case ADJUSTMENT_PITCH_ROLL_P: + newValue = (int)pidProfile->P8[PIDPITCH] + delta; + pidProfile->P8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->P8[PIDROLL] + delta; + pidProfile->P8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + break; + case ADJUSTMENT_PITCH_ROLL_I: + newValue = (int)pidProfile->I8[PIDPITCH] + delta; + pidProfile->I8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->I8[PIDROLL] + delta; + pidProfile->I8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + break; + case ADJUSTMENT_PITCH_ROLL_D: + newValue = (int)pidProfile->D8[PIDPITCH] + delta; + pidProfile->D8[PIDPITCH] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + newValue = (int)pidProfile->D8[PIDROLL] + delta; + pidProfile->D8[PIDROLL] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + break; + case ADJUSTMENT_YAW_P: + newValue = (int)pidProfile->P8[PIDYAW] + delta; + pidProfile->P8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + break; + case ADJUSTMENT_YAW_I: + newValue = (int)pidProfile->I8[PIDYAW] + delta; + pidProfile->I8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + break; + case ADJUSTMENT_YAW_D: + newValue = (int)pidProfile->D8[PIDYAW] + delta; + pidProfile->D8[PIDYAW] = constrain(newValue, 0, 200); // FIXME magic numbers repeated in serial_cli.c + break; default: break; }; @@ -420,11 +474,12 @@ void updateAdjustmentStates(adjustmentRange_t *adjustmentRanges) } } -void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse) +void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions, escAndServoConfig_t *escAndServoConfigToUse, pidProfile_t *pidProfileToUse) { uint8_t index; escAndServoConfig = escAndServoConfigToUse; + pidProfile = pidProfileToUse; for (index = 0; index < MAX_MODE_ACTIVATION_CONDITION_COUNT; index++) { modeActivationCondition_t *modeActivationCondition = &modeActivationConditions[index]; diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 1f2b13de34..03e4bf280b 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -140,9 +140,15 @@ typedef enum { ADJUSTMENT_THROTTLE_EXPO, ADJUSTMENT_PITCH_ROLL_RATE, ADJUSTMENT_YAW_RATE, + ADJUSTMENT_PITCH_ROLL_P, + ADJUSTMENT_PITCH_ROLL_I, + ADJUSTMENT_PITCH_ROLL_D, + ADJUSTMENT_YAW_P, + ADJUSTMENT_YAW_I, + ADJUSTMENT_YAW_D, } adjustmentFunction_e; -#define ADJUSTMENT_FUNCTION_COUNT 6 +#define ADJUSTMENT_FUNCTION_COUNT 12 typedef struct adjustmentConfig_s { uint8_t adjustmentFunction;