diff --git a/src/modules/interface/crtp.h b/src/modules/interface/crtp.h index 094bb9068..b01837a5b 100644 --- a/src/modules/interface/crtp.h +++ b/src/modules/interface/crtp.h @@ -30,7 +30,7 @@ #include #include -#define CRTP_PROTOCOL_VERSION 7 +#define CRTP_PROTOCOL_VERSION 8 #define CRTP_MAX_DATA_SIZE 30 diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index ad0bbb631..b67786652 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/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 * diff --git a/src/modules/interface/planner.h b/src/modules/interface/planner.h index c242a9390..69d7de667 100644 --- a/src/modules/interface/planner.h +++ b/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); diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 7c0e3f4b9..77a047577 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/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 ( diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 1f02a2725..05cc573ab 100644 --- a/src/modules/src/planner.c +++ b/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)