From 3b1e724a6a6539abe94be33c818745ab54b6cda5 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Fri, 6 Apr 2018 17:17:15 -0700 Subject: [PATCH] HighLevelCmd:Broadcast support for startTrajectory The previous design of startTrajectory required to pass in information about the memory layout. When using multiple quadrotors with different trajectories, this design does not allow to start trajectories at the same time using a broadcast. The updated API design now has a defineTrajectory method where the memory layout can be defined for a trajectoryId. StartTrajectory just requires the trajectoryId. CFs can now be configured individually with their trajectories, and trajectory execution can be started simulatanously using broadcasts. Verified with 2 CFs and a modified Crazyswarm package using a figure8 trajectory. --- .../interface/crtp_commander_high_level.h | 2 + src/modules/src/crtp_commander_high_level.c | 117 +++++++++++------- 2 files changed, 76 insertions(+), 43 deletions(-) diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index 9ee0c3a830..542028ab02 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/src/modules/interface/crtp_commander_high_level.h @@ -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); diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index f582978a5c..32c574e186 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -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; @@ -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 { @@ -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); @@ -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) @@ -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; @@ -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; +}