From 4a000c98ed9baaed2bf69371d4cdfd8e9ab9f6a3 Mon Sep 17 00:00:00 2001 From: treymarc Date: Sat, 12 Apr 2014 03:15:17 +0000 Subject: [PATCH] =?UTF-8?q?throtlle=20angle=20correction=20with=20propper?= =?UTF-8?q?=20small=20angle=20calculation=20:=20correction=20angle=20is=20?= =?UTF-8?q?configurabe=20(from=200.1=C2=B0=20to=2090=C2=B0)=20correction?= =?UTF-8?q?=20value=20var=20renamed=20in=20cli=20correction=20is=20disable?= =?UTF-8?q?=20when=20vertical=20or=20inverted?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/cli.c | 3 ++- src/config.c | 5 +++-- src/imu.c | 17 ++++++++++++++--- src/mw.c | 2 +- src/mw.h | 3 ++- 5 files changed, 22 insertions(+), 8 deletions(-) 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 91637cffc7..7274f654ce 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 b75cca42ed..1527e23715 100755 --- a/src/mw.c +++ b/src/mw.c @@ -886,7 +886,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