Merge pull request #75 from treymarc/althold

throtlle angle correction with propper small angle calculation

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 },

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

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;
+        }
+
     }
 }
 

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;
         }
 

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