From bac4b1016ba87ef9d2ef7e82231aafc3d17fd3f0 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Mon, 26 Aug 2024 11:17:22 +0200 Subject: [PATCH 1/5] Adding linear GOTO and Spiral segment to HL commander --- .../interface/crtp_commander_high_level.h | 17 ++- src/modules/interface/planner.h | 7 +- src/modules/src/crtp_commander_high_level.c | 66 ++++++++++- src/modules/src/planner.c | 112 +++++++++++++++++- 4 files changed, 190 insertions(+), 12 deletions(-) diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index ad0bbb631c..d5d0b97bf9 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/src/modules/interface/crtp_commander_high_level.h @@ -193,9 +193,24 @@ bool crtpCommanderHighLevelIsBlocked(); * @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 crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative); +int crtpCommanderHighLevelGoTo(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 c242a9390c..69d7de6671 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 7c0e3f4b9e..78d2c1e377 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -140,6 +140,7 @@ enum TrajectoryCommand_e { COMMAND_LAND_2 = 8, COMMAND_TAKEOFF_WITH_VELOCITY = 9, COMMAND_LAND_WITH_VELOCITY = 10, + COMMAND_SPIRAL = 11, }; struct data_set_group_mask { @@ -211,6 +212,7 @@ struct data_stop { struct data_go_to { 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 @@ -218,6 +220,18 @@ struct data_go_to { 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 +259,7 @@ 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 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 +418,9 @@ 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_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 +627,41 @@ 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, 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) { @@ -804,7 +847,7 @@ bool crtpCommanderHighLevelIsBlocked() return isBlocked; } -int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative) +int crtpCommanderHighLevelGoTo(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 data = { @@ -814,12 +857,29 @@ int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, cons .yaw = yaw, .duration = duration_s, .relative = relative, + .linear = linear, .groupMask = ALL_GROUPS, }; return handleCommand(COMMAND_GO_TO, (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 1f02a27256..05cc573ab5 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) From 996888e56aed052ca09e10556825cedf712f21de Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Fri, 13 Sep 2024 13:53:47 +0200 Subject: [PATCH 2/5] Introducing GOTO2 and keeping GOTO (will be deprecated) --- src/modules/interface/crtp.h | 2 +- .../interface/crtp_commander_high_level.h | 15 +++- src/modules/src/crtp_commander_high_level.c | 69 ++++++++++++++++++- 3 files changed, 81 insertions(+), 5 deletions(-) diff --git a/src/modules/interface/crtp.h b/src/modules/interface/crtp.h index 094bb9068e..b01837a5bd 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 d5d0b97bf9..b67786652d 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/src/modules/interface/crtp_commander_high_level.h @@ -184,6 +184,19 @@ int crtpCommanderBlock(bool doBlock); */ bool crtpCommanderHighLevelIsBlocked(); +/** + * @brief Go to an absolute or relative position (will be deprecated on TBD, use crtpCommanderHighLevelGoTo2) + * + * @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 + * @return zero if the command succeeded, an error code otherwise + */ +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 * @@ -196,7 +209,7 @@ bool crtpCommanderHighLevelIsBlocked(); * @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 crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative, const bool linear); +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) diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 78d2c1e377..77a047577b 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -133,7 +133,7 @@ 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, @@ -141,6 +141,7 @@ enum TrajectoryCommand_e { COMMAND_TAKEOFF_WITH_VELOCITY = 9, COMMAND_LAND_WITH_VELOCITY = 10, COMMAND_SPIRAL = 11, + COMMAND_GO_TO_2 = 12, }; struct data_set_group_mask { @@ -210,6 +211,17 @@ struct data_stop { // "take this much time to go here, then hover" struct data_go_to { + 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 + float x; // m + float y; // m + float z; // m + float yaw; // rad + 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 @@ -259,6 +271,7 @@ 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); @@ -418,6 +431,9 @@ 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; @@ -618,6 +634,37 @@ int go_to(const struct data_go_to* data) 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, 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); @@ -847,7 +894,7 @@ bool crtpCommanderHighLevelIsBlocked() return isBlocked; } -int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative, const bool linear) +int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, const float yaw, const float duration_s, const bool relative) { struct data_go_to data = { @@ -857,13 +904,29 @@ int crtpCommanderHighLevelGoTo(const float x, const float y, const float z, cons .yaw = yaw, .duration = duration_s, .relative = relative, - .linear = linear, .groupMask = ALL_GROUPS, }; 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 = From 42d207174d10e8302ff1a52b1e6fbbc4f687d90c Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Wed, 25 Sep 2024 14:03:27 +0200 Subject: [PATCH 3/5] deprecation comments updated --- src/modules/interface/crtp_commander_high_level.h | 2 +- src/modules/src/crtp_commander_high_level.c | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index b67786652d..c793148d1e 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 (will be deprecated on TBD, use crtpCommanderHighLevelGoTo2) + * @brief Go to an absolute or relative position (will be deprecated, use crtpCommanderHighLevelGoTo2) * * @param x x (m) * @param y y (m) diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 77a047577b..71731bb200 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -133,7 +133,7 @@ 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, // Deprecated (removed after TBD), use COMMAND_GO_TO_2 + 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, From e52490afd44f430115a04693ae4a965110112c56 Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Thu, 3 Oct 2024 17:49:56 +0200 Subject: [PATCH 4/5] limitting parameters to reasonable values & cleanup --- .../interface/crtp_commander_high_level.h | 8 +++--- src/modules/src/planner.c | 25 +++++++++++++++---- 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index c793148d1e..352638ba70 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/src/modules/interface/crtp_commander_high_level.h @@ -214,10 +214,10 @@ int crtpCommanderHighLevelGoTo2(const float x, const float y, const float z, con /** * @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 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 diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 05cc573ab5..a651741fcc 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -37,6 +37,7 @@ implementation of planning state machine */ #include #include "planner.h" +#include "arm_math.h" static struct traj_eval plan_eval(struct planner *p, float t); @@ -219,13 +220,29 @@ int plan_go_to(struct planner *p, bool relative, bool linear, struct vec hover_p 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 + // Limitting the inputs + if (spiral_angle > 2*PI) { + spiral_angle = 2*PI; + DEBUG_PRINT("Warning: spiral angle saturated at 2pi\n"); + } + else if (spiral_angle < -2*PI) { + spiral_angle = -2*PI; + DEBUG_PRINT("Warning: spiral angle saturated at -2pi\n"); + } + if (radius0 < 0) { + radius0 = 0; + DEBUG_PRINT("Warning: radius set to 0 (was negative) \n"); + } + if (radiusF < 0) { + radiusF = 0; + DEBUG_PRINT("Warning: radius set to 0 (was negative) \n"); + } + + 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); @@ -233,8 +250,6 @@ int plan_spiral_from(struct planner *p, const struct traj_eval *curr_eval, bool 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); From 7645a1506f914841321d38ec1bddcc1fb6a25e2b Mon Sep 17 00:00:00 2001 From: matejkarasek Date: Thu, 3 Oct 2024 19:09:08 +0200 Subject: [PATCH 5/5] bugfix --- src/modules/src/planner.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index a651741fcc..7fc4aa2b03 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -38,6 +38,7 @@ implementation of planning state machine #include #include "planner.h" #include "arm_math.h" +#include "debug.h" static struct traj_eval plan_eval(struct planner *p, float t);