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

Merge branch 'development' into dzikuvx-nav-cruise-improvements

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-03-05 15:10:49 +01:00
commit ae774017da
112 changed files with 1552 additions and 2772 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -80,7 +80,7 @@ radar_pois_t radar_pois[RADAR_MAX_POIS];
PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0);
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 5);
PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 6);
PG_RESET_TEMPLATE(navConfig_t, navConfig,
.general = {
@ -165,7 +165,9 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
.launch_climb_angle = 18, // 18 degrees
.launch_max_angle = 45, // 45 deg
.cruise_yaw_rate = 20, // 20dps
.allow_manual_thr_increase = false
.allow_manual_thr_increase = false,
.useFwNavYawControl = 0,
.yawControlDeadband = 0,
}
);
@ -617,12 +619,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_WAYPOINT_PRE_ACTION] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_PRE_ACTION,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION,
.timeoutMs = 0,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_PROCESS_NEXT,
.mwError = MW_NAV_ERROR_NONE,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP)
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
@ -841,7 +844,7 @@ navigationFSMStateFlags_t navGetCurrentStateFlags(void)
static bool navTerrainFollowingRequested(void)
{
// Terrain following not supported on FIXED WING aircraft yet
return !STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXSURFACE);
return !STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXSURFACE);
}
/*************************************************************************************************/
@ -939,7 +942,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na
{
const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // Only on FW for now
DEBUG_SET(DEBUG_CRUISE, 0, 1);
if (checkForPositionSensorTimeout()) { return NAV_FSM_EVENT_SWITCH_TO_IDLE; } // Switch to IDLE if we do not have an healty position. Try the next iteration.
@ -1018,7 +1021,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState)
{
if (!STATE(FIXED_WING)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now
navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState);
@ -1049,7 +1052,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
if (STATE(FIXED_WING) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
if (STATE(FIXED_WING_LEGACY) && (posControl.homeDistance < navConfig()->general.min_rth_distance) && !posControl.flags.forcedRTHActivated) {
// Prevent RTH from activating on airplanes if too close to home unless it's a failsafe RTH
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
}
@ -1078,7 +1081,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
else {
fpVector3_t targetHoldPos;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// Airplane - climbout before turning around
calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away
} else {
@ -1115,14 +1118,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
// If we have valid pos sensor OR we are configured to ignore GPS loss
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const uint8_t rthClimbMarginPercent = STATE(FIXED_WING_LEGACY) ? FW_RTH_CLIMB_MARGIN_PERCENT : MR_RTH_CLIMB_MARGIN_PERCENT;
const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z));
// If we reached desired initial RTH altitude or we don't want to climb first
if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
}
@ -1130,7 +1133,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
posControl.rthState.rthInitialDistance = posControl.homeDistance;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING)) {
if (navConfig()->general.flags.rth_tail_first && !STATE(FIXED_WING_LEGACY)) {
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
}
else {
@ -1144,12 +1147,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL);
/* For multi-rotors execute sanity check during initial ascent as well */
if (!STATE(FIXED_WING) && !validateRTHSanityChecker()) {
if (!STATE(FIXED_WING_LEGACY) && !validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
// Climb to safe altitude and turn to correct direction
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM;
setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z);
} else {
@ -1222,7 +1225,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
if ((posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()) {
// Wait until target heading is reached (with 15 deg margin for error)
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetLandingDetector();
updateClimbRateToAltitudeController(0, ROC_TO_ALT_RESET);
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME;
@ -1367,20 +1370,46 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
/* A helper function to do waypoint-specific action */
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_JUMP:
if(posControl.waypointList[posControl.activeWaypointIndex].p2 != -1){
if(posControl.waypointList[posControl.activeWaypointIndex].p2 == 0){
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// JUMP is the last waypoint and we reached the last jump, switch to finish.
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
} else {
// Finished JUMP, move to next WP
posControl.activeWaypointIndex++;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
}
}
else
{
posControl.waypointList[posControl.activeWaypointIndex].p2--;
}
}
posControl.activeWaypointIndex = posControl.waypointList[posControl.activeWaypointIndex].p1 - 1;
return NAV_FSM_EVENT_NONE; // re-process the state passing to the next WP
case NAV_WP_ACTION_RTH:
default:
posControl.rthState.rthInitialDistance = posControl.homeDistance;
initializeRTHSanityChecker(&navGetCurrentActualPositionAndVelocity()->pos);
calculateAndSetActiveWaypointToLocalPosition(rthGetHomeTargetPosition(RTH_HOME_ENROUTE_INITIAL));
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
};
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(navigationFSMState_t previousState)
@ -1389,9 +1418,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
// If no position sensor available - land immediately
if ((posControl.flags.estPosStatus >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
default:
if (isWaypointReached(&posControl.activeWaypoint, false) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
@ -1407,6 +1435,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
}
break;
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (isWaypointReached(&posControl.activeWaypoint, true) || isWaypointMissed(&posControl.activeWaypoint)) {
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
@ -1426,13 +1457,21 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
else {
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(navigationFSMState_t previousState)
{
UNUSED(previousState);
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
switch ((navWaypointActions_e)posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
case NAV_WP_ACTION_JUMP:
UNREACHABLE();
case NAV_WP_ACTION_RTH:
if (posControl.waypointList[posControl.activeWaypointIndex].p1 != 0) {
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND;
@ -1440,10 +1479,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_REACHED(naviga
else {
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
default:
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_WAYPOINT_NEXT
}
UNREACHABLE();
}
static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_RTH_LAND(navigationFSMState_t previousState)
@ -1673,7 +1711,7 @@ static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode)
case RTH_HOME_ENROUTE_PROPORTIONAL:
{
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING) ? navConfig()->fw.loiter_radius : 0);
float rthTotalDistanceToTravel = posControl.rthState.rthInitialDistance - (STATE(FIXED_WING_LEGACY) ? navConfig()->fw.loiter_radius : 0);
if (rthTotalDistanceToTravel >= 100) {
float ratioNotTravelled = constrainf(posControl.homeDistance / rthTotalDistanceToTravel, 0.0f, 1.0f);
posControl.rthState.homeTmpWaypoint.z = (posControl.rthState.rthInitialAltitude * ratioNotTravelled) + (posControl.rthState.rthFinalAltitude * (1.0f - ratioNotTravelled));
@ -1786,6 +1824,11 @@ float navPidApply3(
}
}
/*
* Limit both output and Iterm to limit windup
*/
pid->integrator = constrain(pid->integrator, outMin, outMax);
return outValConstrained;
}
@ -2063,7 +2106,7 @@ static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWayp
// We consider waypoint reached if within specified radius
posControl.wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING) && isWaypointHome) {
if (STATE(FIXED_WING_LEGACY) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|| (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
@ -2322,7 +2365,7 @@ uint32_t getTotalTravelDistance(void)
*-----------------------------------------------------------*/
void calculateInitialHoldPosition(fpVector3_t * pos)
{
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
calculateFixedWingInitialHoldPosition(pos);
}
else {
@ -2379,7 +2422,7 @@ void calculateNewCruiseTarget(fpVector3_t * origin, int32_t yaw, int32_t distanc
*-----------------------------------------------------------*/
void resetLandingDetector(void)
{
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
resetFixedWingLandingDetector();
}
else {
@ -2391,7 +2434,7 @@ bool isLandingDetected(void)
{
bool landingDetected;
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
landingDetected = isFixedWingLandingDetected();
}
else {
@ -2417,7 +2460,7 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
posControl.desiredState.pos.z = altitudeToUse;
}
else {
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
@ -2440,7 +2483,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
// Set terrain following flag
posControl.flags.isTerrainFollowEnabled = useTerrainFollowing;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingAltitudeController();
}
else {
@ -2450,7 +2493,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
static void setupAltitudeController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
setupFixedWingAltitudeController();
}
else {
@ -2460,7 +2503,7 @@ static void setupAltitudeController(void)
static bool adjustAltitudeFromRCInput(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return adjustFixedWingAltitudeFromRCInput();
}
else {
@ -2473,7 +2516,7 @@ static bool adjustAltitudeFromRCInput(void)
*-----------------------------------------------------------*/
static void resetHeadingController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingHeadingController();
}
else {
@ -2483,7 +2526,7 @@ static void resetHeadingController(void)
static bool adjustHeadingFromRCInput(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return adjustFixedWingHeadingFromRCInput();
}
else {
@ -2496,7 +2539,7 @@ static bool adjustHeadingFromRCInput(void)
*-----------------------------------------------------------*/
static void resetPositionController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingPositionController();
}
else {
@ -2509,7 +2552,7 @@ static bool adjustPositionFromRCInput(void)
{
bool retValue;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
retValue = adjustFixedWingPositionFromRCInput();
}
else {
@ -2607,9 +2650,9 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
setDesiredPosition(&wpPos.pos, DEGREES_TO_CENTIDEGREES(wpData->p1), waypointUpdateFlags);
}
// WP #1 - #15 - common waypoints - pre-programmed mission
// WP #1 - #NAV_MAX_WAYPOINTS - common waypoints - pre-programmed mission
else if ((wpNumber >= 1) && (wpNumber <= NAV_MAX_WAYPOINTS) && !ARMING_FLAG(ARMED)) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_RTH) {
if (wpData->action == NAV_WP_ACTION_WAYPOINT || wpData->action == NAV_WP_ACTION_JUMP || wpData->action == NAV_WP_ACTION_RTH) {
// Only allow upload next waypoint (continue upload mission) or first waypoint (new mission)
if (wpNumber == (posControl.waypointCount + 1) || wpNumber == 1) {
posControl.waypointList[wpNumber - 1] = *wpData;
@ -2777,7 +2820,7 @@ static void processNavigationRCAdjustments(void)
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
}
else {
if (!STATE(FIXED_WING)) {
if (!STATE(FIXED_WING_LEGACY)) {
resetMulticopterBrakingMode();
}
}
@ -2829,7 +2872,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
/* Process controllers */
navigationFSMStateFlags_t navStateFlags = navGetStateFlags(posControl.navState);
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
applyFixedWingNavigationController(navStateFlags, currentTimeUs);
}
else {
@ -2902,7 +2945,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
bool canActivateNavigation = canActivateNavigationModes();
// LAUNCH mode has priority over any other NAV mode
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
if (isNavLaunchEnabled()) { // FIXME: Only available for fixed wing aircrafts now
if (canActivateLaunchMode) {
canActivateLaunchMode = false;
@ -2985,7 +3028,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
*-----------------------------------------------------------*/
bool navigationRequiresThrottleTiltCompensation(void)
{
return !STATE(FIXED_WING) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
return !STATE(FIXED_WING_LEGACY) && (navGetStateFlags(posControl.navState) & NAV_REQUIRE_THRTILT);
}
/*-----------------------------------------------------------
@ -2994,7 +3037,7 @@ bool navigationRequiresThrottleTiltCompensation(void)
bool navigationRequiresAngleMode(void)
{
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING));
return (currentState & NAV_REQUIRE_ANGLE) || ((currentState & NAV_REQUIRE_ANGLE_FW) && STATE(FIXED_WING_LEGACY));
}
/*-----------------------------------------------------------
@ -3003,7 +3046,7 @@ bool navigationRequiresAngleMode(void)
bool navigationRequiresTurnAssistance(void)
{
const navigationFSMStateFlags_t currentState = navGetStateFlags(posControl.navState);
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
// For airplanes turn assistant is always required when controlling position
return (currentState & (NAV_CTL_POS | NAV_CTL_ALT));
}
@ -3018,7 +3061,7 @@ bool navigationRequiresTurnAssistance(void)
int8_t navigationGetHeadingControlState(void)
{
// For airplanes report as manual heading control
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return NAV_HEADING_CONTROL_MANUAL;
}
@ -3043,7 +3086,7 @@ bool navigationTerrainFollowingEnabled(void)
navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
{
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE));
if (usedBypass) {
@ -3083,6 +3126,17 @@ navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass)
}
}
// Don't allow arming if any of JUMP waypoint has invalid settings
if (posControl.waypointCount > 0) {
for (uint8_t wp = 0; wp < posControl.waypointCount ; wp++){
if (posControl.waypointList[wp].action == NAV_WP_ACTION_JUMP){
if((posControl.waypointList[wp].p1 > posControl.waypointCount) || (posControl.waypointList[wp].p2 < -1)){
return NAV_ARMING_BLOCKER_JUMP_WAYPOINT_ERROR;
}
}
}
}
return NAV_ARMING_BLOCKER_NONE;
}
@ -3217,6 +3271,13 @@ void navigationUsePIDs(void)
0.0f,
NAV_DTERM_CUT_HZ
);
navPidInit(&posControl.pids.fw_heading, (float)pidProfile()->bank_fw.pid[PID_POS_HEADING].P / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].I / 10.0f,
(float)pidProfile()->bank_fw.pid[PID_POS_HEADING].D / 100.0f,
0.0f,
2.0f
);
}
void navigationInit(void)
@ -3247,6 +3308,16 @@ void navigationInit(void)
/* Use system config */
navigationUsePIDs();
if (
mixerConfig()->platformType == PLATFORM_BOAT ||
mixerConfig()->platformType == PLATFORM_ROVER ||
navConfig()->fw.useFwNavYawControl
) {
ENABLE_STATE(FW_HEADING_USE_YAW);
} else {
DISABLE_STATE(FW_HEADING_USE_YAW);
}
}
/*-----------------------------------------------------------