mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-20 14:55:18 +03:00
Simplified 2D navigation routenes
This commit is contained in:
parent
b47d666702
commit
f933381d18
4 changed files with 79 additions and 1 deletions
|
@ -201,6 +201,7 @@ COMMON_SRC = \
|
|||
navigation/navigation_pos_estimator.c \
|
||||
navigation/navigation_pos_estimator_agl.c \
|
||||
navigation/navigation_pos_estimator_flow.c \
|
||||
navigation/navigation_rover_boat.c \
|
||||
sensors/barometer.c \
|
||||
sensors/pitotmeter.c \
|
||||
sensors/rangefinder.c \
|
||||
|
|
|
@ -2982,7 +2982,9 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
|
||||
/* Process controllers */
|
||||
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
|
||||
if (STATE(FIXED_WING_LEGACY)) {
|
||||
if (STATE(ROVER) || STATE(BOAT)) {
|
||||
applyRoverBoatNavigationController(navStateFlags, currentTimeUs);
|
||||
} else if (STATE(FIXED_WING_LEGACY)) {
|
||||
applyFixedWingNavigationController(navStateFlags, currentTimeUs);
|
||||
}
|
||||
else {
|
||||
|
|
|
@ -444,6 +444,8 @@ bool adjustFixedWingAltitudeFromRCInput(void);
|
|||
bool adjustFixedWingHeadingFromRCInput(void);
|
||||
bool adjustFixedWingPositionFromRCInput(void);
|
||||
|
||||
void applyFixedWingPositionController(timeUs_t currentTimeUs);
|
||||
|
||||
void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
|
||||
|
||||
void calculateFixedWingInitialHoldPosition(fpVector3_t * pos);
|
||||
|
@ -456,4 +458,9 @@ bool isFixedWingLaunchFinishedOrAborted(void);
|
|||
void abortFixedWingLaunch(void);
|
||||
void applyFixedWingLaunchController(timeUs_t currentTimeUs);
|
||||
|
||||
/*
|
||||
* Rover specific functions
|
||||
*/
|
||||
void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);
|
||||
|
||||
#endif
|
||||
|
|
68
src/main/navigation/navigation_rover_boat.c
Normal file
68
src/main/navigation/navigation_rover_boat.c
Normal file
|
@ -0,0 +1,68 @@
|
|||
/*
|
||||
* This file is part of INAV Project.
|
||||
*
|
||||
* This Source Code Form is subject to the terms of the Mozilla Public
|
||||
* License, v. 2.0. If a copy of the MPL was not distributed with this file,
|
||||
* You can obtain one at http://mozilla.org/MPL/2.0/.
|
||||
*
|
||||
* Alternatively, the contents of this file may be used under the terms
|
||||
* of the GNU General Public License Version 3, as described below:
|
||||
*
|
||||
* This file is free software: you may copy, redistribute and/or modify
|
||||
* it under the terms of the GNU General Public License as published by the
|
||||
* Free Software Foundation, either version 3 of the License, or (at your
|
||||
* option) any later version.
|
||||
*
|
||||
* This file is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
|
||||
* Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with this program. If not, see http://www.gnu.org/licenses/.
|
||||
*/
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
FILE_COMPILE_FOR_SIZE
|
||||
|
||||
#ifdef USE_NAV
|
||||
|
||||
#include "common/utils.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "flight/mixer.h"
|
||||
|
||||
#include "navigation/navigation.h"
|
||||
#include "navigation/navigation_private.h"
|
||||
|
||||
void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
|
||||
{
|
||||
UNUSED(currentTimeUs);
|
||||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
|
||||
if (navStateFlags & NAV_CTL_POS) {
|
||||
|
||||
// if (isYawAdjustmentValid) {
|
||||
rcCommand[YAW] = posControl.rcAdjustment[YAW];
|
||||
// }
|
||||
|
||||
// const uint16_t correctedThrottleValue = constrain(navConfig()->fw.cruise_throttle, navConfig()->fw.min_throttle, navConfig()->fw.max_throttle);
|
||||
rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
|
||||
}
|
||||
}
|
||||
|
||||
void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)
|
||||
{
|
||||
if (navStateFlags & NAV_CTL_EMERG) {
|
||||
rcCommand[ROLL] = 0;
|
||||
rcCommand[PITCH] = 0;
|
||||
rcCommand[YAW] = 0;
|
||||
rcCommand[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
} else if (navStateFlags & NAV_CTL_POS) {
|
||||
applyFixedWingPositionController(currentTimeUs);
|
||||
applyRoverBoatPitchRollThrottleController(navStateFlags, currentTimeUs);
|
||||
}
|
||||
}
|
||||
|
||||
#endif
|
Loading…
Add table
Add a link
Reference in a new issue