Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

On rovers and boats stop motors when mission is done

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-04-22 14:33:50 +02:00
parent 5475f887ce
commit 3acb85feb8
3 changed files with 15 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/navigation/navigation.c
View file

@ -743,7 +743,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
.timeoutMs = 0,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_WP_ENROUTE,
.mwError = MW_NAV_ERROR_FINISH,

1
src/main/navigation/navigation_private.h
View file

@ -276,6 +276,7 @@ typedef enum {
/* Additional flags */
NAV_CTL_LAND = (1 << 14),
NAV_AUTO_WP_DONE = (1 << 15), //Waypoint mission reached the last waypoint and is idling
} navigationFSMStateFlags_t;
typedef struct {

10
src/main/navigation/navigation_rover_boat.c
View file

@ -31,8 +31,8 @@ FILE_COMPILE_FOR_SIZE
#include "build/debug.h"
#include "common/utils.h"
#include "fc/rc_controls.h"
#include "fc/config.h"
#include "flight/mixer.h"
#include "navigation/navigation.h"
#include "navigation/navigation_private.h"
@ -114,12 +114,20 @@ void applyRoverBoatPitchRollThrottleController(navigationFSMStateFlags_t navStat
if (navStateFlags & NAV_CTL_POS) {
if (navStateFlags & NAV_AUTO_WP_DONE) {
/*
* When WP mission is done, stop the motors
*/
rcCommand[YAW] = 0;
rcCommand[THROTTLE] = feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand;
} else {
if (isYawAdjustmentValid) {
rcCommand[YAW] = posControl.rcAdjustment[YAW];
}
rcCommand[THROTTLE] = constrain(navConfig()->fw.cruise_throttle, motorConfig()->mincommand, motorConfig()->maxthrottle);
}
}
}
void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs)