diff --git a/src/cli.c b/src/cli.c index 54a50451e0..7f6b87d4f3 100644 --- a/src/cli.c +++ b/src/cli.c @@ -161,7 +161,8 @@ const clivalue_t valueTable[] = { { "yawdeadband", VAR_UINT8, &cfg.yawdeadband, 0, 100 }, { "alt_hold_throttle_neutral", VAR_UINT8, &cfg.alt_hold_throttle_neutral, 1, 250 }, { "alt_hold_fast_change", VAR_UINT8, &cfg.alt_hold_fast_change, 0, 1 }, - { "throttle_angle_correction", VAR_UINT8, &cfg.throttle_angle_correction, 0, 100 }, + { "throttle_correction_value", VAR_UINT8, &cfg.throttle_correction_value, 0, 150 }, + { "throttle_correction_angle", VAR_UINT16, &cfg.throttle_correction_angle, 1, 900 }, { "rc_rate", VAR_UINT8, &cfg.rcRate8, 0, 250 }, { "rc_expo", VAR_UINT8, &cfg.rcExpo8, 0, 100 }, { "thr_mid", VAR_UINT8, &cfg.thrMid8, 0, 100 }, diff --git a/src/config.c b/src/config.c index 2830ddc879..683948ef8e 100755 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,7 @@ master_t mcfg; // master config struct with data independent from profiles config_t cfg; // profile config struct const char rcChannelLetters[] = "AERT1234"; -static const uint8_t EEPROM_CONF_VERSION = 61; +static const uint8_t EEPROM_CONF_VERSION = 62; static uint32_t enabledSensors = 0; static void resetConf(void); @@ -279,7 +279,8 @@ static void resetConf(void) cfg.yawdeadband = 0; cfg.alt_hold_throttle_neutral = 40; cfg.alt_hold_fast_change = 1; - cfg.throttle_angle_correction = 0; // could be 40 + cfg.throttle_correction_value = 0; // could be 40 + cfg.throttle_correction_angle = 300; // 30.0 deg , could be 225 // Failsafe Variables cfg.failsafe_delay = 10; // 1sec diff --git a/src/imu.c b/src/imu.c index d4eb62a2f9..c4ead3384d 100644 --- a/src/imu.c +++ b/src/imu.c @@ -301,9 +301,20 @@ static void getEstimatedAttitude(void) acc_calc(deltaT); // rotate acc vector into earth frame - if (cfg.throttle_angle_correction) { - int cosZ = ((int32_t)(EstG.V.Z * 100.0f)) / acc_1G; - throttleAngleCorrection = cfg.throttle_angle_correction * constrain(100 - cosZ, 0, 100) / 8; + if (cfg.throttle_correction_value) { + + float cosZ = EstG.V.Z / sqrtf(EstG.V.X * EstG.V.X + EstG.V.Y * EstG.V.Y + EstG.V.Z * EstG.V.Z); + + if (cosZ <= 0) { + throttleAngleCorrection = 0; // we are inverted or vertical , no correction + } else { + int coef = acosf(cosZ) * (1800.0f / M_PI) * (900.0f / cfg.throttle_correction_angle); + // we could replace the float div with hardcode uint8 value (ex 4 = 22.5 deg, 3 = 30 deg, 2 = 45 , up to the cli) + if (coef > 900) + coef = 900; + throttleAngleCorrection = (cfg.throttle_correction_value * coef) / 900; + } + } } diff --git a/src/mw.c b/src/mw.c index a5b6953960..e668ee83df 100755 --- a/src/mw.c +++ b/src/mw.c @@ -884,7 +884,7 @@ void loop(void) } #endif - if (cfg.throttle_angle_correction && (f.ANGLE_MODE || f.HORIZON_MODE)) { + if (cfg.throttle_correction_value && (f.ANGLE_MODE || f.HORIZON_MODE)) { rcCommand[THROTTLE] += throttleAngleCorrection; } diff --git a/src/mw.h b/src/mw.h index a3c5afd741..473a38c6c6 100755 --- a/src/mw.h +++ b/src/mw.h @@ -186,7 +186,8 @@ typedef struct config_t { uint8_t yawdeadband; // introduce a deadband around the stick center for yaw axis. Must be greater than zero. uint8_t alt_hold_throttle_neutral; // defines the neutral zone of throttle stick during altitude hold, default setting is +/-40 uint8_t alt_hold_fast_change; // when disabled, turn off the althold when throttle stick is out of deadband defined with alt_hold_throttle_neutral; when enabled, altitude changes slowly proportional to stick movement - uint8_t throttle_angle_correction; // + uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg + uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. // Servo-related stuff servoParam_t servoConf[8]; // servo configuration