diff --git a/flight/Libraries/paths.c b/flight/Libraries/paths.c
index ab4a8a5a4..d408f818a 100644
--- a/flight/Libraries/paths.c
+++ b/flight/Libraries/paths.c
@@ -101,11 +101,7 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
return;
}
- if(dist_path < 1e-6) {
- status->fractional_progress = 0;
- } else {
- status->fractional_progress = 1 - dist_diff / dist_path;
- }
+ status->fractional_progress = 1 - dist_diff / (1 + dist_path);
status->error = dist_diff;
// Compute direction to travel
@@ -140,10 +136,11 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_path < 1e-6) {
+ // if the path is too short, we cannot determine vector direction.
+ // Fly towards the endpoint to prevent flying away,
+ // but assume progress=1 either way.
+ path_endpoint( start_point, end_point, cur_point, status );
status->fractional_progress = 1;
- status->error = 0;
- status->correction_direction[0] = status->correction_direction[1] = 0;
- status->path_direction[0] = status->path_direction[1] = 0;
return;
}
diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c
index 3d7c926c6..127f70010 100644
--- a/flight/Modules/PathPlanner/pathplanner.c
+++ b/flight/Modules/PathPlanner/pathplanner.c
@@ -126,7 +126,6 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/**
* Module task
*/
-int32_t bad_inits;
static void pathPlannerTask(void *parameters)
{
// update settings on change
@@ -178,7 +177,8 @@ static void pathPlannerTask(void *parameters)
continue;
}
- if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL) {
+ // negative destinations DISABLE this feature
+ if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
continue;
}
@@ -188,21 +188,31 @@ static void pathPlannerTask(void *parameters)
// decide what to do
switch (pathAction.Command) {
+ case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
+ endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) setWaypoint(waypointActive.Index+1);
break;
- case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
- if (!endCondition) setWaypoint(waypointActive.Index+1);
- break;
- case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
- if (endCondition) setWaypoint(pathAction.JumpDestination);
- break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
- if (!endCondition) setWaypoint(pathAction.JumpDestination);
+ endCondition = !endCondition;
+ case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
+ if (endCondition) {
+ if (pathAction.JumpDestination<0) {
+ // waypoint ids <0 code relative jumps
+ setWaypoint(waypointActive.Index - pathAction.JumpDestination );
+ } else {
+ setWaypoint(pathAction.JumpDestination);
+ }
+ }
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
- setWaypoint(pathAction.JumpDestination);
+ if (pathAction.JumpDestination<0) {
+ // waypoint ids <0 code relative jumps
+ setWaypoint(waypointActive.Index - pathAction.JumpDestination );
+ } else {
+ setWaypoint(pathAction.JumpDestination);
+ }
} else {
setWaypoint(waypointActive.Index+1);
}
@@ -261,11 +271,11 @@ void updatePathDesired(UAVObjEvent * ev) {
pathDesired.UID = waypointActive.Index;
if(waypointActive.Index == 0) {
- // Get home position as start point
+ // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
- pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
- pathDesired.Start[PATHDESIRED_START_EAST] = 0;
- pathDesired.Start[PATHDESIRED_START_DOWN] = -1;
+ pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
+ pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
+ pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else {
// Get previous waypoint as start point
@@ -629,7 +639,7 @@ static void createPathLogo()
waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50;
- bad_inits += (WaypointInstSet(i, &waypoint) != 0);
+ WaypointInstSet(i, &waypoint);
}
// Draw P
@@ -637,7 +647,7 @@ static void createPathLogo()
waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[2] = -50;
- bad_inits += (WaypointInstSet(i, &waypoint) != 0);
+ WaypointInstSet(i, &waypoint);
}
waypoint.Position[1] = SIZE * 35;
diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml
index b711ae348..70936f58d 100644
--- a/shared/uavobjectdefinition/pathaction.xml
+++ b/shared/uavobjectdefinition/pathaction.xml
@@ -19,8 +19,8 @@
-
-
+
+
diff --git a/shared/uavobjectdefinition/pathdesired.xml b/shared/uavobjectdefinition/pathdesired.xml
index 9e279a97b..539ed6461 100644
--- a/shared/uavobjectdefinition/pathdesired.xml
+++ b/shared/uavobjectdefinition/pathdesired.xml
@@ -17,7 +17,7 @@
-
+
diff --git a/shared/uavobjectdefinition/pathstatus.xml b/shared/uavobjectdefinition/pathstatus.xml
index a516372b1..7501e7ed1 100644
--- a/shared/uavobjectdefinition/pathstatus.xml
+++ b/shared/uavobjectdefinition/pathstatus.xml
@@ -2,7 +2,7 @@