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

Merge branch 'development' into dzikuvx-nav-yaw-adjustments

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2020-02-20 10:25:00 +01:00
commit e3b0327657
92 changed files with 1437 additions and 455 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -843,7 +843,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);
}
/*************************************************************************************************/
@ -941,7 +941,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.
@ -1020,7 +1020,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);
@ -1051,7 +1051,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;
}
@ -1080,7 +1080,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 {
@ -1117,14 +1117,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);
}
@ -1132,7 +1132,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 {
@ -1146,12 +1146,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 {
@ -1224,7 +1224,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;
@ -1401,8 +1401,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
tmpWaypoint.z = scaleRangef(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10.0f, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10.0f,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
@ -1675,7 +1675,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));
@ -2059,7 +2059,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
@ -2318,7 +2318,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 {
@ -2375,7 +2375,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 {
@ -2387,7 +2387,7 @@ bool isLandingDetected(void)
{
bool landingDetected;
if (STATE(FIXED_WING)) { // FIXED_WING
if (STATE(FIXED_WING_LEGACY)) { // FIXED_WING_LEGACY
landingDetected = isFixedWingLandingDetected();
}
else {
@ -2413,7 +2413,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);
@ -2436,7 +2436,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 {
@ -2446,7 +2446,7 @@ static void resetAltitudeController(bool useTerrainFollowing)
static void setupAltitudeController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
setupFixedWingAltitudeController();
}
else {
@ -2456,7 +2456,7 @@ static void setupAltitudeController(void)
static bool adjustAltitudeFromRCInput(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return adjustFixedWingAltitudeFromRCInput();
}
else {
@ -2469,7 +2469,7 @@ static bool adjustAltitudeFromRCInput(void)
*-----------------------------------------------------------*/
static void resetHeadingController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingHeadingController();
}
else {
@ -2479,7 +2479,7 @@ static void resetHeadingController(void)
static bool adjustHeadingFromRCInput(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
return adjustFixedWingHeadingFromRCInput();
}
else {
@ -2492,7 +2492,7 @@ static bool adjustHeadingFromRCInput(void)
*-----------------------------------------------------------*/
static void resetPositionController(void)
{
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
resetFixedWingPositionController();
}
else {
@ -2505,7 +2505,7 @@ static bool adjustPositionFromRCInput(void)
{
bool retValue;
if (STATE(FIXED_WING)) {
if (STATE(FIXED_WING_LEGACY)) {
retValue = adjustFixedWingPositionFromRCInput();
}
else {
@ -2773,7 +2773,7 @@ static void processNavigationRCAdjustments(void)
posControl.flags.isAdjustingPosition = adjustPositionFromRCInput();
}
else {
if (!STATE(FIXED_WING)) {
if (!STATE(FIXED_WING_LEGACY)) {
resetMulticopterBrakingMode();
}
}
@ -2825,7 +2825,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 {
@ -2898,7 +2898,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;
@ -2981,7 +2981,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);
}
/*-----------------------------------------------------------
@ -2990,7 +2990,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));
}
/*-----------------------------------------------------------
@ -2999,7 +2999,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));
}
@ -3014,7 +3014,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;
}
@ -3039,7 +3039,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) {