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 #include "build/debug.h"
  28
  29 #include "common/utils.h"
  30
  31 #include "fc/rc_controls.h"
  32 #include "fc/config.h"
  33
  34 #include "flight/mixer.h"
  35
  36 #include "navigation/navigation.h"
  37 #include "navigation/navigation_private.h"
  38
  39 #include "sensors/battery.h"
  40
  41 static bool isYawAdjustmentValid = false;
  42 static int32_t navHeadingError;
  43
  44 static void update2DPositionHeadingController(timeUs_t currentTimeUs, timeDelta_t deltaMicros)
  45 {
  46     static timeUs_t previousTimeMonitoringUpdate;
  47     static int32_t previousHeadingError;
  48     static bool errorIsDecreasing;
  49
  50     int32_t targetBearing = calculateBearingToDestination(&posControl.desiredState.pos);
  51
  52     /*
  53      * Calculate NAV heading error
  54      * Units are centidegrees
  55      */
  56     navHeadingError = wrap_18000(targetBearing - posControl.actualState.yaw);
  57
  58     // Slow error monitoring (2Hz rate)
  59     if ((currentTimeUs - previousTimeMonitoringUpdate) >= HZ2US(NAV_FW_CONTROL_MONITORING_RATE)) {
  60         // Check if error is decreasing over time
  61         errorIsDecreasing = (ABS(previousHeadingError) > ABS(navHeadingError));
  62
  63         // Save values for next iteration
  64         previousHeadingError = navHeadingError;
  65         previousTimeMonitoringUpdate = currentTimeUs;
  66     }
  67
  68     posControl.rcAdjustment[YAW] = processHeadingYawController(deltaMicros, navHeadingError, errorIsDecreasing);
  69 }
  70
  71 void applyRoverBoatPositionController(timeUs_t currentTimeUs)
  72 {
  73     static timeUs_t previousTimePositionUpdate;         // Occurs @ GPS update rate
  74     static timeUs_t previousTimeUpdate;                 // Occurs @ looptime rate
  75
  76     const timeDeltaLarge_t deltaMicros = currentTimeUs - previousTimeUpdate;
  77     previousTimeUpdate = currentTimeUs;
  78
  79     // If last position update was too long in the past - ignore it (likely restarting altitude controller)
  80     if (deltaMicros > MAX_POSITION_UPDATE_INTERVAL_US) {
  81         previousTimeUpdate = currentTimeUs;
  82         previousTimePositionUpdate = currentTimeUs;
  83         resetFixedWingPositionController();
  84         return;
  85     }
  86
  87     // Apply controller only if position source is valid. In absence of valid pos sensor (GPS loss), we'd stick in forced ANGLE mode
  88     if ((posControl.flags.estPosStatus >= EST_USABLE)) {
  89         // If we have new position - update velocity and acceleration controllers
  90         if (posControl.flags.horizontalPositionDataNew) {
  91             const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate;
  92             previousTimePositionUpdate = currentTimeUs;
  93
  94             if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) {
  95                 update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate);
  96             } else {
  97                 resetFixedWingPositionController();
  98             }
  99
 100             // Indicate that information is no longer usable
 101             posControl.flags.horizontalPositionDataConsumed = true;
 102         }
 103
 104         isYawAdjustmentValid = true;
 105     }
 106     else {
 107         isYawAdjustmentValid = false;
 108     }
 109 }
 110
 111 void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 112 {
 113     UNUSED(currentTimeUs);
 114     rcCommand[ROLL] = 0;
 115     rcCommand[PITCH] = 0;
 116
 117     if (navStateFlags & NAV_CTL_POS) {
 118
 119         if (navStateFlags & NAV_AUTO_WP_DONE) {
 120             /*
 121              * When WP mission is done, stop the motors
 122              */
 123             rcCommand[YAW] = 0;
 124             rcCommand[THROTTLE] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
 125         } else {
 126             if (isYawAdjustmentValid) {
 127                 rcCommand[YAW] = posControl.rcAdjustment[YAW];
 128             }
 129
 130             rcCommand[THROTTLE] = constrain(currentBatteryProfile->nav.fw.cruise_throttle, motorConfig()->mincommand, getMaxThrottle());
 131         }
 132     }
 133 }
 134
 135 void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
 136 {
 137     if (navStateFlags & NAV_CTL_EMERG) {
 138         rcCommand[ROLL] = 0;
 139         rcCommand[PITCH] = 0;
 140         rcCommand[YAW] = 0;
 141         rcCommand[THROTTLE] = setDesiredThrottle(currentBatteryProfile->failsafe_throttle, true);
 142     } else if (navStateFlags & NAV_CTL_POS) {
 143         applyRoverBoatPositionController(currentTimeUs);
 144         applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
 145     }
 146 }