From 2d2d1e27a521b5b4912ecf323605d79c2ccd1e51 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 8 Apr 2016 17:57:50 +1000 Subject: [PATCH] FLIGHT: Make maximum inclination selectable for roll and pitch independantly. Closes #144 --- src/main/config/config.c | 5 +++-- src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- src/main/io/serial_cli.c | 3 ++- 4 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index 3d3b3f368f..95670e3a2f 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -134,7 +134,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 116; +static const uint8_t EEPROM_CONF_VERSION = 117; static void resetAccelerometerTrims(flightDynamicsTrims_t * accZero, flightDynamicsTrims_t * accGain) { @@ -184,7 +184,8 @@ void resetPidProfile(pidProfile_t *pidProfile) pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX; - pidProfile->max_angle_inclination = 300; // 30 degrees + pidProfile->max_angle_inclination[FD_ROLL] = 300; // 30 degrees + pidProfile->max_angle_inclination[FD_PITCH] = 300; // 30 degrees } #ifdef NAV diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b281af7485..70de6d4fd2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -168,7 +168,7 @@ static void pidOuterLoop(pidProfile_t *pidProfile, rxConfig_t *rxConfig) // This is ROLL/PITCH, run ANGLE/HORIZON controllers if ((FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { float angleTarget = pidRcCommandToAngle(rcCommand[axis]); - float angleError = (constrain(angleTarget, -pidProfile->max_angle_inclination, +pidProfile->max_angle_inclination) - attitude.raw[axis]) / 10.0f; + float angleError = (constrain(angleTarget, -pidProfile->max_angle_inclination[axis], +pidProfile->max_angle_inclination[axis]) - attitude.raw[axis]) / 10.0f; // P[LEVEL] defines self-leveling strength (both for ANGLE and HORIZON modes) if (FLIGHT_MODE(HORIZON_MODE)) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index aef09d959f..1dcc997b90 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -54,7 +54,7 @@ typedef struct pidProfile_s { uint16_t yaw_p_limit; - int16_t max_angle_inclination; // Max possible inclination + int16_t max_angle_inclination[2]; // Max possible inclination (roll and pitch axis separately } pidProfile_t; extern int16_t axisPID[XYZ_AXIS_COUNT]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 2593fbcfba..11df81485e 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -722,7 +722,8 @@ const clivalue_t valueTable[] = { { "i_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDLEVEL], .config.minmax = { 0, 100 }, 0 }, { "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 100 }, 0 }, - { "max_angle_inclination", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination, .config.minmax = { 100, 900 }, 0 }, + { "max_angle_inclination_rll", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination[FD_ROLL], .config.minmax = { 100, 900 }, 0 }, + { "max_angle_inclination_pit", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.max_angle_inclination[FD_PITCH], .config.minmax = { 100, 900 }, 0 }, { "gyro_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } }, { "acc_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } },