From 65459c7ba03950523346339402d431ec8af19601 Mon Sep 17 00:00:00 2001 From: julian_lilov Date: Thu, 3 Oct 2019 16:39:04 +0300 Subject: [PATCH 1/5] LP_616 conditionPointingTowardsNext fix --- flight/modules/PathPlanner/pathplanner.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index 833c208c8..e68eba00f 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -662,7 +662,10 @@ static uint8_t conditionPointingTowardsNext() WaypointData nextWaypoint; WaypointInstGet(nextWaypointId, &nextWaypoint); - float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East)); + PositionStateData positionState; + PositionStateGet(&positionState); + + float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East)); VelocityStateData velocity; VelocityStateGet(&velocity); From a17a153a1bd7df49df84c08c626d3d2e4481f94e Mon Sep 17 00:00:00 2001 From: julian_lilov Date: Fri, 16 Oct 2020 11:17:27 +0300 Subject: [PATCH 2/5] LP_616 updatePathDesired() fix: leaving the circle on tangent --- flight/modules/PathPlanner/pathplanner.c | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index e68eba00f..b07bcb111 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -339,10 +339,23 @@ void updatePathDesired() WaypointData waypointPrev; WaypointInstGet(waypointActive.Index - 1, &waypointPrev); - pathDesired.Start.North = waypointPrev.Position.North; - pathDesired.Start.East = waypointPrev.Position.East; - pathDesired.Start.Down = waypointPrev.Position.Down; - pathDesired.StartingVelocity = waypointPrev.Velocity; + // When exiting CIRCLE, use current UAV position as a start point for PathDesired vector to the next waypoint + // instead of previous waypoint location (that represents the center of the circle) + PathActionData prevAction; + PathActionInstGet(waypointPrev.Action, &prevAction); + if ((prevAction.Mode == PATHACTION_MODE_CIRCLERIGHT) || (prevAction.Mode == PATHACTION_MODE_CIRCLELEFT)) { + PositionStateData positionState; + PositionStateGet(&positionState); + pathDesired.Start.North = positionState.North; + pathDesired.Start.East = positionState.East; + pathDesired.Start.Down = positionState.Down; + pathDesired.StartingVelocity = waypointPrev.Velocity; + } else { + pathDesired.Start.North = waypointPrev.Position.North; + pathDesired.Start.East = waypointPrev.Position.East; + pathDesired.Start.Down = waypointPrev.Position.Down; + pathDesired.StartingVelocity = waypointPrev.Velocity; + } } PathDesiredSet(&pathDesired); From 559907a3e4e779dc981967ee71f9ffc395096c9d Mon Sep 17 00:00:00 2001 From: julian_lilov Date: Mon, 14 Dec 2020 10:12:06 +0200 Subject: [PATCH 3/5] LP_620 --- flight/libraries/plans.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index f2c9d23b7..3e481af6d 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -495,7 +495,7 @@ void plan_run_VelocityRoam() cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); - bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f); + bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.05f || fabsf(cmd.Pitch) > 0.05f); if (!flagRollPitchHasInput) { // no movement desired, re-enter positionHold at current start-position From b2fb571999d0bd929e9e9f02de88c38670a7ac61 Mon Sep 17 00:00:00 2001 From: julian_lilov Date: Mon, 14 Dec 2020 10:32:06 +0200 Subject: [PATCH 4/5] Revert "LP_620" This reverts commit 559907a3e4e779dc981967ee71f9ffc395096c9d. --- flight/libraries/plans.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/libraries/plans.c b/flight/libraries/plans.c index 3e481af6d..f2c9d23b7 100644 --- a/flight/libraries/plans.c +++ b/flight/libraries/plans.c @@ -495,7 +495,7 @@ void plan_run_VelocityRoam() cmd.Pitch = applyExpo(cmd.Pitch, stabSettings.StickExpo.Pitch); cmd.Yaw = applyExpo(cmd.Yaw, stabSettings.StickExpo.Yaw); - bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.05f || fabsf(cmd.Pitch) > 0.05f); + bool flagRollPitchHasInput = (fabsf(cmd.Roll) > 0.0f || fabsf(cmd.Pitch) > 0.0f); if (!flagRollPitchHasInput) { // no movement desired, re-enter positionHold at current start-position From 0ddce8ce96b9abe6b4ca50ac7b0c46f5a859750d Mon Sep 17 00:00:00 2001 From: julian_lilov Date: Thu, 14 Oct 2021 17:14:46 +0300 Subject: [PATCH 5/5] LP_616 safety check preventing possible execution of atan2f(0,0) --- flight/modules/PathPlanner/pathplanner.c | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/flight/modules/PathPlanner/pathplanner.c b/flight/modules/PathPlanner/pathplanner.c index b07bcb111..30b5391ab 100644 --- a/flight/modules/PathPlanner/pathplanner.c +++ b/flight/modules/PathPlanner/pathplanner.c @@ -678,6 +678,11 @@ static uint8_t conditionPointingTowardsNext() PositionStateData positionState; PositionStateGet(&positionState); + // check if current position exactly matches nextWaipoint (in 2D space) + if ((fabsf(nextWaypoint.Position.North - positionState.North) < 1e-6f) && (fabsf(nextWaypoint.Position.East - positionState.East) < 1e-6f)) { + return true; + } + float angle1 = atan2f((nextWaypoint.Position.North - positionState.North), (nextWaypoint.Position.East - positionState.East)); VelocityStateData velocity;