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

first build

Browse source
This commit is contained in:
breadoven 2021-11-29 23:30:08 +00:00
parent 0d360be497
commit d16fa16572
4 changed files with 18 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -1642,11 +1642,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_NEXT(navigatio
{
UNUSED(previousState);
const bool isLastWaypoint = (posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST) ||
(posControl.activeWaypointIndex >= (posControl.waypointCount - 1));
if (isLastWaypoint) {
// Last waypoint reached
if (isLastMissionWaypoint()) { // Last waypoint reached
return NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED;
}
else {
@ -3059,37 +3055,21 @@ static void calculateAndSetActiveWaypoint(const navWaypoint_t * waypoint)
calculateAndSetActiveWaypointToLocalPosition(&localPos);
}
/**
* Returns TRUE if we are in WP mode and executing last waypoint on the list, or in RTH mode, or in PH mode
* In RTH mode our only and last waypoint is home
* In PH mode our waypoint is hold position */
bool isApproachingLastWaypoint(void)
/* Checks if active waypoint is last in mission */
bool isLastMissionWaypoint(void)
{
if (navGetStateFlags(posControl.navState) & NAV_AUTO_WP) {
if (posControl.waypointCount == 0) {
/* No waypoints */
return true;
}
else if ((posControl.activeWaypointIndex == (posControl.waypointCount - 1)) ||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST)) {
return true;
}
else {
return false;
}
}
else if (navGetStateFlags(posControl.navState) & NAV_CTL_POS) {
// If POS controller is active we are in Poshold or RTH mode - assume last waypoint
return true;
}
else {
return false;
}
return FLIGHT_MODE(NAV_WP_MODE) && (posControl.activeWaypointIndex >= (posControl.waypointCount - 1) ||
(posControl.waypointList[posControl.activeWaypointIndex].flag == NAV_WP_FLAG_LAST));
}
bool isWaypointWait(void)
/* Checks if approaching hold position requiring fixed wing circling loiter */
bool isApproachingHoldPosition(void)
{
return NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
if (FLIGHT_MODE(NAV_WP_MODE)) { // WP mode last WP hold and Timed hold positions
return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED;
}
// RTH spiral climb and Home positions and POSHOLD (Course Hold excluded, no loiter required)
return (navGetCurrentStateFlags() & NAV_CTL_POS) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
}
float getActiveWaypointSpeed(void)

7
src/main/navigation/navigation_fixedwing.c
View file

@ -277,10 +277,9 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod)
// If angular visibility of a waypoint is less than 30deg, don't calculate circular loiter, go straight to the target
#define TAN_15DEG 0.26795f
bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait())
&& (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG))
&& (distanceToActualTarget > 50.0f)
&& !FLIGHT_MODE(NAV_COURSE_HOLD_MODE);
bool needToCalculateCircularLoiter = isApproachingHoldPosition() &&
(distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) &&
(distanceToActualTarget > 50.0f);
// Calculate virtual position for straight movement
if (needToCalculateCircularLoiter) {

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

@ -446,7 +446,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed)
*/
if (
(navGetCurrentStateFlags() & NAV_AUTO_WP &&
!isApproachingLastWaypoint() &&
!isLastMissionWaypoint() &&
newVelTotal < maxSpeed &&
!navConfig()->mc.slowDownForTurning
) || newVelTotal > maxSpeed

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

@ -418,8 +418,8 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
bool isWaypointReached(const navWaypointPosition_t * waypoint, const bool isWaypointHome);
bool isWaypointMissed(const navWaypointPosition_t * waypoint);
bool isWaypointWait(void);
bool isApproachingLastWaypoint(void);
bool isApproachingHoldPosition(void);
bool isLastMissionWaypoint(void);
float getActiveWaypointSpeed(void);
void updateActualHeading(bool headingValid, int32_t newHeading);