Improved HL commander - spiral & linear segment

src/modules/interface/crtp.h

@@ -30,7 +30,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
-#define CRTP_PROTOCOL_VERSION 7
+#define CRTP_PROTOCOL_VERSION 8
 
 #define CRTP_MAX_DATA_SIZE 30
 

src/modules/interface/crtp_commander_high_level.h

@@ -185,7 +185,7 @@ int crtpCommanderBlock(bool doBlock);
 bool crtpCommanderHighLevelIsBlocked();
 
 /**
- * @brief Go to an absolute or relative position
+ * @brief Go to an absolute or relative position (will be deprecated on TBD, use crtpCommanderHighLevelGoTo2)
  *
  * @param x          x (m)
  * @param y          y (m)
@@ -197,6 +197,34 @@ bool crtpCommanderHighLevelIsBlocked();
  */
 int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative);
 
+/**
+ * @brief Go to an absolute or relative position
+ *
+ * @param x          x (m)
+ * @param y          y (m)
+ * @param z          z (m)
+ * @param yaw        yaw (rad)
+ * @param duration_s time it should take to reach the position (s)
+ * @param relative   true if x, y, z is relative to the current position
+ * @param linear     true if linear interpolation should be used instead of a smooth polynomial
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelGoTo2(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative, const bool linear);
+
+/**
+ * @brief Follow a spiral segment (spline approximation of and arc for <= 90-degree segments)
+ *
+ * @param phi         spiral angle (rad), positive for left turn, negative for right turn
+ * @param r0          initial radius (m)
+ * @param rf          final radius (m)
+ * @param dz          altitude gain (m)
+ * @param duration_s  time it should take to reach the end of the spiral (s)
+ * @param sideways    true if crazyflie should spiral sideways instead of forward
+ * @param clockwise   true if crazyflie should spiral clockwise instead of counter-clockwise
+ * @return zero if the command succeeded, an error code otherwise
+ */
+int crtpCommanderHighLevelSpiral(const float phi, const float r0, const float rf, const float dz, const float duration_s, const bool sideways);
+
 /**
  * @brief Returns whether the trajectory with the given ID is defined
  *

src/modules/interface/planner.h

@@ -109,10 +109,13 @@ 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);
 
 // 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);
+int plan_go_to(struct planner *p, bool relative, bool linear, 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);
+int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool relative, bool linear, struct vec hover_pos, float hover_yaw, float duration, float t);
+
+// move along a spiral
+int plan_spiral_from(struct planner *p, const struct traj_eval *curr_eval, bool sideways, bool clockwise, float spiral_angle, float radius0, float radiusf, float ascent, float duration, float t);
 
 // start trajectory. start_from param is ignored if relative == false.
 int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, struct vec start_from);

src/modules/src/crtp_commander_high_level.c

@@ -133,13 +133,15 @@ enum TrajectoryCommand_e {
   COMMAND_TAKEOFF                 = 1, // Deprecated (removed after August 2023), use COMMAND_TAKEOFF_2
   COMMAND_LAND                    = 2, // Deprecated (removed after August 2023), use COMMAND_LAND_2
   COMMAND_STOP                    = 3,
-  COMMAND_GO_TO                   = 4,
+  COMMAND_GO_TO                   = 4, // Deprecated (removed after TBD), use COMMAND_GO_TO_2
   COMMAND_START_TRAJECTORY        = 5,
   COMMAND_DEFINE_TRAJECTORY       = 6,
   COMMAND_TAKEOFF_2               = 7,
   COMMAND_LAND_2                  = 8,
   COMMAND_TAKEOFF_WITH_VELOCITY   = 9,
   COMMAND_LAND_WITH_VELOCITY      = 10,
+  COMMAND_SPIRAL                  = 11,
+  COMMAND_GO_TO_2                 = 12,
 };
 
 struct data_set_group_mask {
@@ -218,6 +220,30 @@ struct data_go_to {
   float duration; // sec
 } __attribute__((packed));
 
+// "take this much time to go here, then hover"
+struct data_go_to_2 {
+  uint8_t groupMask; // mask for which CFs this should apply to
+  uint8_t relative;  // set to true, if position/yaw are relative to current setpoint
+  uint8_t linear;  // set to true for linear interpolation instead of smooth polynomial
+  float x; // m
+  float y; // m
+  float z; // m
+  float yaw; // rad
+  float duration; // sec
+} __attribute__((packed));
+
+// "fly along a spiral"
+struct data_spiral {
+  uint8_t groupMask; // mask for which CFs this should apply to
+  uint8_t sideways;  // set to true, if crazyfly should spiral sideways instead of forward
+  uint8_t clockwise; // set to true, if crazyfly should spiral clockwise instead of counter-clockwise
+  float phi; // rad
+  float r0; // m
+  float rf; // m
+  float dz; // m
+  float duration; // sec
+} __attribute__((packed));
+
 // starts executing a specified trajectory
 struct data_start_trajectory {
   uint8_t groupMask; // mask for which CFs this should apply to
@@ -245,6 +271,8 @@ static int takeoff_with_velocity(const struct data_takeoff_with_velocity* data);
 static int land_with_velocity(const struct data_land_with_velocity* data);
 static int stop(const struct data_stop* data);
 static int go_to(const struct data_go_to* data);
+static int go_to2(const struct data_go_to_2* data);
+static int spiral(const struct data_spiral* data);
 static int start_trajectory(const struct data_start_trajectory* data);
 static int define_trajectory(const struct data_define_trajectory* data);
 
@@ -403,6 +431,12 @@ static int handleCommand(const enum TrajectoryCommand_e command, const uint8_t*
     case COMMAND_GO_TO:
       ret = go_to((const struct data_go_to*)data);
       break;
+    case COMMAND_GO_TO_2:
+      ret = go_to2((const struct data_go_to_2*)data);
+      break;
+    case COMMAND_SPIRAL:
+      ret = spiral((const struct data_spiral*)data);
+      break;
     case COMMAND_START_TRAJECTORY:
       ret = start_trajectory((const struct data_start_trajectory*)data);
       break;
@@ -609,16 +643,72 @@ int go_to(const struct data_go_to* data)
       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);
+      result = plan_go_to_from(&planner, &ev, data->relative, false, hover_pos, data->yaw, data->duration, t);
+    }
+    else {
+      result = plan_go_to(&planner, data->relative, false, hover_pos, data->yaw, data->duration, t);
+    }
+    xSemaphoreGive(lockTraj);
+  }
+  return result;
+}
+
+int go_to2(const struct data_go_to_2* 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},
+  };
+
+  if (isBlocked) {
+    return EBUSY;
+  }
+
+  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;
+    if (plan_is_disabled(&planner) || plan_is_stopped(&planner)) {
+      ev.pos = pos;
+      ev.vel = vel;
+      ev.yaw = yaw;
+      result = plan_go_to_from(&planner, &ev, data->relative, data->linear, hover_pos, data->yaw, data->duration, t);
     }
     else {
-      result = plan_go_to(&planner, data->relative, hover_pos, data->yaw, data->duration, t);
+      result = plan_go_to(&planner, data->relative, data->linear, hover_pos, data->yaw, data->duration, t);
     }
     xSemaphoreGive(lockTraj);
   }
   return result;
 }
 
+int spiral(const struct data_spiral* 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},
+  };
+
+  if (isBlocked) {
+    return EBUSY;
+  }
+
+  int result = 0;
+  if (isInGroup(data->groupMask)) {
+    xSemaphoreTake(lockTraj, portMAX_DELAY);
+    float t = usecTimestamp() / 1e6;
+    ev.pos = pos;
+    ev.vel = vel;
+    ev.yaw = yaw;
+    result = plan_spiral_from(&planner, &ev, data->sideways, data->clockwise, data->phi, data->r0, data->rf, data->dz, data->duration, t);
+    xSemaphoreGive(lockTraj);
+  }
+  return result;
+}
+
 int start_trajectory(const struct data_start_trajectory* data)
 {
   if (isBlocked) {
@@ -820,6 +910,39 @@ int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, cons
   return handleCommand(COMMAND_GO_TO, (const uint8_t*)&data);
 }
 
+int crtpCommanderHighLevelGoTo2(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative, const bool linear)
+{
+  struct data_go_to_2 data =
+  {
+    .x = x,
+    .y = y,
+    .z = z,
+    .yaw = yaw,
+    .duration = duration_s,
+    .relative = relative,
+    .linear = linear,
+    .groupMask = ALL_GROUPS,
+  };
+
+  return handleCommand(COMMAND_GO_TO_2, (const uint8_t*)&data);
+}
+
+int crtpCommanderHighLevelSpiral(const float phi, const float r0, const float rf, const float dz, const float duration_s, const bool sideways)
+{
+  struct data_spiral data =
+  {
+    .phi = phi,
+    .r0 = r0,
+    .rf = rf,
+    .dz = dz,
+    .duration = duration_s,
+    .sideways = sideways,
+    .groupMask = ALL_GROUPS,
+  };
+
+  return handleCommand(COMMAND_SPIRAL, (const uint8_t*)&data);
+}
+
 bool crtpCommanderHighLevelIsTrajectoryDefined(uint8_t trajectoryId)
 {
   return (

src/modules/src/planner.c

@@ -176,7 +176,7 @@ int plan_land(struct planner *p, struct vec curr_pos, float curr_yaw, float hove
 	return 0;
 }
 
-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)
+int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool relative, bool linear, struct vec hover_pos, float hover_yaw, float duration, float t)
 {
 	if (relative) {
 		hover_pos = vadd(hover_pos, curr_eval->pos);
@@ -186,11 +186,22 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r
 	// compute the shortest possible rotation towards 0
 	float curr_yaw = normalize_radians(curr_eval->yaw);
 	hover_yaw = normalize_radians(hover_yaw);
-	float goal_yaw = curr_yaw + shortest_signed_angle_radians(curr_yaw, hover_yaw);
-
-	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
+	float delta_yaw = shortest_signed_angle_radians(curr_yaw, hover_yaw);
+	float goal_yaw = curr_yaw + delta_yaw;
+
+	if (linear) {
+		struct vec vel = vdiv(vsub(hover_pos,curr_eval->pos), duration);
+		float omz = delta_yaw/duration;
+
+		piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
+		curr_eval->pos, curr_yaw, vel, omz, vzero(),
+		hover_pos,      goal_yaw, vel, omz, vzero());
+	}
+	else {
+		piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
 		curr_eval->pos, curr_yaw, curr_eval->vel, curr_eval->omega.z, curr_eval->acc,
 		hover_pos,      goal_yaw,      vzero(),        0,                  vzero());
+	}
 
 	p->reversed = false;
 	p->state = TRAJECTORY_STATE_FLYING;
@@ -200,10 +211,99 @@ int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool r
 	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(struct planner *p, bool relative, bool linear, 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);
+	return plan_go_to_from(p, &setpoint, relative, linear, hover_pos, hover_yaw, duration, t);
+}
+
+int plan_spiral_from(struct planner *p, const struct traj_eval *curr_eval, bool sideways, bool clockwise, float spiral_angle, float radius0, float radiusF, float ascent, float duration, float t)
+{
+	int sense = -1 + 2*clockwise; // -1 counter-clockwise, +1 clockwise
+	float omz =  -sense*spiral_angle/duration;
+	float dz = ascent/duration;
+	float dr = (radiusF - radius0)/duration;
+
+	// struct traj_eval setpoint = plan_current_goal(p, t);
+
+	struct vec pos0 = curr_eval->pos;
+
+	float phi0 = normalize_radians(curr_eval->yaw);
+
+	float yawF = phi0 - sense*spiral_angle;
+	float phiF = phi0 - sense*spiral_angle;
+
+	// TODO sideways, now assuming forward/backward
+
+	float c0 = cosf(phi0);
+	float s0 = sinf(phi0);
+	float cF = cosf(phiF);
+	float sF = sinf(phiF);
+
+	float z0 = pos0.z;
+	float xCenter, yCenter, dx0, ddx0, xF, dxF, ddxF, dy0, ddy0, yF, dyF, ddyF;
+
+	if (sideways)
+	{
+		// center position
+		xCenter = pos0.x + sense*radius0*c0;
+		yCenter = pos0.y + sense*radius0*s0;
+
+		// x0 = -sense*radius0*c0;
+		dx0 = sense*(radius0*s0*omz - dr*c0);
+		ddx0 = sense*(radius0*c0*omz*omz + 2*dr*s0*omz);
+
+		xF = -sense*radiusF*cF;
+		dxF = sense*(radiusF*sF*omz - dr*cF);
+		ddxF = sense*(radiusF*cF*omz*omz + 2*dr*sF*omz);
+
+		// y0 = -sense*radius0*s0;
+		dy0 = sense*(-radius0*c0*omz - dr*s0);
+		ddy0 = sense*(radius0*s0*omz*omz - 2*dr*c0*omz);
+
+		yF = -sense*radiusF*sF;
+		dyF = sense*(-radiusF*cF*omz - dr*sF);
+		ddyF = sense*(radiusF*sF*omz*omz - 2*dr*cF*omz);
+	}
+	else
+	{
+		// center position
+		xCenter = pos0.x + sense*radius0*s0;
+		yCenter = pos0.y - sense*radius0*c0;
+
+		// x0 = -sense*radius0*s0;
+		dx0 = -sense*(radius0*c0*omz + dr*s0);
+		ddx0 = sense*(radius0*s0*omz*omz - 2*dr*c0*omz);
+
+		xF = -sense*radiusF*sF;
+		dxF = -sense*(radiusF*cF*omz + dr*sF);
+		ddxF = sense*(radiusF*sF*omz*omz - 2*dr*cF*omz);
+
+		// y0 = sense*radius0*c0;
+		dy0 = sense*(-radius0*s0*omz + dr*c0);
+		ddy0 = sense*(-radius0*c0*omz*omz - 2*dr*s0*omz);
+
+		yF = sense*radiusF*cF;
+		dyF = sense*(-radiusF*sF*omz + dr*cF);
+		ddyF = sense*(-radiusF*cF*omz*omz - 2*dr*sF*omz);
+	}
+
+	struct vec posF = mkvec(xCenter + xF, yCenter + yF, z0 + ascent);
+	struct vec vel0 = mkvec(dx0, dy0, dz);
+	struct vec velF = mkvec(dxF, dyF, dz);
+	struct vec acc0 = mkvec(ddx0, ddy0, 0);
+	struct vec accF = mkvec(ddxF, ddyF, 0);
+
+	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
+		pos0, phi0, vel0, omz, acc0,
+		posF, yawF, velF, omz, accF);
+
+	p->reversed = false;
+	p->state = TRAJECTORY_STATE_FLYING;
+	p->type = TRAJECTORY_TYPE_PIECEWISE;
+	p->planned_trajectory.t_begin = t;
+	p->trajectory = &p->planned_trajectory;
+	return 0;
 }
 
 int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, struct vec start_from)