High-level commander: trajectory upload and exec.

This adds the possibility to upload polynomial trajectories using the existing memory interface.
Trajectories can be executed with the high-level commander.
Tested with crazyflie_ros (test_high_level.py).
See issue bitcraze#293.

src/modules/interface/crtp_commander_high_level.h

@@ -46,6 +46,12 @@ Header file for high-level commander that computes smooth setpoints based on hig
 
 #include "stabilizer_types.h"
 
+// allocate memory to store trajectories
+// 4k allows us to store 31 poly4d pieces
+// other (compressed) formats might be added in the future
+#define TRAJECTORY_MEMORY_SIZE 4096
+extern uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE];
+
 /* Public functions */
 void crtpCommanderHighLevelInit(void);
 

src/modules/interface/planner.h

@@ -79,3 +79,6 @@ int plan_land(struct planner *p, struct vec pos, float yaw, float height, float
 
 // 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);
+
+// start trajectory
+int plan_start_trajectory(struct planner *p, const struct piecewise_traj* trajectory, bool reversed);

src/modules/interface/pptraj.h

@@ -42,7 +42,6 @@ Header file for piecewise polynomial trajectories
 
 #define PP_DEGREE (7)
 #define PP_SIZE (PP_DEGREE + 1)
-#define PP_MAX_PIECES (30)
 
 
 //
@@ -83,7 +82,7 @@ struct poly4d
 {
 	float p[4][PP_SIZE];
 	float duration; // TODO use int millis instead?
-};
+} __attribute__((packed));
 
 // construct a 4d zero polynomial.
 struct poly4d poly4d_zero(float duration);
@@ -139,9 +138,11 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t);
 
 struct piecewise_traj
 {
-	struct poly4d pieces[PP_MAX_PIECES];
 	float t_begin;
+	float timescale;
+	struct vec shift;
 	unsigned char n_pieces;
+	struct poly4d* pieces;
 };
 
 static inline float piecewise_duration(struct piecewise_traj const *pp)
@@ -150,7 +151,7 @@ static inline float piecewise_duration(struct piecewise_traj const *pp)
 	for (int i = 0; i < pp->n_pieces; ++i) {
 		total_dur += pp->pieces[i].duration;
 	}
-	return total_dur;
+	return total_dur * pp->timescale;
 }
 
 void piecewise_plan_5th_order(struct piecewise_traj *p, float duration,
@@ -167,14 +168,6 @@ struct traj_eval piecewise_eval(
 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)
 {

src/modules/src/crtp_commander_high_level.c

@@ -56,11 +56,14 @@ such as: take-off, landing, polynomial trajectories.
 #include "param.h"
 
 // Global variables
+uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE];
+
 static bool isInit = false;
 static struct planner planner;
 static uint8_t group_mask;
 static struct vec pos; // last known state (position [m])
 static float yaw; // last known state (yaw [rad])
+static struct piecewise_traj trajectory;
 
 // makes sure that we don't evaluate the trajectory while it is being changed
 static xSemaphoreHandle lockTraj;
@@ -74,6 +77,7 @@ enum TrajectoryCommand_e {
   COMMAND_LAND                    = 2,
   COMMAND_STOP                    = 3,
   COMMAND_GO_TO                   = 4,
+  COMMAND_START_TRAJECTORY        = 5,
 };
 
 struct data_set_group_mask {
@@ -110,6 +114,33 @@ struct data_go_to {
   float duration; // sec
 } __attribute__((packed));
 
+enum TrajectoryLocation_e {
+  TRAJECTORY_LOCATION_MEM = 0, // for trajectories that are uploaded dynamically
+  // Future features might include trajectories on flash or uSD card
+};
+
+enum TrajectoryType_e {
+  TRAJECTORY_TYPE_POLY4D = 0, // struct poly4d, see pptraj.h
+  // Future types might include versions without yaw
+};
+
+// starts executing a specified trajectory
+struct data_start_trajectory {
+  uint8_t groupMask; // mask for which CFs this should apply to
+  uint8_t relative;  // set to true, if trajectory should be shifted to current setpoint
+  uint8_t reversed;  // set to true, if trajectory should be executed in reverse
+  uint8_t trajectoryLocation; // one of TrajectoryLocation_e
+  uint8_t trajectoryType;     // one of TrajectoryType_e
+  union
+  {
+    struct {
+      uint32_t offset;  // offset in uploaded memory
+      uint8_t n_pieces;
+    } __attribute__((packed)) mem; // if trajectoryLocation is TRAJECTORY_LOCATION_MEM
+  } trajectoryIdentifier;
+  float timescale; // time factor; 1 = original speed; >1: slower; <1: faster
+} __attribute__((packed));
+
 // Private functions
 static void crtpCommanderHighLevelTask(void * prm);
 
@@ -118,6 +149,7 @@ static int takeoff(const struct data_takeoff* data);
 static int land(const struct data_land* data);
 static int stop(const struct data_stop* data);
 static int go_to(const struct data_go_to* data);
+static int start_trajectory(const struct data_start_trajectory* data);
 
 // Helper functions
 static struct vec state2vec(struct vec3_s v)
@@ -147,7 +179,6 @@ void crtpCommanderHighLevelInit(void)
   yaw = 0;
 
   isInit = true;
-  DEBUG_PRINT("traj. initialized.\n");
 }
 
 void crtpCommanderHighLevelStop()
@@ -224,6 +255,9 @@ void crtpCommanderHighLevelTask(void * prm)
       case COMMAND_GO_TO:
         ret = go_to((const struct data_go_to*)&p.data[1]);
         break;
+      case COMMAND_START_TRAJECTORY:
+        ret = start_trajectory((const struct data_start_trajectory*)&p.data[1]);
+        break;
       default:
         ret = ENOEXEC;
         break;
@@ -290,3 +324,36 @@ int go_to(const struct data_go_to* data)
   }
   return result;
 }
+
+int start_trajectory(const struct data_start_trajectory* data)
+{
+  int result = 0;
+  if (isInGroup(data->groupMask)
+    && data->trajectoryLocation == TRAJECTORY_LOCATION_MEM
+    && data->trajectoryType == TRAJECTORY_TYPE_POLY4D) {
+    xSemaphoreTake(lockTraj, portMAX_DELAY);
+    float t = usecTimestamp() / 1e6;
+    trajectory.t_begin = t;
+    trajectory.timescale = data->timescale;
+    trajectory.n_pieces = data->trajectoryIdentifier.mem.n_pieces;
+    trajectory.pieces = (struct poly4d*)&trajectories_memory[data->trajectoryIdentifier.mem.offset];
+    if (data->relative) {
+      trajectory.shift = vzero();
+      struct traj_eval traj_init;
+      if (data->reversed) {
+        traj_init = piecewise_eval_reversed(&trajectory, trajectory.t_begin);
+      }
+      else {
+        traj_init = piecewise_eval(&trajectory, trajectory.t_begin);
+      }
+      struct vec shift_pos = vsub(pos, traj_init.pos);
+      trajectory.shift = shift_pos;
+    } else {
+      trajectory.shift = vzero();
+    }
+
+    result = plan_start_trajectory(&planner, &trajectory, data->reversed);
+    xSemaphoreGive(lockTraj);
+  }
+  return result;
+}

src/modules/src/mem_cf2.c

@@ -43,6 +43,7 @@
 
 #include "ledring12.h"
 #include "locodeck.h"
+#include "crtp_commander_high_level.h"
 
 #include "console.h"
 #include "assert.h"
@@ -65,14 +66,16 @@
 #define EEPROM_ID       0x00
 #define LEDMEM_ID       0x01
 #define LOCO_ID         0x02
-#define OW_FIRST_ID     0x03
+#define TRAJ_ID         0x03
+#define OW_FIRST_ID     0x04
 
 #define STATUS_OK 0
 
 #define MEM_TYPE_EEPROM 0x00
 #define MEM_TYPE_OW     0x01
 #define MEM_TYPE_LED12  0x10
 #define MEM_TYPE_LOCO   0x11
+#define MEM_TYPE_TRAJ   0x12
 
 #define MEM_LOCO_INFO             0x0000
 #define MEM_LOCO_ANCHOR_BASE      0x1000
@@ -191,6 +194,9 @@ void createInfoResponse(CRTPPacket* p, uint8_t memId)
     case LOCO_ID:
       createInfoResponseBody(p, MEM_TYPE_LOCO, MEM_LOCO_ANCHOR_BASE + MEM_LOCO_ANCHOR_PAGE_SIZE * LOCODECK_NR_OF_ANCHORS, noData);
       break;
+    case TRAJ_ID:
+      createInfoResponseBody(p, MEM_TYPE_TRAJ, sizeof(trajectories_memory), noData);
+      break;
     default:
       if (owGetinfo(memId - OW_FIRST_ID, &serialNbr))
       {
@@ -252,6 +258,17 @@ void memReadProcess()
       status = handleLocoMemRead(memAddr, readLen, &p.data[6]);
       break;
 
+    case TRAJ_ID:
+      {
+        if (memAddr + readLen <= sizeof(trajectories_memory) &&
+            memcpy(&p.data[6], &(trajectories_memory[memAddr]), readLen)) {
+          status = STATUS_OK;
+        } else {
+          status = EIO;
+        }
+      }
+      break;
+
     default:
       {
         memId = memId - OW_FIRST_ID;
@@ -368,6 +385,17 @@ void memWriteProcess()
       status = EIO;
       break;
 
+    case TRAJ_ID:
+      {
+        if ((memAddr + writeLen) <= sizeof(trajectories_memory)) {
+          memcpy(&(trajectories_memory[memAddr]), &p.data[5], writeLen);
+          status = STATUS_OK;
+        } else {
+          status = EIO;
+        }
+      }
+      break;
+
     default:
       {
         memId = memId - OW_FIRST_ID;

src/modules/src/planner.c

@@ -39,6 +39,7 @@ implementation of planning state machine
 #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)
 {
@@ -59,6 +60,7 @@ void plan_init(struct planner *p)
 	p->state = TRAJECTORY_STATE_IDLE;
 	p->reversed = false;
 	p->trajectory = NULL;
+	planned_trajectory.pieces = pieces;
 }
 
 void plan_stop(struct planner *p)
@@ -143,3 +145,12 @@ int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hov
 	p->trajectory = &planned_trajectory;
 	return 0;
 }
+
+int plan_start_trajectory( struct planner *p, const struct piecewise_traj* trajectory, bool reversed)
+{
+	p->reversed = reversed;
+	p->trajectory = trajectory;
+	p->state = TRAJECTORY_STATE_FLYING;
+
+	return 0;
+}