Skip to content

Commit

Permalink
Adding linear GOTO and Spiral segment to HL commander
Browse files Browse the repository at this point in the history
  • Loading branch information
@matejkarasek
matejkarasek committed Sep 9, 2024
1 parent 750995b commit bac4b10
Show file tree
Hide file tree
Showing 4 changed files with 190 additions and 12 deletions.
17 changes: 16 additions & 1 deletion src/modules/interface/crtp_commander_high_level.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
7 changes: 5 additions & 2 deletions src/modules/interface/planner.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
66 changes: 63 additions & 3 deletions src/modules/src/crtp_commander_high_level.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -211,13 +212,26 @@ 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
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
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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) {
Expand Down Expand Up @@ -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 =
{
Expand All @@ -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 (
Expand Down
112 changes: 106 additions & 6 deletions src/modules/src/planner.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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;
Expand All @@ -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)
Expand Down

0 comments on commit bac4b10

Please sign in to comment.