src/main/navigation/navigation_rover_boat.c

   1 /*
   2  * This file is part of INAV Project.
   3  *
   4  * This Source Code Form is subject to the terms of the Mozilla Public
   5  * License, v. 2.0. If a copy of the MPL was not distributed with this file,
   6  * You can obtain one at http://mozilla.org/MPL/2.0/.
   7  *
   8  * Alternatively, the contents of this file may be used under the terms
   9  * of the GNU General Public License Version 3, as described below:
  10  *
  11  * This file is free software: you may copy, redistribute and/or modify
  12  * it under the terms of the GNU General Public License as published by the
  13  * Free Software Foundation, either version 3 of the License, or (at your
  14  * option) any later version.
  15  *
  16  * This file is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
  19  * Public License for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License
  22  * along with this program. If not, see http://www.gnu.org/licenses/.
  23  */
  24
  25 #include "platform.h"
  26
  27 FILE_COMPILE_FOR_SIZE
  28
  29 #ifdef USE_NAV
  30
  31 #include "build/debug.h"
  32 #include "common/utils.h"
  33 #include "fc/rc_controls.h"
  34 #include "fc/config.h"
  35 #include "flight/mixer.h"
  36 #include "navigation/navigation.h"
  37 #include "navigation/navigation_private.h"
  38
  39 static bool isYawAdjustmentValid = false;
  40 static int32_t navHeadingError;
  41
  42 static void update2DPositionHeadingController(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
  43 {
  44     static timeUs_t previousTimeMonitoringUpdate;
  45     static int32_t previousHeadingError;
  46     static bool errorIsDecreasing;
  47
  48     int32_t targetBearing = calculateBearingToDestination(&posControl.desiredState.pos);
  49
  50     /*
  51      * Calculate NAV heading error
  52      * Units are centidegrees
  53      */
  54     navHeadingError = wrap_18000(targetBearing - posControl.actualState.yaw);
  55
  56     // Slow error monitoring (2Hz rate)
  57     if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
  58         // Check if error is decreasing over time
  59         errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
  60
  61         // Save values for next iteration
  62         previousHeadingError = navHeadingError;
  63         previousTimeMonitoringUpdate = currentTimeUs;
  64     }
  65
  66     posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
  67 }
  68
  69 void applyRoverBoatPositionController(timeUs_t currentTimeUs)
  70 {
  71     static timeUs_t previousTimePositionUpdate;         // Occurs @ GPS update rate
  72     static timeUs_t previousTimeUpdate;                 // Occurs @ looptime rate
  73
  74     const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate;
  75     previousTimeUpdate = currentTimeUs;
  76
  77     // If last position update was too long in the past - ignore it (likely restarting altitude controller)
  78     if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
  79         previousTimeUpdate = currentTimeUs;
  80         previousTimePositionUpdate = currentTimeUs;
  81         resetFixedWingPositionController();
  82         return;
  83     }
  84
  85     // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
  86     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
  87         // If we have new position - update velocity and acceleration controllers
  88         if (posControl.flags.horizontalPositionDataNew) {
  89             const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
  90             previousTimePositionUpdate = currentTimeUs;
  91
  92             if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) {
  93                 update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
  94             } else {
  95                 resetFixedWingPositionController();
  96             }
  97
  98             // Indicate that information is no longer usable
  99             posControl.flags.horizontalPositionDataConsumed = 1;
 100         }
 101
 102         isYawAdjustmentValid = true;
 103     }
 104     else {
 105         isYawAdjustmentValid = false;
 106     }
 107 }
 108
 109 void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 110 {
 111     UNUSED(currentTimeUs);
 112     rcCommand[ROLL] = 0;
 113     rcCommand[PITCH] = 0;
 114
 115     if (navStateFlags & NAV_CTL_POS) {
 116
 117         if (navStateFlags & NAV_AUTO_WP_DONE) {
 118             /*
 119              * When WP mission is done, stop the motors
 120              */
 121             rcCommand[YAW] = 0;
 122             rcCommand[THROTTLE] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
 123         } else {
 124             if (isYawAdjustmentValid) {
 125                 rcCommand[YAW] = posControl.rcAdjustment[YAW];
 126             }
 127
 128             rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
 129         }
 130     }
 131 }
 132
 133 void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 134 {
 135     if (navStateFlags & NAV_CTL_EMERG) {
 136         rcCommand[ROLL] = 0;
 137         rcCommand[PITCH] = 0;
 138         rcCommand[YAW] = 0;
 139         rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
 140     } else if (navStateFlags & NAV_CTL_POS) {
 141         applyRoverBoatPositionController(currentTimeUs);
 142         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
 143     }
 144 }
 145
 146 #endif