Merge pull request #117 from iNavFlight/thr-mid-hover

NAV: Make altitude hold aware of throttle expo curve midpoint. Closes #105

src/main/config/config.c

@@ -216,6 +216,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
 void resetNavConfig(navConfig_t * navConfig)
 {
     // Navigation flags
+    navConfig->flags.use_thr_mid_for_althold = 1;
     navConfig->flags.extra_arming_safety = 1;
     navConfig->flags.user_control_mode = NAV_GPS_ATTI;
     navConfig->flags.rth_alt_control_style = NAV_RTH_AT_LEAST_ALT;
@@ -802,6 +803,7 @@ void activateConfig(void)
     navigationUsePIDs(&currentProfile->pidProfile);
     navigationUseRcControlsConfig(&currentProfile->rcControlsConfig);
     navigationUseRxConfig(&masterConfig.rxConfig);
+    navigationUseFlight3DConfig(&masterConfig.flight3DConfig);
     navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig);
 #endif
 

src/main/drivers/pwm_rx.c

@@ -252,7 +252,6 @@ static void ppmEdgeCallback(timerCCHandlerRec_t* cbRec, captureCompare_t capture
                 ppmDev.numChannels = ppmDev.pulseIndex;
             }
         } else {
-            debug[2]++;
             ppmDev.stableFramesSeenCount = 0;
         }
 

src/main/flight/navigation_rewrite.c

@@ -2118,6 +2118,11 @@ void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig)
     posControl.rcControlsConfig = initialRcControlsConfig;
 }
 
+void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig)
+{
+    posControl.flight3DConfig = initialFlight3DConfig;
+}
+
 void navigationUseRxConfig(rxConfig_t * initialRxConfig)
 {
     posControl.rxConfig = initialRxConfig;
@@ -2170,6 +2175,7 @@ void navigationInit(navConfig_t *initialnavConfig,
                     pidProfile_t *initialPidProfile,
                     rcControlsConfig_t *initialRcControlsConfig,
                     rxConfig_t * initialRxConfig,
+                    flight3DConfig_t * initialFlight3DConfig,
                     escAndServoConfig_t * initialEscAndServoConfig)
 {
     /* Initial state */
@@ -2201,6 +2207,7 @@ void navigationInit(navConfig_t *initialnavConfig,
     navigationUseRcControlsConfig(initialRcControlsConfig);
     navigationUseRxConfig(initialRxConfig);
     navigationUseEscAndServoConfig(initialEscAndServoConfig);
+    navigationUseFlight3DConfig(initialFlight3DConfig);
 }
 
 /*-----------------------------------------------------------

src/main/flight/navigation_rewrite.h

@@ -26,6 +26,7 @@
 
 #include "flight/pid.h"
 #include "flight/failsafe.h"
+#include "flight/mixer.h"
 
 /* GPS Home location data */
 extern gpsLocation_t        GPS_home;
@@ -68,7 +69,7 @@ enum {
 
 typedef struct navConfig_s {
     struct {
-        uint8_t __stub;                     // Don't remember throttle when althold was initiated, assume that throttle is at middle = zero climb rate
+        uint8_t use_thr_mid_for_althold;    // Don't remember throttle when althold was initiated, assume that throttle is at Thr Mid = zero climb rate
         uint8_t extra_arming_safety;        // Forcibly apply 100% throttle tilt compensation
         uint8_t user_control_mode;          // NAV_GPS_ATTI or NAV_GPS_CRUISE
         uint8_t rth_alt_control_style;      // Controls how RTH controls altitude
@@ -218,10 +219,12 @@ void navigationUseConfig(navConfig_t *navConfigToUse);
 void navigationUseRcControlsConfig(rcControlsConfig_t *initialRcControlsConfig);
 void navigationUseRxConfig(rxConfig_t * initialRxConfig);
 void navigationUseEscAndServoConfig(escAndServoConfig_t * initialEscAndServoConfig);
+void navigationUseFlight3DConfig(flight3DConfig_t * initialFlight3DConfig);
 void navigationInit(navConfig_t *initialnavConfig,
                     pidProfile_t *initialPidProfile,
                     rcControlsConfig_t *initialRcControlsConfig,
                     rxConfig_t * initialRxConfig,
+                    flight3DConfig_t * initialFlight3DConfig,
                     escAndServoConfig_t * initialEscAndServoConfig);
 
 /* Navigation system updates */

src/main/flight/navigation_rewrite_multicopter.c

@@ -35,6 +35,10 @@
 #include "sensors/acceleration.h"
 #include "sensors/boardalignment.h"
 
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+#include "io/rc_curves.h"
+
 #include "flight/pid.h"
 #include "flight/imu.h"
 #include "flight/navigation_rewrite.h"
@@ -55,6 +59,7 @@ extern int16_t magHold;
  * Altitude controller for multicopter aircraft
  *-----------------------------------------------------------*/
 static int16_t rcCommandAdjustedThrottle;
+static int16_t altHoldThrottleRCZero = 1500;
 static filterStatePt1_t altholdThrottleFilterState;
 
 /* Calculate global altitude setpoint based on surface setpoint */
@@ -99,10 +104,21 @@ static void updateAltitudeThrottleController_MC(uint32_t deltaMicros)
 
 bool adjustMulticopterAltitudeFromRCInput(void)
 {
-    int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - posControl.rxConfig->midrc;
+    int16_t rcThrottleAdjustment = rcCommand[THROTTLE] - altHoldThrottleRCZero;
     if (ABS(rcThrottleAdjustment) > posControl.rcControlsConfig->alt_hold_deadband) {
         // set velocity proportional to stick movement
-        float rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / 500;
+        float rcClimbRate;
+
+        // Make sure we can satisfy max_manual_climb_rate in both up and down directions
+        if (rcThrottleAdjustment > 0) {
+            // Scaling from altHoldThrottleRCZero to maxthrottle
+            rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (posControl.escAndServoConfig->maxthrottle - altHoldThrottleRCZero);
+        }
+        else {
+            // Scaling from minthrottle to altHoldThrottleRCZero
+            rcClimbRate = rcThrottleAdjustment * posControl.navConfig->max_manual_climb_rate / (altHoldThrottleRCZero - posControl.escAndServoConfig->minthrottle);
+        }
+
         updateAltitudeTargetFromClimbRate(rcClimbRate);
 
         return true;
@@ -120,6 +136,24 @@ bool adjustMulticopterAltitudeFromRCInput(void)
 void setupMulticopterAltitudeController(void)
 {
     // Nothing here
+    if (posControl.navConfig->flags.use_thr_mid_for_althold) {
+        altHoldThrottleRCZero = lookupThrottleRCMid;
+    }
+    else {
+        // If throttle status is THROTTLE_LOW - use Thr Mid anyway
+        throttleStatus_e throttleStatus = calculateThrottleStatus(posControl.rxConfig, posControl.flight3DConfig->deadband3d_throttle);
+        if (throttleStatus == THROTTLE_LOW) {
+            altHoldThrottleRCZero = lookupThrottleRCMid;
+        }
+        else {
+            altHoldThrottleRCZero = rcCommand[THROTTLE];
+        }
+    }
+
+    // Make sure we are able to satisfy the deadband
+    altHoldThrottleRCZero = constrain(altHoldThrottleRCZero,
+                                      posControl.escAndServoConfig->minthrottle + posControl.rcControlsConfig->alt_hold_deadband + 10, 
+                                      posControl.escAndServoConfig->maxthrottle - posControl.rcControlsConfig->alt_hold_deadband - 10);
 }
 
 void resetMulticopterAltitudeController()

src/main/flight/navigation_rewrite_private.h

@@ -62,6 +62,7 @@ typedef struct navigationFlags_s {
 
     // Behaviour modifiers
     bool isGCSAssistedNavigationEnabled;    // Does iNav accept WP#255 - follow-me etc.
+    //bool isTerrainFollowEnabled;            // Does iNav use sonar for terrain following (adjusting baro altitude target according to sonar readings)
 
     bool forcedRTHActivated;
 } navigationFlags_t;
@@ -259,6 +260,7 @@ typedef struct {
     rcControlsConfig_t *        rcControlsConfig;
     pidProfile_t *              pidProfile;
     rxConfig_t *                rxConfig;
+    flight3DConfig_t *          flight3DConfig;
     escAndServoConfig_t *       escAndServoConfig;
 } navigationPosControl_t;
 

src/main/io/rc_curves.c

@@ -27,7 +27,7 @@
 int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH];     // lookup table for expo & RC rate PITCH+ROLL
 int16_t lookupYawRC[YAW_LOOKUP_LENGTH];     // lookup table for expo & RC rate YAW
 int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];   // lookup table for expo & mid THROTTLE
-
+int16_t lookupThrottleRCMid;                        // THROTTLE curve mid point
 
 void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
 {
@@ -49,6 +49,8 @@ void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoCo
 {
     uint8_t i;
 
+    lookupThrottleRCMid = escAndServoConfig->minthrottle + (int32_t)(escAndServoConfig->maxthrottle - escAndServoConfig->minthrottle) * controlRateConfig->thrMid8 / 100; // [MINTHROTTLE;MAXTHROTTLE]
+
     for (i = 0; i < THROTTLE_LOOKUP_LENGTH; i++) {
         int16_t tmp = 10 * i - controlRateConfig->thrMid8;
         uint8_t y = 1;

src/main/io/rc_curves.h

@@ -23,6 +23,7 @@
 extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH];   // lookup table for expo & RC rate PITCH+ROLL
 extern int16_t lookupYawRC[YAW_LOOKUP_LENGTH];     // lookup table for expo & RC rate YAW
 extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH];   // lookup table for expo & mid THROTTLE
+extern int16_t lookupThrottleRCMid;                         // THROTTLE curve mid
 
 void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
 void generateYawCurve(controlRateConfig_t *controlRateConfig);

src/main/io/serial_cli.c

@@ -586,6 +586,7 @@ const clivalue_t valueTable[] = {
     { "inav_max_eph_epv",           VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.max_eph_epv, .config.minmax = { 0,  9999 }, 0 },
     { "inav_baro_epv",              VAR_FLOAT  | MASTER_VALUE, &masterConfig.navConfig.inav.baro_epv, .config.minmax = { 0,  9999 }, 0 },
 
+    { "nav_use_midthr_for_althold", VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.use_thr_mid_for_althold, .config.lookup = { TABLE_OFF_ON }, 0 },
     { "nav_extra_arming_safety",    VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.extra_arming_safety, .config.lookup = { TABLE_OFF_ON }, 0 },
     { "nav_user_control_mode",      VAR_UINT8  | MASTER_VALUE | MODE_LOOKUP, &masterConfig.navConfig.flags.user_control_mode, .config.lookup = { TABLE_NAV_USER_CTL_MODE }, 0 },
     { "nav_position_timeout",       VAR_UINT8  | MASTER_VALUE, &masterConfig.navConfig.pos_failure_timeout, .config.minmax = { 0,  10 }, 0 },

src/main/main.c

@@ -452,6 +452,7 @@ void init(void)
             &currentProfile->pidProfile,
             &currentProfile->rcControlsConfig,
             &masterConfig.rxConfig,
+            &masterConfig.flight3DConfig,
             &masterConfig.escAndServoConfig
         );
 #endif