Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

HighLevelCmd:Broadcast support for startTrajectory #309

Merged
merged 1 commit into from
Apr 20, 2018
Merged

HighLevelCmd:Broadcast support for startTrajectory #309

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/modules/interface/crtp_commander_high_level.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,8 @@ Header file for high-level commander that computes smooth setpoints based on hig
#define TRAJECTORY_MEMORY_SIZE 4096
extern uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE];

#define NUM_TRAJECTORY_DEFINITIONS 10

/* Public functions */
void crtpCommanderHighLevelInit(void);

Expand Down
117 changes: 74 additions & 43 deletions src/modules/src/crtp_commander_high_level.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,34 @@ such as: take-off, landing, polynomial trajectories.
#include "log.h"
#include "param.h"

// Local types
enum TrajectoryLocation_e {
TRAJECTORY_LOCATION_INVALID = 0,
TRAJECTORY_LOCATION_MEM = 1, // for trajectories that are uploaded dynamically
// Future features might include trajectories on flash or uSD card
};

enum TrajectoryType_e {
TRAJECTORY_TYPE_POLY4D = 0, // struct poly4d, see pptraj.h
// Future types might include versions without yaw
};

struct trajectoryDescription
{
uint8_t trajectoryLocation; // one of TrajectoryLocation_e
uint8_t trajectoryType; // one of TrajectoryType_e
union
{
struct {
uint32_t offset; // offset in uploaded memory
uint8_t n_pieces;
} __attribute__((packed)) mem; // if trajectoryLocation is TRAJECTORY_LOCATION_MEM
} trajectoryIdentifier;
} __attribute__((packed));

// Global variables
uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE];
static struct trajectoryDescription trajectory_descriptions[NUM_TRAJECTORY_DEFINITIONS];

static bool isInit = false;
static struct planner planner;
Expand All @@ -78,6 +104,7 @@ enum TrajectoryCommand_e {
COMMAND_STOP = 3,
COMMAND_GO_TO = 4,
COMMAND_START_TRAJECTORY = 5,
COMMAND_DEFINE_TRAJECTORY = 6,
};

struct data_set_group_mask {
Expand Down Expand Up @@ -114,33 +141,21 @@ struct data_go_to {
float duration; // sec
} __attribute__((packed));

enum TrajectoryLocation_e {
TRAJECTORY_LOCATION_MEM = 0, // for trajectories that are uploaded dynamically
// Future features might include trajectories on flash or uSD card
};

enum TrajectoryType_e {
TRAJECTORY_TYPE_POLY4D = 0, // struct poly4d, see pptraj.h
// Future types might include versions without yaw
};

// starts executing a specified trajectory
struct data_start_trajectory {
uint8_t groupMask; // mask for which CFs this should apply to
uint8_t relative; // set to true, if trajectory should be shifted to current setpoint
uint8_t reversed; // set to true, if trajectory should be executed in reverse
uint8_t trajectoryLocation; // one of TrajectoryLocation_e
uint8_t trajectoryType; // one of TrajectoryType_e
union
{
struct {
uint32_t offset; // offset in uploaded memory
uint8_t n_pieces;
} __attribute__((packed)) mem; // if trajectoryLocation is TRAJECTORY_LOCATION_MEM
} trajectoryIdentifier;
uint8_t trajectoryId; // id of the trajectory (previously defined by COMMAND_DEFINE_TRAJECTORY)
float timescale; // time factor; 1 = original speed; >1: slower; <1: faster
} __attribute__((packed));

// starts executing a specified trajectory
struct data_define_trajectory {
uint8_t trajectoryId;
struct trajectoryDescription description;
} __attribute__((packed));

// Private functions
static void crtpCommanderHighLevelTask(void * prm);

Expand All @@ -150,6 +165,7 @@ static int land(const struct data_land* data);
static int stop(const struct data_stop* data);
static int go_to(const struct data_go_to* data);
static int start_trajectory(const struct data_start_trajectory* data);
static int define_trajectory(const struct data_define_trajectory* data);

// Helper functions
static struct vec state2vec(struct vec3_s v)
Expand Down Expand Up @@ -258,6 +274,9 @@ void crtpCommanderHighLevelTask(void * prm)
case COMMAND_START_TRAJECTORY:
ret = start_trajectory((const struct data_start_trajectory*)&p.data[1]);
break;
case COMMAND_DEFINE_TRAJECTORY:
ret = define_trajectory((const struct data_define_trajectory*)&p.data[1]);
break;
default:
ret = ENOEXEC;
break;
Expand Down Expand Up @@ -328,32 +347,44 @@ int go_to(const struct data_go_to* data)
int start_trajectory(const struct data_start_trajectory* data)
{
int result = 0;
if (isInGroup(data->groupMask)
&& data->trajectoryLocation == TRAJECTORY_LOCATION_MEM
&& data->trajectoryType == TRAJECTORY_TYPE_POLY4D) {
xSemaphoreTake(lockTraj, portMAX_DELAY);
float t = usecTimestamp() / 1e6;
trajectory.t_begin = t;
trajectory.timescale = data->timescale;
trajectory.n_pieces = data->trajectoryIdentifier.mem.n_pieces;
trajectory.pieces = (struct poly4d*)&trajectories_memory[data->trajectoryIdentifier.mem.offset];
if (data->relative) {
trajectory.shift = vzero();
struct traj_eval traj_init;
if (data->reversed) {
traj_init = piecewise_eval_reversed(&trajectory, trajectory.t_begin);
}
else {
traj_init = piecewise_eval(&trajectory, trajectory.t_begin);
if (isInGroup(data->groupMask)) {
if (data->trajectoryId < NUM_TRAJECTORY_DEFINITIONS) {
struct trajectoryDescription* trajDesc = &trajectory_descriptions[data->trajectoryId];
if ( trajDesc->trajectoryLocation == TRAJECTORY_LOCATION_MEM
&& trajDesc->trajectoryType == TRAJECTORY_TYPE_POLY4D) {
xSemaphoreTake(lockTraj, portMAX_DELAY);
float t = usecTimestamp() / 1e6;
trajectory.t_begin = t;
trajectory.timescale = data->timescale;
trajectory.n_pieces = trajDesc->trajectoryIdentifier.mem.n_pieces;
trajectory.pieces = (struct poly4d*)&trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset];
if (data->relative) {
trajectory.shift = vzero();
struct traj_eval traj_init;
if (data->reversed) {
traj_init = piecewise_eval_reversed(&trajectory, trajectory.t_begin);
}
else {
traj_init = piecewise_eval(&trajectory, trajectory.t_begin);
}
struct vec shift_pos = vsub(pos, traj_init.pos);
trajectory.shift = shift_pos;
} else {
trajectory.shift = vzero();
}
result = plan_start_trajectory(&planner, &trajectory, data->reversed);
xSemaphoreGive(lockTraj);
}
struct vec shift_pos = vsub(pos, traj_init.pos);
trajectory.shift = shift_pos;
} else {
trajectory.shift = vzero();
}

result = plan_start_trajectory(&planner, &trajectory, data->reversed);
xSemaphoreGive(lockTraj);
}
return result;
}

int define_trajectory(const struct data_define_trajectory* data)
{
if (data->trajectoryId >= NUM_TRAJECTORY_DEFINITIONS) {
return ENOEXEC;
}
trajectory_descriptions[data->trajectoryId] = data->description;
return 0;
}