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