notifySetpointsStop packet to re-enable high level commander

src/modules/interface/commander.h

@@ -46,6 +46,13 @@ uint32_t commanderGetInactivityTime(void);
 void commanderSetSetpoint(setpoint_t *setpoint, int priority);
 int commanderGetActivePriority(void);
 
+/* Inform the commander that streaming setpoints are about to stop.
+ * Parameter controls the amount of time the last setpoint will remain valid.
+ * This gives the PC time to send the next command, e.g. with the high-level
+ * commander, before we enter timeout mode.
+ */
+void commanderNotifySetpointsStop(int remainValidMillisecs);
+
 void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state);
 
 #endif /* COMMANDER_H_ */

src/modules/interface/crtp_commander_high_level.h

@@ -60,8 +60,16 @@ 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.
+// When flying sequences of high-level commands, the high-level commander uses
+// its own history of commands to determine the initial conditions of the next
+// trajectory it plans. However, when switching from a low-level streaming
+// setpoint mode to high-level, any past command history is invalid because of
+// the intervening low-level commands. Therefore, we must tell the high-level
+// commander what initial conditions to use for trajectory planning.
+void crtpCommanderHighLevelTellState(const state_t *state);
+
+// Send the trajectory planner to idle state, where it has no plan. Used when
+// switching from high-level to low-level commands, or for emergencies.
 void crtpCommanderHighLevelStop();
 
 // True if we have landed or emergency-stopped.

src/modules/interface/planner.h

@@ -95,6 +95,9 @@ int plan_land(struct planner *p, struct vec curr_pos, float curr_yaw, float hove
 // 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);
 
+// same as above, but with current state provided from outside.
+int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, 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/src/commander.c

@@ -33,11 +33,14 @@
 #include "crtp_commander.h"
 #include "crtp_commander_high_level.h"
 
+#include "cf_math.h"
 #include "param.h"
 #include "static_mem.h"
 
 static bool isInit;
 const static setpoint_t nullSetpoint;
+static setpoint_t tempSetpoint;
+static state_t lastState;
 const static int priorityDisable = COMMANDER_PRIORITY_DISABLE;
 
 static uint32_t lastUpdate;
@@ -78,9 +81,25 @@ void commanderSetSetpoint(setpoint_t *setpoint, int priority)
     // This is a potential race but without effect on functionality
     xQueueOverwrite(setpointQueue, setpoint);
     xQueueOverwrite(priorityQueue, &priority);
+    // Send the high-level planner to idle so it will forget its current state
+    // and start over if we switch from low-level to high-level in the future.
+    crtpCommanderHighLevelStop();
   }
 }
 
+void commanderNotifySetpointsStop(int remainValidMillisecs)
+{
+  uint32_t currentTime = xTaskGetTickCount();
+  int timeSetback = MIN(
+    COMMANDER_WDT_TIMEOUT_SHUTDOWN - M2T(remainValidMillisecs),
+    currentTime
+  );
+  xQueuePeek(setpointQueue, &tempSetpoint, 0);
+  tempSetpoint.timestamp = currentTime - timeSetback;
+  xQueueOverwrite(setpointQueue, &tempSetpoint);
+  crtpCommanderHighLevelTellState(&lastState);
+}
+
 void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
 {
   xQueuePeek(setpointQueue, setpoint, 0);
@@ -107,6 +126,10 @@ void commanderGetSetpoint(setpoint_t *setpoint, const state_t *state)
     setpoint->attitudeRate.yaw = 0;
     // Keep Z as it is
   }
+  // This copying is not strictly necessary because stabilizer.c already keeps
+  // a static state_t containing the most recent state estimate. However, it is
+  // not accessible by the public interface.
+  lastState = *state;
 }
 
 bool commanderTest(void)

src/modules/src/crtp_commander.c

@@ -27,6 +27,7 @@
 
 #include "crtp_commander.h"
 
+#include "cfassert.h"
 #include "commander.h"
 #include "crtp.h"
 
@@ -47,16 +48,87 @@ void crtpCommanderInit(void)
   isInit = true;
 }
 
+enum crtpSetpointGenericChannel {
+  SET_SETPOINT_CHANNEL = 0,
+  META_COMMAND_CHANNEL = 1,
+};
 
+/* Channel 1 of the generic commander port is used for "meta-commands"
+ * that alter the behavior of the commander itself, e.g. mode switching.
+ * Although we use the generic commander port due to increasing pressure on the
+ * 4-bit space of ports numbers, meta-commands that are unrelated to
+ * streaming generic setpoint control modes are permitted.
+ *
+ * The packet format for meta-commands is:
+ * +------+==========================+
+ * | TYPE |     DATA                 |
+ * +------+==========================+
+ *
+ * TYPE is an 8-bit value. The remainder of the data depends on the command.
+ * The maximum data size is 29 bytes.
+ */
+
+/* To add a new packet:
+ *   1 - Add a new type in the metaCommand_e enum.
+ *   2 - Implement a decoder function with good documentation about the data
+ *       structure and the intent of the packet.
+ *   3 - Add the decoder function to the metaCommandDecoders array.
+ *   4 - Create a new params group for your handler if necessary
+ *   5 - Pull-request your change :-)
+ */
+
+/* ---===== 1 - metaCommand_e enum =====--- */
+enum metaCommand_e {
+  metaNotifySetpointsStop = 0,
+  nMetaCommands,
+};
+
+typedef void (*metaCommandDecoder_t)(const void *data, size_t datalen);
+
+/* ---===== 2 - Decoding functions =====--- */
+
+/* notifySetpointsStop meta-command. See commander.h function
+ * commanderNotifySetpointsStop() for description and motivation.
+ */
+struct notifySetpointsStopPacket {
+  uint32_t remainValidMillisecs;
+} __attribute__((packed));
+void notifySetpointsStopDecoder(const void *data, size_t datalen)
+{
+  ASSERT(datalen == sizeof(struct notifySetpointsStopPacket));
+  const struct notifySetpointsStopPacket *values = data;
+  commanderNotifySetpointsStop(values->remainValidMillisecs);
+}
+
+ /* ---===== packetDecoders array =====--- */
+const static metaCommandDecoder_t metaCommandDecoders[] = {
+  [metaNotifySetpointsStop] = notifySetpointsStopDecoder,
+};
+
+/* Decoder switch */
 static void commanderCrtpCB(CRTPPacket* pk)
 {
   static setpoint_t setpoint;
 
   if(pk->port == CRTP_PORT_SETPOINT && pk->channel == 0) {
     crtpCommanderRpytDecodeSetpoint(&setpoint, pk);
     commanderSetSetpoint(&setpoint, COMMANDER_PRIORITY_CRTP);
-  } else if (pk->port == CRTP_PORT_SETPOINT_GENERIC && pk->channel == 0) {
-    crtpCommanderGenericDecodeSetpoint(&setpoint, pk);
-    commanderSetSetpoint(&setpoint, COMMANDER_PRIORITY_CRTP);
+  } else if (pk->port == CRTP_PORT_SETPOINT_GENERIC) {
+    switch (pk->channel) {
+    case SET_SETPOINT_CHANNEL:
+      crtpCommanderGenericDecodeSetpoint(&setpoint, pk);
+      commanderSetSetpoint(&setpoint, COMMANDER_PRIORITY_CRTP);
+      break;
+    case META_COMMAND_CHANNEL: {
+        uint8_t metaCmd = pk->data[0];
+        if (metaCmd < nMetaCommands && (metaCommandDecoders[metaCmd] != NULL)) {
+          metaCommandDecoders[metaCmd](pk->data + 1, pk->size - 1);
+        }
+      }
+      break;
+    default:
+      /* Do nothing */
+      break;
+    }
   }
 }

src/modules/src/crtp_commander_high_level.c

@@ -50,7 +50,6 @@ such as: take-off, landing, polynomial trajectories.
 // Crazyswarm includes
 #include "crtp.h"
 #include "crtp_commander_high_level.h"
-#include "debug.h"
 #include "planner.h"
 #include "log.h"
 #include "param.h"
@@ -90,6 +89,7 @@ static bool isInit = false;
 static struct planner planner;
 static uint8_t group_mask;
 static struct vec pos; // last known setpoint (position [m])
+static struct vec vel; // last known setpoint (velocity [m/s])
 static float yaw; // last known setpoint yaw (yaw [rad])
 static struct piecewise_traj trajectory;
 static struct piecewise_traj_compressed  compressed_trajectory;
@@ -221,21 +221,33 @@ void crtpCommanderHighLevelInit(void)
   lockTraj = xSemaphoreCreateMutexStatic(&lockTrajBuffer);
 
   pos = vzero();
+  vel = vzero();
   yaw = 0;
 
   isInit = true;
 }
 
 void crtpCommanderHighLevelStop()
 {
+  xSemaphoreTake(lockTraj, portMAX_DELAY);
   plan_stop(&planner);
+  xSemaphoreGive(lockTraj);
 }
 
 bool crtpCommanderHighLevelIsStopped()
 {
   return plan_is_stopped(&planner);
 }
 
+void crtpCommanderHighLevelTellState(const state_t *state)
+{
+  xSemaphoreTake(lockTraj, portMAX_DELAY);
+  pos = state2vec(state->position);
+  vel = state2vec(state->velocity);
+  yaw = radians(state->attitude.yaw);
+  xSemaphoreGive(lockTraj);
+}
+
 void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state)
 {
   xSemaphoreTake(lockTraj, portMAX_DELAY);
@@ -250,6 +262,7 @@ void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
   // if we are on the ground, update the last setpoint with the current state estimate
   if (plan_is_stopped(&planner)) {
     pos = state2vec(state->position);
+    vel = state2vec(state->velocity);
     yaw = radians(state->attitude.yaw);
   }
 
@@ -277,6 +290,7 @@ void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
 
     // store the last setpoint
     pos = ev.pos;
+    vel = ev.vel;
     yaw = ev.yaw;
   }
 }
@@ -411,12 +425,26 @@ int stop(const struct data_stop* data)
 
 int go_to(const struct data_go_to* data)
 {
+  static struct traj_eval ev = {
+    // pos, vel, yaw will be filled before using
+    .acc = {0.0f, 0.0f, 0.0f},
+    .omega = {0.0f, 0.0f, 0.0f},
+  };
+
   int result = 0;
   if (isInGroup(data->groupMask)) {
     struct vec hover_pos = mkvec(data->x, data->y, data->z);
     xSemaphoreTake(lockTraj, portMAX_DELAY);
     float t = usecTimestamp() / 1e6;
-    result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t);
+    if (plan_is_stopped(&planner)) {
+      ev.pos = pos;
+      ev.vel = vel;
+      ev.yaw = yaw;
+      result = plan_go_to_from(&planner, &ev, data->relative, hover_pos, data->yaw, data->duration, t);
+    }
+    else {
+      result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t);
+    }
     xSemaphoreGive(lockTraj);
   }
   return result;

src/modules/src/planner.c

@@ -148,8 +148,7 @@ int plan_takeoff(struct planner *p, struct vec curr_pos, float curr_yaw, float h
 
 int plan_land(struct planner *p, struct vec curr_pos, float curr_yaw, float hover_height, float hover_yaw, float duration, float t)
 {
-	if (   p->state == TRAJECTORY_STATE_IDLE
-		|| p->state == TRAJECTORY_STATE_LANDING) {
+	if (p->state == TRAJECTORY_STATE_LANDING) {
 		return 1;
 	}
 
@@ -162,20 +161,16 @@ int plan_land(struct planner *p, struct vec curr_pos, float curr_yaw, float hove
 	return 0;
 }
 
-int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t)
+int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t)
 {
-	// allow in any state, i.e., can also be used to take-off or land
-
-	struct traj_eval setpoint = plan_current_goal(p, t);
-
 	if (relative) {
-		hover_pos = vadd(hover_pos, setpoint.pos);
-		hover_yaw += setpoint.yaw;
+		hover_pos = vadd(hover_pos, curr_eval->pos);
+		hover_yaw += curr_eval->yaw;
 	}
 
 	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());
+		curr_eval->pos, curr_eval->yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc,
+		hover_pos,      hover_yaw,      vzero(),        0,                  vzero());
 
 	p->reversed = false;
 	p->state = TRAJECTORY_STATE_FLYING;
@@ -185,6 +180,12 @@ int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hov
 	return 0;
 }
 
+int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t)
+{
+	struct traj_eval setpoint = plan_current_goal(p, t);
+	return plan_go_to_from(p, &setpoint, relative, hover_pos, hover_yaw, duration, t);
+}
+
 int plan_start_trajectory( struct planner *p, const struct piecewise_traj* trajectory, bool reversed)
 {
 	p->reversed = reversed;

src/utils/interface/cf_math.h

@@ -39,3 +39,6 @@
 
 #define DEG_TO_RAD (PI/180.0f)
 #define RAD_TO_DEG (180.0f/PI)
+
+#define MIN(a, b) ((b) < (a) ? (b) : (a))
+#define MAX(a, b) ((b) > (a) ? (b) : (a))