From c26cceb47b4095f1d19c0fece4c80001381898c8 Mon Sep 17 00:00:00 2001
From: James Cotton <peabody124@gmail.com>
Date: Thu, 15 Mar 2012 04:33:32 -0500
Subject: [PATCH] If the waypoint index is changed then update teh position
 desired.  This should probably be done with change callbacks because
 currently changing the waypoints themselves doesn't update position desired.

---
 flight/Modules/PathPlanner/pathplanner.c | 40 ++++++++++++++----------
 1 file changed, 24 insertions(+), 16 deletions(-)

diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c
index 446638735..baf9db989 100644
--- a/flight/Modules/PathPlanner/pathplanner.c
+++ b/flight/Modules/PathPlanner/pathplanner.c
@@ -49,6 +49,7 @@ static xQueueHandle queue;
 
 // Private functions
 static void pathPlannerTask(void *parameters);
+static void updatePositionDesired();
 
 /**
  * Module initialization
@@ -126,6 +127,7 @@ static void pathPlannerTask(void *parameters)
 
 	// Main thread loop
 	bool pathplanner_active = false;
+	uint32_t last_index=0;
 	while (1)
 	{
 		FlightStatusGet(&flightStatus);
@@ -142,15 +144,10 @@ static void pathPlannerTask(void *parameters)
 			waypointActive.Index = 0;
 			WaypointActiveSet(&waypointActive);
 
-			WaypointInstGet(waypointActive.Index, &waypoint);
-
-			PositionDesiredGet(&positionDesired);
-			positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
-			positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
-			positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
-			PositionDesiredSet(&positionDesired);
+			updatePositionDesired();
 
 			pathplanner_active = true;
+			last_index = waypointActive.Index;
 
 			continue;
 		}
@@ -159,6 +156,9 @@ static void pathPlannerTask(void *parameters)
 		WaypointActiveGet(&waypointActive);
 		WaypointInstGet(waypointActive.Index, &waypoint);
 
+		if(last_index != waypointActive.Index)
+			updatePositionDesired();
+
 		float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
 		     powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
 			powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2);
@@ -170,16 +170,9 @@ static void pathPlannerTask(void *parameters)
 					waypointActive.Index++;
 					WaypointActiveSet(&waypointActive);
 					
-					if(WaypointInstGet(waypointActive.Index, &waypoint) != 0) {
-						// Oh shit, tried to go to non-existant waypoint
-						continue;
-					}
+					updatePositionDesired();
+					last_index = waypointActive.Index;
 
-					PositionDesiredGet(&positionDesired);
-					positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
-					positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
-					positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
-					PositionDesiredSet(&positionDesired);
 					break;
 				case WAYPOINT_ACTION_RTH:
 					// Fly back to the home location but 20 m above it
@@ -196,6 +189,21 @@ static void pathPlannerTask(void *parameters)
 	}
 }
 
+static void updatePositionDesired()
+{
+	PositionDesiredData positionDesired;
+	WaypointActiveData waypointActive;
+	WaypointData waypoint;
+
+	WaypointInstGet(waypointActive.Index, &waypoint);
+	
+	PositionDesiredGet(&positionDesired);
+	positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
+	positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
+	positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
+	PositionDesiredSet(&positionDesired);
+}
+
 /**
   * @}
   * @}