From 8b51f6164365e629c423c84b4f92fe660ee19af3 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Sat, 24 Mar 2018 20:34:22 -0700 Subject: [PATCH] High-level commander: trajectory upload and exec. This adds the possibility to upload polynomial trajectories using the existing memory interface. Trajectories can be executed with the high-level commander. Tested with crazyflie_ros (test_high_level.py). See issue #293. --- .../interface/crtp_commander_high_level.h | 6 ++ src/modules/interface/planner.h | 3 + src/modules/interface/pptraj.h | 17 ++-- src/modules/src/crtp_commander_high_level.c | 69 ++++++++++++++++- src/modules/src/mem_cf2.c | 30 +++++++- src/modules/src/planner.c | 11 +++ src/modules/src/pptraj.c | 77 ++++++++----------- 7 files changed, 156 insertions(+), 57 deletions(-) diff --git a/src/modules/interface/crtp_commander_high_level.h b/src/modules/interface/crtp_commander_high_level.h index d26e4dc065..9ee0c3a830 100644 --- a/src/modules/interface/crtp_commander_high_level.h +++ b/src/modules/interface/crtp_commander_high_level.h @@ -46,6 +46,12 @@ Header file for high-level commander that computes smooth setpoints based on hig #include "stabilizer_types.h" +// allocate memory to store trajectories +// 4k allows us to store 31 poly4d pieces +// other (compressed) formats might be added in the future +#define TRAJECTORY_MEMORY_SIZE 4096 +extern uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE]; + /* Public functions */ void crtpCommanderHighLevelInit(void); diff --git a/src/modules/interface/planner.h b/src/modules/interface/planner.h index ba336eddc9..4f59df7192 100644 --- a/src/modules/interface/planner.h +++ b/src/modules/interface/planner.h @@ -79,3 +79,6 @@ int plan_land(struct planner *p, struct vec pos, float yaw, float height, float // 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); + +// start trajectory +int plan_start_trajectory(struct planner *p, const struct piecewise_traj* trajectory, bool reversed); diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h index b05cf2eaa3..43b79f757e 100644 --- a/src/modules/interface/pptraj.h +++ b/src/modules/interface/pptraj.h @@ -42,7 +42,6 @@ Header file for piecewise polynomial trajectories #define PP_DEGREE (7) #define PP_SIZE (PP_DEGREE + 1) -#define PP_MAX_PIECES (30) // @@ -83,7 +82,7 @@ struct poly4d { float p[4][PP_SIZE]; float duration; // TODO use int millis instead? -}; +} __attribute__((packed)); // construct a 4d zero polynomial. struct poly4d poly4d_zero(float duration); @@ -139,9 +138,11 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t); struct piecewise_traj { - struct poly4d pieces[PP_MAX_PIECES]; float t_begin; + float timescale; + struct vec shift; unsigned char n_pieces; + struct poly4d* pieces; }; static inline float piecewise_duration(struct piecewise_traj const *pp) @@ -150,7 +151,7 @@ static inline float piecewise_duration(struct piecewise_traj const *pp) for (int i = 0; i < pp->n_pieces; ++i) { total_dur += pp->pieces[i].duration; } - return total_dur; + return total_dur * pp->timescale; } void piecewise_plan_5th_order(struct piecewise_traj *p, float duration, @@ -167,14 +168,6 @@ struct traj_eval piecewise_eval( struct traj_eval piecewise_eval_reversed( struct piecewise_traj const *traj, float t); -void piecewise_scale(struct piecewise_traj *pp, float x, float y, float z, float yaw); - -void piecewise_shift(struct piecewise_traj *pp, float x, float y, float z, float yaw); -static inline void piecewise_shift_vec(struct piecewise_traj *pp, struct vec pos, float yaw) { - piecewise_shift(pp, pos.x, pos.y, pos.z, yaw); -} - -void piecewise_stretchtime(struct piecewise_traj *pp, float s); static inline bool piecewise_is_finished(struct piecewise_traj const *traj, float t) { diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 00031bdcf2..f582978a5c 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -56,11 +56,14 @@ such as: take-off, landing, polynomial trajectories. #include "param.h" // Global variables +uint8_t trajectories_memory[TRAJECTORY_MEMORY_SIZE]; + static bool isInit = false; static struct planner planner; static uint8_t group_mask; static struct vec pos; // last known state (position [m]) static float yaw; // last known state (yaw [rad]) +static struct piecewise_traj trajectory; // makes sure that we don't evaluate the trajectory while it is being changed static xSemaphoreHandle lockTraj; @@ -74,6 +77,7 @@ enum TrajectoryCommand_e { COMMAND_LAND = 2, COMMAND_STOP = 3, COMMAND_GO_TO = 4, + COMMAND_START_TRAJECTORY = 5, }; struct data_set_group_mask { @@ -110,6 +114,33 @@ 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; + float timescale; // time factor; 1 = original speed; >1: slower; <1: faster +} __attribute__((packed)); + // Private functions static void crtpCommanderHighLevelTask(void * prm); @@ -118,6 +149,7 @@ static int takeoff(const struct data_takeoff* data); 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); // Helper functions static struct vec state2vec(struct vec3_s v) @@ -147,7 +179,6 @@ void crtpCommanderHighLevelInit(void) yaw = 0; isInit = true; - DEBUG_PRINT("traj. initialized.\n"); } void crtpCommanderHighLevelStop() @@ -224,6 +255,9 @@ void crtpCommanderHighLevelTask(void * prm) case COMMAND_GO_TO: ret = go_to((const struct data_go_to*)&p.data[1]); break; + case COMMAND_START_TRAJECTORY: + ret = start_trajectory((const struct data_start_trajectory*)&p.data[1]); + break; default: ret = ENOEXEC; break; @@ -290,3 +324,36 @@ int go_to(const struct data_go_to* data) } return result; } + +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); + } + 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; +} diff --git a/src/modules/src/mem_cf2.c b/src/modules/src/mem_cf2.c index 730a39a62a..5c4941ae90 100644 --- a/src/modules/src/mem_cf2.c +++ b/src/modules/src/mem_cf2.c @@ -43,6 +43,7 @@ #include "ledring12.h" #include "locodeck.h" +#include "crtp_commander_high_level.h" #include "console.h" #include "assert.h" @@ -65,7 +66,8 @@ #define EEPROM_ID 0x00 #define LEDMEM_ID 0x01 #define LOCO_ID 0x02 -#define OW_FIRST_ID 0x03 +#define TRAJ_ID 0x03 +#define OW_FIRST_ID 0x04 #define STATUS_OK 0 @@ -73,6 +75,7 @@ #define MEM_TYPE_OW 0x01 #define MEM_TYPE_LED12 0x10 #define MEM_TYPE_LOCO 0x11 +#define MEM_TYPE_TRAJ 0x12 #define MEM_LOCO_INFO 0x0000 #define MEM_LOCO_ANCHOR_BASE 0x1000 @@ -191,6 +194,9 @@ void createInfoResponse(CRTPPacket* p, uint8_t memId) case LOCO_ID: createInfoResponseBody(p, MEM_TYPE_LOCO, MEM_LOCO_ANCHOR_BASE + MEM_LOCO_ANCHOR_PAGE_SIZE * LOCODECK_NR_OF_ANCHORS, noData); break; + case TRAJ_ID: + createInfoResponseBody(p, MEM_TYPE_TRAJ, sizeof(trajectories_memory), noData); + break; default: if (owGetinfo(memId - OW_FIRST_ID, &serialNbr)) { @@ -252,6 +258,17 @@ void memReadProcess() status = handleLocoMemRead(memAddr, readLen, &p.data[6]); break; + case TRAJ_ID: + { + if (memAddr + readLen <= sizeof(trajectories_memory) && + memcpy(&p.data[6], &(trajectories_memory[memAddr]), readLen)) { + status = STATUS_OK; + } else { + status = EIO; + } + } + break; + default: { memId = memId - OW_FIRST_ID; @@ -368,6 +385,17 @@ void memWriteProcess() status = EIO; break; + case TRAJ_ID: + { + if ((memAddr + writeLen) <= sizeof(trajectories_memory)) { + memcpy(&(trajectories_memory[memAddr]), &p.data[5], writeLen); + status = STATUS_OK; + } else { + status = EIO; + } + } + break; + default: { memId = memId - OW_FIRST_ID; diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 387af6aaae..1cdf017ffc 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -39,6 +39,7 @@ implementation of planning state machine #include "planner.h" static struct piecewise_traj planned_trajectory; +static struct poly4d pieces[1]; // the on-board planner requires a single piece, only static void plan_takeoff_or_landing(struct planner *p, struct vec pos, float yaw, float height, float duration) { @@ -59,6 +60,7 @@ void plan_init(struct planner *p) p->state = TRAJECTORY_STATE_IDLE; p->reversed = false; p->trajectory = NULL; + planned_trajectory.pieces = pieces; } void plan_stop(struct planner *p) @@ -143,3 +145,12 @@ int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hov p->trajectory = &planned_trajectory; return 0; } + +int plan_start_trajectory( struct planner *p, const struct piecewise_traj* trajectory, bool reversed) +{ + p->reversed = reversed; + p->trajectory = trajectory; + p->state = TRAJECTORY_STATE_FLYING; + + return 0; +} \ No newline at end of file diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c index be325beea7..158beaecb7 100644 --- a/src/modules/src/pptraj.c +++ b/src/modules/src/pptraj.c @@ -41,6 +41,8 @@ See Daniel Mellinger, Vijay Kumar: "Minimum snap trajectory generation and contr #define GRAV (9.81f) +static struct poly4d poly4d_tmp; + // polynomials are stored with ascending degree void polylinear(float p[PP_SIZE], float duration, float x0, float x1) @@ -209,16 +211,16 @@ static float polyval_yaw(struct poly4d const *p, float t) // uses L1 norm instead of Euclidean, evaluates polynomial instead of root-finding float poly4d_max_accel_approx(struct poly4d const *p) { - static struct poly4d acc; - acc = *p; - polyder4d(&acc); - polyder4d(&acc); + struct poly4d* acc = &poly4d_tmp; + *acc = *p; + polyder4d(acc); + polyder4d(acc); int steps = 10 * p->duration; float step = p->duration / (steps - 1); float t = 0; float amax = 0; for (int i = 0; i < steps; ++i) { - struct vec ddx = polyval_xyz(&acc, t); + struct vec ddx = polyval_xyz(acc, t); float ddx_minkowski = vminkowski(ddx); if (ddx_minkowski > amax) amax = ddx_minkowski; t += step; @@ -246,19 +248,19 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t) out.yaw = polyval_yaw(p, t); // 1st derivative - static struct poly4d deriv; - deriv = *p; - polyder4d(&deriv); - out.vel = polyval_xyz(&deriv, t); - float dyaw = polyval_yaw(&deriv, t); + struct poly4d* deriv = &poly4d_tmp; + *deriv = *p; + polyder4d(deriv); + out.vel = polyval_xyz(deriv, t); + float dyaw = polyval_yaw(deriv, t); // 2nd derivative - polyder4d(&deriv); - out.acc = polyval_xyz(&deriv, t); + polyder4d(deriv); + out.acc = polyval_xyz(deriv, t); // 3rd derivative - polyder4d(&deriv); - struct vec jerk = polyval_xyz(&deriv, t); + polyder4d(deriv); + struct vec jerk = polyval_xyz(deriv, t); struct vec thrust = vadd(out.acc, mkvec(0, 0, GRAV)); // float thrust_mag = mass * vmag(thrust); @@ -290,10 +292,13 @@ struct traj_eval piecewise_eval( t = t - traj->t_begin; while (cursor < traj->n_pieces) { struct poly4d const *piece = &(traj->pieces[cursor]); - if (t <= piece->duration) { - return poly4d_eval(piece, t); + if (t <= piece->duration * traj->timescale) { + poly4d_tmp = *piece; + poly4d_shift(&poly4d_tmp, traj->shift.x, traj->shift.y, traj->shift.z, 0); + poly4d_stretchtime(&poly4d_tmp, traj->timescale); + return poly4d_eval(&poly4d_tmp, t); } - t -= piece->duration; + t -= piece->duration * traj->timescale; ++cursor; } // if we get here, the trajectory has ended @@ -312,15 +317,17 @@ struct traj_eval piecewise_eval_reversed( t = t - traj->t_begin; while (cursor >= 0) { struct poly4d const *piece = &(traj->pieces[cursor]); - if (t <= piece->duration) { - struct poly4d piece_reversed = *piece; + if (t <= piece->duration * traj->timescale) { + poly4d_tmp = *piece; + poly4d_shift(&poly4d_tmp, traj->shift.x, traj->shift.y, traj->shift.z, 0); + poly4d_stretchtime(&poly4d_tmp, traj->timescale); for (int i = 0; i < 4; ++i) { - polyreflect(piece_reversed.p[i]); + polyreflect(poly4d_tmp.p[i]); } - t = t - piece->duration; - return poly4d_eval(&piece_reversed, t); + t = t - piece->duration * traj->timescale; + return poly4d_eval(&poly4d_tmp, t); } - t -= piece->duration; + t -= piece->duration * traj->timescale; --cursor; } // if we get here, the trajectory has ended @@ -340,6 +347,8 @@ void piecewise_plan_5th_order(struct piecewise_traj *pp, float duration, { struct poly4d *p = &pp->pieces[0]; p->duration = duration; + pp->timescale = 1.0; + pp->shift = vzero(); pp->n_pieces = 1; poly5(p->p[0], duration, p0.x, v0.x, a0.x, p1.x, v1.x, a1.x); poly5(p->p[1], duration, p0.y, v0.y, a0.y, p1.y, v1.y, a1.y); @@ -354,6 +363,8 @@ void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *pp, float duration, { struct poly4d *p = &pp->pieces[0]; p->duration = duration; + pp->timescale = 1.0; + pp->shift = vzero(); pp->n_pieces = 1; poly7_nojerk(p->p[0], duration, p0.x, v0.x, a0.x, p1.x, v1.x, a1.x); poly7_nojerk(p->p[1], duration, p0.y, v0.y, a0.y, p1.y, v1.y, a1.y); @@ -361,23 +372,3 @@ void piecewise_plan_7th_order_no_jerk(struct piecewise_traj *pp, float duration, poly7_nojerk(p->p[3], duration, y0, dy0, 0, y1, dy1, 0); } -void piecewise_scale(struct piecewise_traj *pp, float x, float y, float z, float yaw) -{ - for (int i = 0; i < PP_MAX_PIECES; ++i) { - poly4d_scale(&pp->pieces[i], x, y, z, yaw); - } -} - -void piecewise_shift(struct piecewise_traj *pp, float x, float y, float z, float yaw) -{ - for (int i = 0; i < PP_MAX_PIECES; ++i) { - poly4d_shift(&pp->pieces[i], x, y, z, yaw); - } -} - -void piecewise_stretchtime(struct piecewise_traj *pp, float s) -{ - for (int i = 0; i < PP_MAX_PIECES; ++i) { - poly4d_stretchtime(&pp->pieces[i], s); - } -}