From 9a36b1ef45c1a8bf12d65d0a882326008f68b9a1 Mon Sep 17 00:00:00 2001 From: "Konstantin Sharlaimov (DigitalEntity)" Date: Fri, 18 Mar 2016 14:20:39 +1000 Subject: [PATCH] NAV: Use Throttle expo curve midpoint as hover point for multicopters --- src/main/config/config.c | 2 + src/main/drivers/pwm_rx.c | 1 - src/main/flight/navigation_rewrite.c | 7 ++++ src/main/flight/navigation_rewrite.h | 5 ++- .../flight/navigation_rewrite_multicopter.c | 38 ++++++++++++++++++- src/main/flight/navigation_rewrite_private.h | 2 + src/main/io/rc_curves.c | 4 +- src/main/io/rc_curves.h | 1 + src/main/io/serial_cli.c | 1 + src/main/main.c | 1 + 10 files changed, 57 insertions(+), 5 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index da0b43e827..2f8121b43c 100755 --- a/src/main/config/config.c +++ b/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(¤tProfile->pidProfile); navigationUseRcControlsConfig(¤tProfile->rcControlsConfig); navigationUseRxConfig(&masterConfig.rxConfig); + navigationUseFlight3DConfig(&masterConfig.flight3DConfig); navigationUseEscAndServoConfig(&masterConfig.escAndServoConfig); #endif diff --git a/src/main/drivers/pwm_rx.c b/src/main/drivers/pwm_rx.c index d5e85cf880..a97ea2d706 100644 --- a/src/main/drivers/pwm_rx.c +++ b/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; } diff --git a/src/main/flight/navigation_rewrite.c b/src/main/flight/navigation_rewrite.c index 0265fed5ed..522a9528e2 100755 --- a/src/main/flight/navigation_rewrite.c +++ b/src/main/flight/navigation_rewrite.c @@ -2106,6 +2106,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; @@ -2158,6 +2163,7 @@ void navigationInit(navConfig_t *initialnavConfig, pidProfile_t *initialPidProfile, rcControlsConfig_t *initialRcControlsConfig, rxConfig_t * initialRxConfig, + flight3DConfig_t * initialFlight3DConfig, escAndServoConfig_t * initialEscAndServoConfig) { /* Initial state */ @@ -2189,6 +2195,7 @@ void navigationInit(navConfig_t *initialnavConfig, navigationUseRcControlsConfig(initialRcControlsConfig); navigationUseRxConfig(initialRxConfig); navigationUseEscAndServoConfig(initialEscAndServoConfig); + navigationUseFlight3DConfig(initialFlight3DConfig); } /*----------------------------------------------------------- diff --git a/src/main/flight/navigation_rewrite.h b/src/main/flight/navigation_rewrite.h index 40d6e17553..0a4d02eb22 100755 --- a/src/main/flight/navigation_rewrite.h +++ b/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 */ diff --git a/src/main/flight/navigation_rewrite_multicopter.c b/src/main/flight/navigation_rewrite_multicopter.c index 4095ebfb7b..4e2a1c304b 100755 --- a/src/main/flight/navigation_rewrite_multicopter.c +++ b/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() diff --git a/src/main/flight/navigation_rewrite_private.h b/src/main/flight/navigation_rewrite_private.h index adde07d6a6..b02f49a173 100755 --- a/src/main/flight/navigation_rewrite_private.h +++ b/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; diff --git a/src/main/io/rc_curves.c b/src/main/io/rc_curves.c index 80b0bd4aa7..c4b198332c 100644 --- a/src/main/io/rc_curves.c +++ b/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; diff --git a/src/main/io/rc_curves.h b/src/main/io/rc_curves.h index f79b505992..369fed640d 100644 --- a/src/main/io/rc_curves.h +++ b/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); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 13704b5fd1..111b60a028 100644 --- a/src/main/io/serial_cli.c +++ b/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 }, diff --git a/src/main/main.c b/src/main/main.c index f3380dc10d..5dc2b783bf 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -452,6 +452,7 @@ void init(void) ¤tProfile->pidProfile, ¤tProfile->rcControlsConfig, &masterConfig.rxConfig, + &masterConfig.flight3DConfig, &masterConfig.escAndServoConfig ); #endif