src/main/flight/altitudehold.c

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight 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  * Cleanflight 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 Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18
  19 #include <stdbool.h>
  20 #include <stdint.h>
  21 #include <stdlib.h>
  22 #include <math.h>
  23
  24
  25 #include "platform.h"
  26 #include "debug.h"
  27
  28 #include "common/maths.h"
  29 #include "common/axis.h"
  30
  31 #include "drivers/sensor.h"
  32 #include "drivers/accgyro.h"
  33
  34 #include "sensors/sensors.h"
  35 #include "sensors/acceleration.h"
  36 #include "sensors/barometer.h"
  37 #include "sensors/sonar.h"
  38
  39 #include "rx/rx.h"
  40
  41 #include "io/rc_controls.h"
  42 #include "io/escservo.h"
  43
  44 #include "flight/mixer.h"
  45 #include "flight/pid.h"
  46 #include "flight/imu.h"
  47
  48 #include "config/runtime_config.h"
  49
  50 int32_t setVelocity = 0;
  51 uint8_t velocityControl = 0;
  52 int32_t errorVelocityI = 0;
  53 int32_t altHoldThrottleAdjustment = 0;
  54 int32_t AltHold;
  55 int32_t vario = 0;                      // variometer in cm/s
  56
  57
  58 static barometerConfig_t *barometerConfig;
  59 static pidProfile_t *pidProfile;
  60 static rcControlsConfig_t *rcControlsConfig;
  61 static escAndServoConfig_t *escAndServoConfig;
  62
  63 void configureAltitudeHold(
  64         pidProfile_t *initialPidProfile,
  65         barometerConfig_t *intialBarometerConfig,
  66         rcControlsConfig_t *initialRcControlsConfig,
  67         escAndServoConfig_t *initialEscAndServoConfig
  68 )
  69 {
  70     pidProfile = initialPidProfile;
  71     barometerConfig = intialBarometerConfig;
  72     rcControlsConfig = initialRcControlsConfig;
  73     escAndServoConfig = initialEscAndServoConfig;
  74 }
  75
  76 #if defined(BARO) || defined(SONAR)
  77
  78 static int16_t initialThrottleHold;
  79 static int32_t EstAlt;                // in cm
  80
  81 // 40hz update rate (20hz LPF on acc)
  82 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
  83
  84 #define DEGREES_80_IN_DECIDEGREES 800
  85
  86 static void applyMultirotorAltHold(void)
  87 {
  88     static uint8_t isAltHoldChanged = 0;
  89     // multirotor alt hold
  90     if (rcControlsConfig->alt_hold_fast_change) {
  91         // rapid alt changes
  92         if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
  93             errorVelocityI = 0;
  94             isAltHoldChanged = 1;
  95             rcCommand[THROTTLE] += (rcData[THROTTLE] > initialThrottleHold) ? -rcControlsConfig->alt_hold_deadband : rcControlsConfig->alt_hold_deadband;
  96         } else {
  97             if (isAltHoldChanged) {
  98                 AltHold = EstAlt;
  99                 isAltHoldChanged = 0;
 100             }
 101             rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
 102         }
 103     } else {
 104         // slow alt changes, mostly used for aerial photography
 105         if (ABS(rcData[THROTTLE] - initialThrottleHold) > rcControlsConfig->alt_hold_deadband) {
 106             // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
 107             setVelocity = (rcData[THROTTLE] - initialThrottleHold) / 2;
 108             velocityControl = 1;
 109             isAltHoldChanged = 1;
 110         } else if (isAltHoldChanged) {
 111             AltHold = EstAlt;
 112             velocityControl = 0;
 113             isAltHoldChanged = 0;
 114         }
 115         rcCommand[THROTTLE] = constrain(initialThrottleHold + altHoldThrottleAdjustment, escAndServoConfig->minthrottle, escAndServoConfig->maxthrottle);
 116     }
 117 }
 118
 119 static void applyFixedWingAltHold(airplaneConfig_t *airplaneConfig)
 120 {
 121     // handle fixedwing-related althold. UNTESTED! and probably wrong
 122     // most likely need to check changes on pitch channel and 'reset' althold similar to
 123     // how throttle does it on multirotor
 124
 125     rcCommand[PITCH] += altHoldThrottleAdjustment * airplaneConfig->fixedwing_althold_dir;
 126 }
 127
 128 void applyAltHold(airplaneConfig_t *airplaneConfig)
 129 {
 130     if (STATE(FIXED_WING)) {
 131         applyFixedWingAltHold(airplaneConfig);
 132     } else {
 133         applyMultirotorAltHold();
 134     }
 135 }
 136
 137 void updateAltHoldState(void)
 138 {
 139     // Baro alt hold activate
 140     if (!IS_RC_MODE_ACTIVE(BOXBARO)) {
 141         DISABLE_FLIGHT_MODE(BARO_MODE);
 142         return;
 143     }
 144
 145     if (!FLIGHT_MODE(BARO_MODE)) {
 146         ENABLE_FLIGHT_MODE(BARO_MODE);
 147         AltHold = EstAlt;
 148         initialThrottleHold = rcData[THROTTLE];
 149         errorVelocityI = 0;
 150         altHoldThrottleAdjustment = 0;
 151     }
 152 }
 153
 154 void updateSonarAltHoldState(void)
 155 {
 156     // Sonar alt hold activate
 157     if (!IS_RC_MODE_ACTIVE(BOXSONAR)) {
 158         DISABLE_FLIGHT_MODE(SONAR_MODE);
 159         return;
 160     }
 161
 162     if (!FLIGHT_MODE(SONAR_MODE)) {
 163         ENABLE_FLIGHT_MODE(SONAR_MODE);
 164         AltHold = EstAlt;
 165         initialThrottleHold = rcData[THROTTLE];
 166         errorVelocityI = 0;
 167         altHoldThrottleAdjustment = 0;
 168     }
 169 }
 170
 171 bool isThrustFacingDownwards(attitudeEulerAngles_t *attitude)
 172 {
 173     return ABS(attitude->values.roll) < DEGREES_80_IN_DECIDEGREES && ABS(attitude->values.pitch) < DEGREES_80_IN_DECIDEGREES;
 174 }
 175
 176 /*
 177 * This (poorly named) function merely returns whichever is higher, roll inclination or pitch inclination.
 178 * //TODO: Fix this up. We could either actually return the angle between 'down' and the normal of the craft
 179 * (my best interpretation of scalar 'tiltAngle') or rename the function.
 180 */
 181 int16_t calculateTiltAngle(attitudeEulerAngles_t *attitude)
 182 {
 183     return MAX(ABS(attitude->values.roll), ABS(attitude->values.pitch));
 184 }
 185
 186 int32_t calculateAltHoldThrottleAdjustment(int32_t vel_tmp, float accZ_tmp, float accZ_old)
 187 {
 188     int32_t result = 0;
 189     int32_t error;
 190     int32_t setVel;
 191
 192     if (!isThrustFacingDownwards(&attitude)) {
 193         return result;
 194     }
 195
 196     // Altitude P-Controller
 197
 198     if (!velocityControl) {
 199         error = constrain(AltHold - EstAlt, -500, 500);
 200         error = applyDeadband(error, 10); // remove small P parameter to reduce noise near zero position
 201         setVel = constrain((pidProfile->P8[PIDALT] * error / 128), -300, +300); // limit velocity to +/- 3 m/s
 202     } else {
 203         setVel = setVelocity;
 204     }
 205     // Velocity PID-Controller
 206
 207     // P
 208     error = setVel - vel_tmp;
 209     result = constrain((pidProfile->P8[PIDVEL] * error / 32), -300, +300);
 210
 211     // I
 212     errorVelocityI += (pidProfile->I8[PIDVEL] * error);
 213     errorVelocityI = constrain(errorVelocityI, -(8192 * 200), (8192 * 200));
 214     result += errorVelocityI / 8192;     // I in range +/-200
 215
 216     // D
 217     result -= constrain(pidProfile->D8[PIDVEL] * (accZ_tmp + accZ_old) / 512, -150, 150);
 218
 219     return result;
 220 }
 221
 222 void calculateEstimatedAltitude(uint32_t currentTime)
 223 {
 224     static uint32_t previousTime;
 225     uint32_t dTime;
 226     int32_t baroVel;
 227     float dt;
 228     float vel_acc;
 229     int32_t vel_tmp;
 230     float accZ_tmp;
 231     int32_t sonarAlt = -1;
 232     static float accZ_old = 0.0f;
 233     static float vel = 0.0f;
 234     static float accAlt = 0.0f;
 235     static int32_t lastBaroAlt;
 236
 237     static int32_t baroAlt_offset = 0;
 238     float sonarTransition;
 239
 240 #ifdef SONAR
 241     int16_t tiltAngle;
 242 #endif
 243
 244     dTime = currentTime - previousTime;
 245     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
 246         return;
 247
 248     previousTime = currentTime;
 249
 250 #ifdef BARO
 251     if (!isBaroCalibrationComplete()) {
 252         performBaroCalibrationCycle();
 253         vel = 0;
 254         accAlt = 0;
 255     }
 256
 257     BaroAlt = baroCalculateAltitude();
 258 #else
 259     BaroAlt = 0;
 260 #endif
 261
 262 #ifdef SONAR
 263     tiltAngle = calculateTiltAngle(&attitude);
 264     sonarAlt = sonarRead();
 265     sonarAlt = sonarCalculateAltitude(sonarAlt, tiltAngle);
 266 #endif
 267
 268     if (sonarAlt > 0 && sonarAlt < 200) {
 269         baroAlt_offset = BaroAlt - sonarAlt;
 270         BaroAlt = sonarAlt;
 271     } else {
 272         BaroAlt -= baroAlt_offset;
 273         if (sonarAlt > 0  && sonarAlt <= 300) {
 274             sonarTransition = (300 - sonarAlt) / 100.0f;
 275             BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
 276         }
 277     }
 278
 279     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 280
 281     // Integrator - velocity, cm/sec
 282     if (accSumCount) {
 283         accZ_tmp = (float)accSum[2] / (float)accSumCount;
 284     } else {
 285         accZ_tmp = 0;
 286     }
 287     vel_acc = accZ_tmp * accVelScale * (float)accTimeSum;
 288
 289     // Integrator - Altitude in cm
 290     accAlt += (vel_acc * 0.5f) * dt + vel * dt;                                                                 // integrate velocity to get distance (x= a/2 * t^2)
 291     accAlt = accAlt * barometerConfig->baro_cf_alt + (float)BaroAlt * (1.0f - barometerConfig->baro_cf_alt);    // complementary filter for altitude estimation (baro & acc)
 292     vel += vel_acc;
 293
 294 #ifdef DEBUG_ALT_HOLD
 295     debug[1] = accSum[2] / accSumCount; // acceleration
 296     debug[2] = vel;                     // velocity
 297     debug[3] = accAlt;                  // height
 298 #endif
 299
 300     imuResetAccelerationSum();
 301
 302 #ifdef BARO
 303     if (!isBaroCalibrationComplete()) {
 304         return;
 305     }
 306 #endif
 307
 308     if (sonarAlt > 0 && sonarAlt < 200) {
 309         // the sonar has the best range
 310         EstAlt = BaroAlt;
 311     } else {
 312         EstAlt = accAlt;
 313     }
 314
 315     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
 316     lastBaroAlt = BaroAlt;
 317
 318     baroVel = constrain(baroVel, -1500, 1500);  // constrain baro velocity +/- 1500cm/s
 319     baroVel = applyDeadband(baroVel, 10);       // to reduce noise near zero
 320
 321     // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
 322     // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
 323     vel = vel * barometerConfig->baro_cf_vel + baroVel * (1.0f - barometerConfig->baro_cf_vel);
 324     vel_tmp = lrintf(vel);
 325
 326     // set vario
 327     vario = applyDeadband(vel_tmp, 5);
 328
 329     altHoldThrottleAdjustment = calculateAltHoldThrottleAdjustment(vel_tmp, accZ_tmp, accZ_old);
 330
 331     accZ_old = accZ_tmp;
 332 }
 333
 334 int32_t altitudeHoldGetEstimatedAltitude(void)
 335 {
 336     return EstAlt;
 337 }
 338
 339 #endif
 340