src/main/flight/alt_hold.c

   1 /*
   2  * This file is part of Betaflight.
   3  *
   4  * Betaflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Betaflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Betaflight. If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include "platform.h"
  19 #include "math.h"
  20
  21 #ifdef USE_ALTITUDE_HOLD
  22
  23 #include "build/debug.h"
  24 #include "common/maths.h"
  25 #include "config/config.h"
  26
  27 #include "fc/rc.h"
  28 #include "fc/runtime_config.h"
  29
  30 #include "flight/autopilot.h"
  31 #include "flight/failsafe.h"
  32 #include "flight/position.h"
  33
  34 #include "rx/rx.h"
  35 #include "pg/autopilot.h"
  36
  37 #include "alt_hold.h"
  38
  39 static const float taskIntervalSeconds = HZ_TO_INTERVAL(ALTHOLD_TASK_RATE_HZ); // i.e. 0.01 s
  40
  41 typedef struct {
  42     bool isActive;
  43     float targetAltitudeCm;
  44     float maxVelocity;
  45     float targetVelocity;
  46     float deadband;
  47     bool allowStickAdjustment;
  48 } altHoldState_t;
  49
  50 altHoldState_t altHold;
  51
  52 void altHoldReset(void)
  53 {
  54     resetAltitudeControl();
  55     altHold.targetAltitudeCm = getAltitudeCm();
  56     altHold.targetVelocity = 0.0f;
  57 }
  58
  59 void altHoldInit(void)
  60 {
  61     altHold.isActive = false;
  62     altHold.deadband = altHoldConfig()->alt_hold_deadband / 100.0f;
  63     altHold.allowStickAdjustment = altHoldConfig()->alt_hold_deadband;
  64     altHold.maxVelocity = altHoldConfig()->alt_hold_adjust_rate * 10.0f; // 50 in CLI means 500cm/s
  65     altHoldReset();
  66 }
  67
  68 void altHoldProcessTransitions(void) {
  69
  70     if (FLIGHT_MODE(ALT_HOLD_MODE)) {
  71         if (!altHold.isActive) {
  72             altHoldReset();
  73             altHold.isActive = true;
  74         }
  75     } else {
  76         altHold.isActive = false;
  77     }
  78
  79     // ** the transition out of alt hold (exiting altHold) may be rough.  Some notes... **
  80     // The original PR had a gradual transition from hold throttle to pilot control throttle
  81     // using !(altHoldRequested && altHold.isAltHoldActive) to run an exit function
  82     // a cross-fade factor was sent to mixer.c based on time since the flight mode request was terminated
  83     // it was removed primarily to simplify this PR
  84
  85     // hence in this PR's the user's throttle needs to be close to the hover throttle value on exiting altHold
  86     // its not so bad because the 'target adjustment' by throttle requires that
  87     // user throttle must be not more than half way out from hover for a stable hold
  88 }
  89
  90 void altHoldUpdateTargetAltitude(void)
  91 {
  92     // User can adjust the target altitude with throttle, but only when
  93     // - throttle is outside deadband, and
  94     // - throttle is not low (zero), and
  95     // - deadband is not configured to zero
  96
  97     float stickFactor = 0.0f;
  98
  99     if (altHold.allowStickAdjustment && calculateThrottleStatus() != THROTTLE_LOW) {
 100         const float rcThrottle = rcCommand[THROTTLE];
 101         const float lowThreshold = apConfig()->hover_throttle - altHold.deadband * (apConfig()->hover_throttle - PWM_RANGE_MIN);
 102         const float highThreshold = apConfig()->hover_throttle + altHold.deadband * (PWM_RANGE_MAX - apConfig()->hover_throttle);
 103
 104         if (rcThrottle < lowThreshold) {
 105             stickFactor = scaleRangef(rcThrottle, PWM_RANGE_MIN, lowThreshold, -1.0f, 0.0f);
 106         } else if (rcThrottle > highThreshold) {
 107             stickFactor = scaleRangef(rcThrottle, highThreshold, PWM_RANGE_MAX, 0.0f, 1.0f);
 108         }
 109     }
 110
 111     // if failsafe is active, and we get here, we are in failsafe landing mode, it controls throttle
 112     if (failsafeIsActive()) {
 113         // descend at up to 10 times faster when high
 114         // default landing timeout is now 60s; must to get the quad down within this limit
 115         // need a rapid descent when initiated high, and must slow down closer to ground
 116         // this code doubles descent rate at 20m, to max 10x (10m/s on defaults) at 200m
 117         // the deceleration may be a bit rocky if it starts very high up
 118         // constant (set) deceleration target in the last 2m
 119         stickFactor = -(0.9f + constrainf(getAltitudeCm() / 2000.0f, 0.1f, 9.0f));
 120     }
 121     altHold.targetVelocity = stickFactor * altHold.maxVelocity;
 122
 123     // prevent stick input from moving target altitude too far away from current altitude
 124     // otherwise it can be difficult to bring target altitude close to current altitude in a reasonable time
 125     // using maxVelocity means the stick can bring altitude target to current within 1s
 126     // this constrains the P and I response to user target changes, but not D of F responses
 127     // Range is compared to distance that might be traveled in one second
 128     if (fabsf(getAltitudeCm() - altHold.targetAltitudeCm) < altHold.maxVelocity * 1.0 /* s */) {
 129         altHold.targetAltitudeCm += altHold.targetVelocity * taskIntervalSeconds;
 130     }
 131 }
 132
 133 void altHoldUpdate(void)
 134 {
 135     // check if the user has changed the target altitude using sticks
 136     if (altHoldConfig()->alt_hold_adjust_rate) {
 137         altHoldUpdateTargetAltitude();
 138     }
 139     altitudeControl(altHold.targetAltitudeCm, taskIntervalSeconds, altHold.targetVelocity);
 140 }
 141
 142 void updateAltHold(timeUs_t currentTimeUs) {
 143     UNUSED(currentTimeUs);
 144
 145     // check for enabling Alt Hold, otherwise do as little as possible while inactive
 146     altHoldProcessTransitions();
 147
 148     if (altHold.isActive) {
 149         altHoldUpdate();
 150     }
 151 }
 152
 153 bool isAltHoldActive(void) {
 154     return altHold.isActive;
 155 }
 156 #endif