Add High-Level Commander, see #293

The initial high-level commander supports takeoff, landing, stop, and goTo. Each CF can be part of one of 8 groups, and commands can be broadcasted for specific groups, only.
Tested in a mocap system with kalman estimator and mellinger controller using crazyflie_ros (high-level-setpoints branch; test_high_level.py).

Makefile

@@ -151,6 +151,8 @@ PROJ_OBJ += controller_$(CONTROLLER).o
 PROJ_OBJ += power_distribution_$(POWER_DISTRIBUTION).o
 PROJ_OBJ_CF2 += estimator_kalman.o
 
+# High-Level Commander
+PROJ_OBJ += crtp_commander_high_level.o planner.o pptraj.o
 
 # Deck Core
 PROJ_OBJ_CF2 += deck.o deck_info.o deck_drivers.o deck_test.o

src/config/config.h

@@ -91,6 +91,7 @@
 #define USDLOG_TASK_PRI         1
 #define USDWRITE_TASK_PRI       0
 #define PCA9685_TASK_PRI        3
+#define CMD_HIGH_LEVEL_TASK_PRI 2
 
 #define SYSLINK_TASK_PRI        5
 #define USBLINK_TASK_PRI        3
@@ -125,6 +126,7 @@
 #define USDLOG_TASK_NAME        "USDLOG"
 #define USDWRITE_TASK_NAME      "USDWRITE"
 #define PCA9685_TASK_NAME       "PCA9685"
+#define CMD_HIGH_LEVEL_TASK_NAME "CMDHL"
 
 //Task stack sizes
 #define SYSTEM_TASK_STACKSIZE         (2* configMINIMAL_STACK_SIZE)
@@ -149,6 +151,7 @@
 #define USDLOG_TASK_STACKSIZE         (2 * configMINIMAL_STACK_SIZE)
 #define USDWRITE_TASK_STACKSIZE       (2 * configMINIMAL_STACK_SIZE)
 #define PCA9685_TASK_STACKSIZE        (2 * configMINIMAL_STACK_SIZE)
+#define CMD_HIGH_LEVEL_TASK_STACKSIZE configMINIMAL_STACK_SIZE
 
 //The radio channel. From 0 to 125
 #define RADIO_CHANNEL 80

src/modules/interface/crtp.h

@@ -44,6 +44,7 @@ typedef enum {
   CRTP_PORT_LOG              = 0x05,
   CRTP_PORT_LOCALIZATION     = 0x06,
   CRTP_PORT_SETPOINT_GENERIC = 0x07,
+  CRTP_PORT_SETPOINT_HL      = 0x08,
   CRTP_PORT_PLATFORM         = 0x0D,
   CRTP_PORT_LINK             = 0x0F,
 } CRTPPort;

src/modules/interface/crtp_commander_high_level.h

@@ -0,0 +1,62 @@
+/*
+ *    ______
+ *   / ____/________ _____  __  ________      ______ __________ ___
+ *  / /   / ___/ __ `/_  / / / / / ___/ | /| / / __ `/ ___/ __ `__ \
+ * / /___/ /  / /_/ / / /_/ /_/ (__  )| |/ |/ / /_/ / /  / / / / / /
+ * \____/_/   \__,_/ /___/\__, /____/ |__/|__/\__,_/_/  /_/ /_/ /_/
+ *                       /____/
+ *
+ * Crazyswarm advanced control firmware for Crazyflie
+ *
+
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+/*
+Header file for high-level commander that computes smooth setpoints based on high-level inputs.
+*/
+
+#ifndef CRTP_COMMANDER_HIGH_LEVEL_H_
+#define CRTP_COMMANDER_HIGH_LEVEL_H_
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "math3d.h"
+
+#include "stabilizer_types.h"
+
+/* Public functions */
+void crtpCommanderHighLevelInit(void);
+
+// Retrieves the current setpoint
+void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state);
+
+// Tell the trajectory planner that it should cut power.
+// Should be used if an emergency is detected.
+void crtpCommanderHighLevelStop();
+
+// True if we have landed or emergency-stopped.
+bool crtpCommanderHighLevelIsStopped();
+
+#endif /* CRTP_COMMANDER_HIGH_LEVEL_H_ */

src/modules/interface/planner.h

@@ -0,0 +1,81 @@
+/*
+ *    ______
+ *   / ____/________ _____  __  ________      ______ __________ ___
+ *  / /   / ___/ __ `/_  / / / / / ___/ | /| / / __ `/ ___/ __ `__ \
+ * / /___/ /  / /_/ / / /_/ /_/ (__  )| |/ |/ / /_/ / /  / / / / / /
+ * \____/_/   \__,_/ /___/\__, /____/ |__/|__/\__,_/_/  /_/ /_/ /_/
+ *                       /____/
+ *
+ * Crazyswarm advanced control firmware for Crazyflie
+ *
+
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+/*
+Header file for planning state machine
+*/
+
+#pragma once
+
+#include "math3d.h"
+#include "pptraj.h"
+
+enum trajectory_state
+{
+	TRAJECTORY_STATE_IDLE            = 0,
+	TRAJECTORY_STATE_FLYING          = 1,
+	TRAJECTORY_STATE_LANDING         = 3,
+};
+
+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
+};
+
+// initialize the planner
+void plan_init(struct planner *p);
+
+// tell the planner to stop.
+// subsequently, plan_is_stopped(p) will return true,
+// and it is no longer valid to call plan_current_goal(p).
+void plan_stop(struct planner *p);
+
+// query if the planner is stopped.
+// currently this is true at startup before we take off,
+// and also after an emergency stop.
+bool plan_is_stopped(struct planner *p);
+
+// get the planner's current goal.
+struct traj_eval plan_current_goal(struct planner *p, float t);
+
+// start a takeoff trajectory.
+int plan_takeoff(struct planner *p, struct vec pos, float yaw, float height, float duration, float t);
+
+// start a landing trajectory.
+int plan_land(struct planner *p, struct vec pos, float yaw, float height, float duration, float t);
+
+// move to a given position, then hover there.
+int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t);

src/modules/interface/pptraj.h

@@ -0,0 +1,182 @@
+/*
+ *    ______
+ *   / ____/________ _____  __  ________      ______ __________ ___
+ *  / /   / ___/ __ `/_  / / / / / ___/ | /| / / __ `/ ___/ __ `__ \
+ * / /___/ /  / /_/ / / /_/ /_/ (__  )| |/ |/ / /_/ / /  / / / / / /
+ * \____/_/   \__,_/ /___/\__, /____/ |__/|__/\__,_/_/  /_/ /_/ /_/
+ *                       /____/
+ *
+ * Crazyswarm advanced control firmware for Crazyflie
+ *
+
+The MIT License (MIT)
+
+Copyright (c) 2018 Wolfgang Hoenig and James Alan Preiss
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+*/
+
+/*
+Header file for piecewise polynomial trajectories
+*/
+
+#pragma once
+
+#include "math3d.h"
+
+#define PP_DEGREE (7)
+#define PP_SIZE (PP_DEGREE + 1)
+#define PP_MAX_PIECES (30)
+
+
+//
+// 1d polynomial functions.
+// coefficients are stored in ascending order, i.e. p[0] is the constant term.
+//
+
+// evaluate a polynomial using horner's rule.
+float polyval(float const p[PP_SIZE], float t);
+
+// construct a linear polynomial from p(0) = x0 to p(duration) = x1.
+void polylinear(float p[PP_SIZE], float duration, float x0, float x1);
+
+// plan a degree-5 polynomial with the given duration T,
+// and given initial/final position, velocity, and acceleration
+void poly5(float poly[PP_SIZE], float T,
+	float x0, float dx0, float ddx0,
+	float xf, float dxf, float ddxf);
+
+// scale a polynomial in place.
+void polyscale(float p[PP_SIZE], float s);
+
+// compute the derivate of a polynomial in place.
+void polyder(float p[PP_SIZE]);
+
+// e.g. if s==2 the new polynomial will be stretched to take 2x longer
+void polystretchtime(float p[PP_SIZE], float s);
+
+// reflect a polynomial about the x-axis, e.g. p_after(-x) == p_before(x)
+void polyreflect(float p[PP_SIZE]);
+
+
+//
+// 4d single polynomial piece for x-y-z-yaw, includes duration.
+//
+
+struct poly4d
+{
+	float p[4][PP_SIZE];
+	float duration; // TODO use int millis instead?
+};
+
+// construct a 4d zero polynomial.
+struct poly4d poly4d_zero(float duration);
+
+// construct a 4d linear polynomial.
+struct poly4d poly4d_linear(float duration,
+	struct vec p0, struct vec p1, float yaw0, float yaw1);
+
+// scale a 4d polynomial in-place.
+void poly4d_scale(struct poly4d *p, float x, float y, float z, float yaw);
+
+// shift a 4d polynomial by the given values in-place.
+void poly4d_shift(struct poly4d *p, float x, float y, float z, float yaw);
+static inline void poly4d_shift_vec(struct poly4d *p, struct vec pos, float yaw) {
+	poly4d_shift(p, pos.x, pos.y, pos.z, yaw);
+}
+
+// e.g. if s==2 the new polynomial will be stretched to take 2x longer.
+void poly4d_stretchtime(struct poly4d *p, float s);
+
+// compute the derivative of a 4d polynomial in-place.
+void polyder4d(struct poly4d *p);
+
+// compute loose maximum of acceleration -
+// uses L1 norm instead of Euclidean, evaluates polynomial instead of root-finding
+float poly4d_max_accel_approx(struct poly4d const *p);
+
+
+// output of differentially flat 4d polynomials.
+struct traj_eval
+{
+	struct vec pos;
+	struct vec vel;
+	struct vec acc;
+	struct vec omega;
+	float yaw;
+};
+
+// a special value of traj_eval that indicates an invalid result.
+struct traj_eval traj_eval_invalid(void);
+
+// check if a traj_eval represents an invalid result.
+bool is_traj_eval_valid(struct traj_eval const *ev);
+
+// evaluate a single polynomial piece
+struct traj_eval poly4d_eval(struct poly4d const *p, float t);
+
+
+
+// ----------------------------------//
+// piecewise polynomial trajectories //
+// ----------------------------------//
+
+struct piecewise_traj
+{
+	struct poly4d pieces[PP_MAX_PIECES];
+	float t_begin;
+	unsigned char n_pieces;
+};
+
+static inline float piecewise_duration(struct piecewise_traj const *pp)
+{
+	float total_dur = 0;
+	for (int i = 0; i < pp->n_pieces; ++i) {
+		total_dur += pp->pieces[i].duration;
+	}
+	return total_dur;
+}
+
+void piecewise_plan_5th_order(struct piecewise_traj *p, float duration,
+	struct vec p0, float y0, struct vec v0, float dy0, struct vec a0,
+	struct vec p1, float y1, struct vec v1, float dy1, struct vec a1);
+
+void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *p, float duration,
+	struct vec p0, float y0, struct vec v0, float dy0, struct vec a0,
+	struct vec p1, float y1, struct vec v1, float dy1, struct vec a1);
+
+struct traj_eval piecewise_eval(
+	struct piecewise_traj const *traj, float t);
+
+struct traj_eval piecewise_eval_reversed(
+	struct piecewise_traj const *traj, float t);
+
+void piecewise_scale(struct piecewise_traj *pp, float x, float y, float z, float yaw);
+
+void piecewise_shift(struct piecewise_traj *pp, float x, float y, float z, float yaw);
+static inline void piecewise_shift_vec(struct piecewise_traj *pp, struct vec pos, float yaw) {
+	piecewise_shift(pp, pos.x, pos.y, pos.z, yaw);
+}
+
+void piecewise_stretchtime(struct piecewise_traj *pp, float s);
+
+static inline bool piecewise_is_finished(struct piecewise_traj const *traj, float t)
+{
+	return (t - traj->t_begin) >= piecewise_duration(traj);
+}