diff --git a/src/main/config/config.c b/src/main/config/config.c index ad55a40972..75794acf54 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -159,7 +159,7 @@ static void resetPidProfile(pidProfile_t *pidProfile) pidProfile->D_f[YAW] = 0.05f; pidProfile->A_level = 5.0f; pidProfile->H_level = 3.0f; - pidProfile->H_sensitivity = 10.0f; + pidProfile->H_sensitivity = 75; } #ifdef GPS diff --git a/src/main/flight/flight.c b/src/main/flight/flight.c index b55ef9cd80..b9737f6e1d 100644 --- a/src/main/flight/flight.c +++ b/src/main/flight/flight.c @@ -96,6 +96,7 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control { float RateError, errorAngle, AngleRate, gyroRate; float ITerm,PTerm,DTerm; + int32_t stickPosAil, stickPosEle, mostDeflectedPos; static float lastGyroRate[3]; static float delta1[3], delta2[3]; float delta, deltaSum; @@ -107,19 +108,23 @@ static void pidBaseflight(pidProfile_t *pidProfile, controlRateConfig_t *control if (FLIGHT_MODE(HORIZON_MODE)) { - if(abs(rcCommand[FD_ROLL]) > abs(rcCommand[FD_PITCH])){ - axis = FD_ROLL; + // Figure out the raw stick positions + stickPosAil = getRcStickPosition(FD_ROLL); + stickPosEle = getRcStickPosition(FD_PITCH); + + if(abs(stickPosAil) > abs(stickPosEle)){ + mostDeflectedPos = abs(stickPosAil); } else { - axis = FD_PITCH; + mostDeflectedPos = abs(stickPosEle); } // Progressively turn off the horizon self level strength as the stick is banged over - horizonLevelStrength = (float)(500 - abs(rcCommand[axis])) / 500; // 1 at centre stick, 0 = max stick deflection + horizonLevelStrength = (float)(500 - mostDeflectedPos) / 500; // 1 at centre stick, 0 = max stick deflection if(pidProfile->H_sensitivity == 0){ horizonLevelStrength = 0; } else { - horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (10 / pidProfile->H_sensitivity)) + 1, 0, 1); + horizonLevelStrength = constrainf(((horizonLevelStrength - 1) * (100 / pidProfile->H_sensitivity)) + 1, 0, 1); } } diff --git a/src/main/flight/flight.h b/src/main/flight/flight.h index 6461ce21e8..6e23355c97 100644 --- a/src/main/flight/flight.h +++ b/src/main/flight/flight.h @@ -42,7 +42,7 @@ typedef struct pidProfile_s { float D_f[3]; float A_level; float H_level; - float H_sensitivity; + uint8_t H_sensitivity; } pidProfile_t; typedef enum { diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 745acd4682..54a8e8f772 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -140,7 +140,7 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch); void updateActivatedModes(modeActivationCondition_t *modeActivationConditions); - +int32_t getRcStickPosition(int32_t axis); typedef enum { ADJUSTMENT_NONE = 0, diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index b1a3156300..81eef513d6 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -392,7 +392,7 @@ const clivalue_t valueTable[] = { { "level_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_level, 0, 10 }, { "level_angle", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.A_level, 0, 10 }, - { "sensitivity_horizon", VAR_FLOAT | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, + { "sensitivity_horizon", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.H_sensitivity, 0, 250 }, { "p_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.P8[PIDALT], 0, 200 }, { "i_alt", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.I8[PIDALT], 0, 200 }, diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 5c2d834eed..e17a599ed3 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -846,7 +846,7 @@ static bool processOutCommand(uint8_t cmdMSP) if (i == PIDLEVEL) { serialize8(constrain(lrintf(currentProfile->pidProfile.A_level * 10.0f), 0, 250)); serialize8(constrain(lrintf(currentProfile->pidProfile.H_level * 10.0f), 0, 250)); - serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity * 10.0f), 0, 250)); + serialize8(constrain(lrintf(currentProfile->pidProfile.H_sensitivity), 0, 250)); } else { serialize8(currentProfile->pidProfile.P8[i]); serialize8(currentProfile->pidProfile.I8[i]); @@ -1165,7 +1165,7 @@ static bool processInCommand(void) if (i == PIDLEVEL) { currentProfile->pidProfile.A_level = (float)read8() / 10.0f; currentProfile->pidProfile.H_level = (float)read8() / 10.0f; - currentProfile->pidProfile.H_sensitivity = (float)read8() / 10.0f; + currentProfile->pidProfile.H_sensitivity = read8(); } else { currentProfile->pidProfile.P8[i] = read8(); currentProfile->pidProfile.I8[i] = read8(); diff --git a/src/main/mw.c b/src/main/mw.c index 54defd191e..ef4b04fe94 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -338,6 +338,11 @@ void mwArm(void) } } +int32_t getRcStickPosition(int32_t axis) { + + return min(abs(rcData[axis] - masterConfig.rxConfig.midrc), 500); +} + // Automatic ACC Offset Calibration bool AccInflightCalibrationArmed = false; bool AccInflightCalibrationMeasurementDone = false; diff --git a/src/main/mw.h b/src/main/mw.h index aebee40bee..941efe8b37 100644 --- a/src/main/mw.h +++ b/src/main/mw.h @@ -22,3 +22,5 @@ void handleInflightCalibrationStickPosition(); void mwDisarm(void); void mwArm(void); + +int32_t getRcStickPosition(int32_t axis); \ No newline at end of file