Merge pull request #1410 from flapper-drones/PR_HLcommander_improved

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, 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), limited to +/- 2pi
+ * @param r0          initial radius (m), must be positive
+ * @param rf          final radius (m), must be positive
+ * @param dz          altitude gain (m), positive to climb, negative to descent
+ * @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 (will be removed), 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 (