src/main/navigation/navigation_fw_launch.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 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <math.h>
  21 #include <string.h>
  22
  23 #include "platform.h"
  24
  25 #if defined(USE_NAV)
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "common/axis.h"
  31 #include "common/maths.h"
  32
  33 #include "config/feature.h"
  34
  35 #include "drivers/time.h"
  36 #include "drivers/sensor.h"
  37 #include "drivers/accgyro/accgyro.h"
  38
  39 #include "sensors/sensors.h"
  40 #include "sensors/rangefinder.h"
  41 #include "sensors/barometer.h"
  42 #include "sensors/acceleration.h"
  43 #include "sensors/boardalignment.h"
  44 #include "sensors/compass.h"
  45
  46 #include "io/gps.h"
  47 #include "io/beeper.h"
  48
  49 #include "flight/pid.h"
  50 #include "flight/imu.h"
  51 #include "flight/mixer.h"
  52
  53 #include "fc/config.h"
  54 #include "fc/rc_controls.h"
  55 #include "fc/runtime_config.h"
  56
  57 #include "navigation/navigation.h"
  58 #include "navigation/navigation_private.h"
  59
  60
  61 typedef struct FixedWingLaunchState_s {
  62     /* Launch detection */
  63     timeUs_t launchDetectorPreviousUpdate;
  64     timeUs_t launchDetectionTimeAccum;
  65     bool launchDetected;
  66
  67     /* Launch progress */
  68     timeUs_t launchStartedTime;
  69     bool launchFinished;
  70     bool motorControlAllowed;
  71 } FixedWingLaunchState_t;
  72
  73 static EXTENDED_FASTRAM FixedWingLaunchState_t   launchState;
  74
  75 #define COS_MAX_LAUNCH_ANGLE                0.70710678f                 // cos(45), just to be safe
  76 #define SWING_LAUNCH_MIN_ROTATION_RATE      DEGREES_TO_RADIANS(100)     // expect minimum 100dps rotation rate
  77 static void updateFixedWingLaunchDetector(timeUs_t currentTimeUs)
  78 {
  79     const float swingVelocity = (fabsf(imuMeasuredRotationBF.z) > SWING_LAUNCH_MIN_ROTATION_RATE) ? (imuMeasuredAccelBF.y / imuMeasuredRotationBF.z) : 0;
  80     const bool isForwardAccelerationHigh = (imuMeasuredAccelBF.x > navConfig()->fw.launch_accel_thresh);
  81     const bool isAircraftAlmostLevel = (calculateCosTiltAngle() >= cos_approx(DEGREES_TO_RADIANS(navConfig()->fw.launch_max_angle)));
  82
  83     const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel;
  84     const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0);
  85
  86     if (isBungeeLaunched || isSwingLaunched) {
  87         launchState.launchDetectionTimeAccum += (currentTimeUs - launchState.launchDetectorPreviousUpdate);
  88         launchState.launchDetectorPreviousUpdate = currentTimeUs;
  89         if (launchState.launchDetectionTimeAccum >= MS2US((uint32_t)navConfig()->fw.launch_time_thresh)) {
  90             launchState.launchDetected = true;
  91         }
  92     }
  93     else {
  94         launchState.launchDetectorPreviousUpdate = currentTimeUs;
  95         launchState.launchDetectionTimeAccum = 0;
  96     }
  97 }
  98
  99 void resetFixedWingLaunchController(timeUs_t currentTimeUs)
 100 {
 101     launchState.launchDetectorPreviousUpdate = currentTimeUs;
 102     launchState.launchDetectionTimeAccum = 0;
 103     launchState.launchStartedTime = 0;
 104     launchState.launchDetected = false;
 105     launchState.launchFinished = false;
 106     launchState.motorControlAllowed = false;
 107 }
 108
 109 bool isFixedWingLaunchDetected(void)
 110 {
 111     return launchState.launchDetected;
 112 }
 113
 114 void enableFixedWingLaunchController(timeUs_t currentTimeUs)
 115 {
 116     launchState.launchStartedTime = currentTimeUs;
 117     launchState.motorControlAllowed = true;
 118 }
 119
 120 bool isFixedWingLaunchFinishedOrAborted(void)
 121 {
 122     return launchState.launchFinished;
 123 }
 124
 125 void abortFixedWingLaunch(void)
 126 {
 127     launchState.launchFinished = true;
 128 }
 129
 130 #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 //ms
 131
 132 static void applyFixedWingLaunchIdleLogic(void)
 133 {
 134     // Until motors are started don't use PID I-term
 135     pidResetErrorAccumulators();
 136
 137     // We're not flying yet, reset TPA filter
 138     pidResetTPAFilter();
 139
 140     // Throttle control logic
 141     if (navConfig()->fw.launch_idle_throttle <= getThrottleIdleValue())
 142     {
 143         ENABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);             // If MOTOR_STOP is enabled mixer will keep motor stopped
 144         rcCommand[THROTTLE] = getThrottleIdleValue(); // If MOTOR_STOP is disabled, motors will spin at minthrottle
 145     }
 146     else
 147     {
 148         static float timeThrottleRaisedMs;
 149         if (calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC) == THROTTLE_LOW)
 150         {
 151             timeThrottleRaisedMs = millis();
 152         }
 153         else
 154         {
 155             const float timeSinceMotorStartMs = MIN(millis() - timeThrottleRaisedMs, LAUNCH_MOTOR_IDLE_SPINUP_TIME);
 156             rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
 157                                                 0.0f, LAUNCH_MOTOR_IDLE_SPINUP_TIME,
 158                                                 getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
 159         }
 160     }
 161 }
 162
 163 static inline bool isFixedWingLaunchMaxAltitudeReached(void)
 164 {
 165     return (navConfig()->fw.launch_max_altitude > 0) && (getEstimatedActualPosition(Z) >= navConfig()->fw.launch_max_altitude);
 166 }
 167 static inline bool isLaunchModeMinTimeElapsed(float timeSinceLaunchMs)
 168 {
 169     return timeSinceLaunchMs > navConfig()->fw.launch_min_time;
 170 }
 171
 172 static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs)
 173 {
 174     return timeSinceLaunchMs >= navConfig()->fw.launch_timeout;
 175 }
 176
 177 static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs)
 178 {
 179     return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached();
 180 }
 181
 182 void applyFixedWingLaunchController(timeUs_t currentTimeUs)
 183 {
 184     // Called at PID rate
 185
 186     if (launchState.launchDetected) {
 187         bool applyLaunchIdleLogic = true;
 188
 189         if (launchState.motorControlAllowed) {
 190             // If launch detected we are in launch procedure - control airplane
 191             const float timeElapsedSinceLaunchMs = US2MS(currentTimeUs - launchState.launchStartedTime);
 192
 193             launchState.launchFinished = isFixedWingLaunchCompleted(timeElapsedSinceLaunchMs);
 194
 195             // Motor control enabled
 196             if (timeElapsedSinceLaunchMs >= navConfig()->fw.launch_motor_timer) {
 197                 // Don't apply idle logic anymore
 198                 applyLaunchIdleLogic = false;
 199
 200                 // Increase throttle gradually over `launch_motor_spinup_time` milliseconds
 201                 if (navConfig()->fw.launch_motor_spinup_time > 0) {
 202                     const float timeSinceMotorStartMs = constrainf(timeElapsedSinceLaunchMs - navConfig()->fw.launch_motor_timer, 0.0f, navConfig()->fw.launch_motor_spinup_time);
 203                     const uint16_t minIdleThrottle = MAX(getThrottleIdleValue(), navConfig()->fw.launch_idle_throttle);
 204                     rcCommand[THROTTLE] = scaleRangef(timeSinceMotorStartMs,
 205                                                       0.0f, navConfig()->fw.launch_motor_spinup_time,
 206                                                       minIdleThrottle, navConfig()->fw.launch_throttle);
 207                 }
 208                 else {
 209                     rcCommand[THROTTLE] = navConfig()->fw.launch_throttle;
 210                 }
 211             }
 212         }
 213
 214         if (applyLaunchIdleLogic) {
 215             // Launch idle logic - low throttle and zero out PIDs
 216             applyFixedWingLaunchIdleLogic();
 217         }
 218     }
 219     else {
 220         // We are waiting for launch - update launch detector
 221         updateFixedWingLaunchDetector(currentTimeUs);
 222
 223         // Launch idle logic - low throttle and zero out PIDs
 224         applyFixedWingLaunchIdleLogic();
 225     }
 226
 227     // Control beeper
 228     if (!launchState.launchFinished) {
 229         beeper(BEEPER_LAUNCH_MODE_ENABLED);
 230     }
 231
 232     // Lock out controls
 233     rcCommand[ROLL] = 0;
 234     rcCommand[PITCH] = pidAngleToRcCommand(-DEGREES_TO_DECIDEGREES(navConfig()->fw.launch_climb_angle), pidProfile()->max_angle_inclination[FD_PITCH]);
 235     rcCommand[YAW] = 0;
 236 }
 237
 238 #endif