Skip to content

Commit

Permalink
Merge pull request bitcraze#303 from USC-ACTLab/high-level-traj
Browse files Browse the repository at this point in the history
High-level commander: trajectory upload and exec.
  • Loading branch information
ataffanel authored Apr 6, 2018
2 parents e9e84fd + 8b51f61 commit 2107af2
Show file tree
Hide file tree
Showing 7 changed files with 156 additions and 57 deletions.
6 changes: 6 additions & 0 deletions src/modules/interface/crtp_commander_high_level.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand Down
3 changes: 3 additions & 0 deletions src/modules/interface/planner.h
Original file line number Diff line number Diff line change
Expand Up @@ -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);
17 changes: 5 additions & 12 deletions src/modules/interface/pptraj.h
Original file line number Diff line number Diff line change
Expand Up @@ -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)


//
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand All @@ -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,
Expand All @@ -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)
{
Expand Down
69 changes: 68 additions & 1 deletion src/modules/src/crtp_commander_high_level.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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 {
Expand Down Expand Up @@ -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);

Expand All @@ -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)
Expand Down Expand Up @@ -147,7 +179,6 @@ void crtpCommanderHighLevelInit(void)
yaw = 0;

isInit = true;
DEBUG_PRINT("traj. initialized.\n");
}

void crtpCommanderHighLevelStop()
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
}
30 changes: 29 additions & 1 deletion src/modules/src/mem_cf2.c
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@

#include "ledring12.h"
#include "locodeck.h"
#include "crtp_commander_high_level.h"

#include "console.h"
#include "assert.h"
Expand All @@ -65,14 +66,16 @@
#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

#define MEM_TYPE_EEPROM 0x00
#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
Expand Down Expand Up @@ -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))
{
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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;
Expand Down
11 changes: 11 additions & 0 deletions src/modules/src/planner.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand All @@ -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)
Expand Down Expand Up @@ -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;
}
Loading

0 comments on commit 2107af2

Please sign in to comment.