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

HLCmd: Avoid setpoint discontinuities #351

Merged
merged 1 commit into from
Jul 27, 2018
Merged

HLCmd: Avoid setpoint discontinuities #351

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
17 changes: 12 additions & 5 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 @@ -87,8 +87,8 @@ static struct trajectoryDescription trajectory_descriptions[NUM_TRAJECTORY_DEFIN
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 vec pos; // last known setpoint (position [m])
static float yaw; // last known setpoint yaw (yaw [rad])
static struct piecewise_traj trajectory;

// makes sure that we don't evaluate the trajectory while it is being changed
Expand Down Expand Up @@ -209,9 +209,6 @@ bool crtpCommanderHighLevelIsStopped()

void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state)
{
pos = state2vec(state->position);
yaw = radians(state->attitude.yaw);

xSemaphoreTake(lockTraj, portMAX_DELAY);
float t = usecTimestamp() / 1e6;
struct traj_eval ev = plan_current_goal(&planner, t);
Expand All @@ -221,6 +218,12 @@ void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
}
xSemaphoreGive(lockTraj);

// if we are on the ground, update the last setpoint with the current state estimate
if (plan_is_stopped(&planner)) {
pos = state2vec(state->position);
yaw = radians(state->attitude.yaw);
}

if (is_traj_eval_valid(&ev)) {
setpoint->position.x = ev.pos.x;
setpoint->position.y = ev.pos.y;
Expand All @@ -242,6 +245,10 @@ void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *stat
setpoint->acceleration.x = ev.acc.x;
setpoint->acceleration.y = ev.acc.y;
setpoint->acceleration.z = ev.acc.z;

// store the last setpoint
pos = ev.pos;
yaw = ev.yaw;
}
}

Expand Down