From eb7e0f379877cb54766a564a25d4969b2f8a78ee Mon Sep 17 00:00:00 2001
From: Wolfgang Hoenig <whoenig@usc.edu>
Date: Fri, 27 Sep 2019 11:39:38 -0700
Subject: [PATCH] Refactor planner to avoid global static members

This simplifies using the same code in other C-projects as well
as creating Python bindings using SWIG. The latter is used in the
Crazyswarm simulator for more accurate (and efficient) simulation.
---
 src/modules/interface/planner.h |  3 +++
 src/modules/src/planner.c       | 23 ++++++++++-------------
 src/modules/src/pptraj.c        |  2 +-
 3 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/src/modules/interface/planner.h b/src/modules/interface/planner.h
index 4f59df7192..f04f423342 100644
--- a/src/modules/interface/planner.h
+++ b/src/modules/interface/planner.h
@@ -53,6 +53,9 @@ struct planner
 	enum trajectory_state state;	// current state
 	bool reversed;					// true, if trajectory should be evaluated in reverse
 	const struct piecewise_traj* trajectory; // pointer to trajectory
+
+	struct piecewise_traj planned_trajectory; // trajectory for on-board planning
+	struct poly4d pieces[1]; // the on-board planner requires a single piece, only
 };
 
 // initialize the planner
diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c
index 1cdf017ffc..595a2d71ab 100644
--- a/src/modules/src/planner.c
+++ b/src/modules/src/planner.c
@@ -35,18 +35,15 @@ SOFTWARE.
 /*
 implementation of planning state machine
 */
-
+#include <stddef.h>
 #include "planner.h"
 
-static struct piecewise_traj planned_trajectory;
-static struct poly4d pieces[1]; // the on-board planner requires a single piece, only
-
 static void plan_takeoff_or_landing(struct planner *p, struct vec pos, float yaw, float height, float duration)
 {
 	struct vec takeoff_pos = pos;
 	takeoff_pos.z = height;
 
-	piecewise_plan_7th_order_no_jerk(&planned_trajectory, duration,
+	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
 		pos,         yaw, vzero(), 0, vzero(),
 		takeoff_pos,   0, vzero(), 0, vzero());
 }
@@ -60,7 +57,7 @@ void plan_init(struct planner *p)
 	p->state = TRAJECTORY_STATE_IDLE;
 	p->reversed = false;
 	p->trajectory = NULL;
-	planned_trajectory.pieces = pieces;
+	p->planned_trajectory.pieces = p->pieces;
 }
 
 void plan_stop(struct planner *p)
@@ -104,8 +101,8 @@ int plan_takeoff(struct planner *p, struct vec pos, float yaw, float height, flo
 	plan_takeoff_or_landing(p, pos, yaw, height, duration);
 	p->reversed = false;
 	p->state = TRAJECTORY_STATE_FLYING;
-	planned_trajectory.t_begin = t;
-	p->trajectory = &planned_trajectory;
+	p->planned_trajectory.t_begin = t;
+	p->trajectory = &p->planned_trajectory;
 	return 0;
 }
 
@@ -119,8 +116,8 @@ int plan_land(struct planner *p, struct vec pos, float yaw, float height, float
 	plan_takeoff_or_landing(p, pos, yaw, height, duration);
 	p->reversed = false;
 	p->state = TRAJECTORY_STATE_LANDING;
-	planned_trajectory.t_begin = t;
-	p->trajectory = &planned_trajectory;
+	p->planned_trajectory.t_begin = t;
+	p->trajectory = &p->planned_trajectory;
 	return 0;
 }
 
@@ -135,14 +132,14 @@ int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hov
 		hover_yaw += setpoint.yaw;
 	}
 
-	piecewise_plan_7th_order_no_jerk(&planned_trajectory, duration,
+	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
 		setpoint.pos, setpoint.yaw, setpoint.vel, setpoint.omega.z, setpoint.acc,
 		hover_pos,    hover_yaw,    vzero(),      0,                vzero());
 
 	p->reversed = false;
 	p->state = TRAJECTORY_STATE_FLYING;
-	planned_trajectory.t_begin = t;
-	p->trajectory = &planned_trajectory;
+	p->planned_trajectory.t_begin = t;
+	p->trajectory = &p->planned_trajectory;
 	return 0;
 }
 
diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c
index 5b45699b50..be434afca6 100644
--- a/src/modules/src/pptraj.c
+++ b/src/modules/src/pptraj.c
@@ -275,7 +275,7 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t)
 	// float thrust_mag = mass * vmag(thrust);
 
 	struct vec z_body = vnormalize(thrust);
-	struct vec x_world = mkvec(cos(out.yaw), sin(out.yaw), 0);
+	struct vec x_world = mkvec(cosf(out.yaw), sinf(out.yaw), 0);
 	struct vec y_body = vnormalize(vcross(z_body, x_world));
 	struct vec x_body = vcross(y_body, z_body);