diff --git a/src/modules/interface/planner.h b/src/modules/interface/planner.h index c242a9390c..5e2380aaad 100644 --- a/src/modules/interface/planner.h +++ b/src/modules/interface/planner.h @@ -114,8 +114,9 @@ int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hov // same as above, but with current state provided from outside. int plan_go_to_from(struct planner *p, const struct traj_eval *curr_eval, bool relative, struct vec hover_pos, float hover_yaw, float duration, float t); -// start trajectory. start_from param is ignored if relative == false. -int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, struct vec start_from); +// start trajectory. start_from and start_yaw params are ignored if relative == false. +// relative_yaw is only relevant if relative == true, in which case it controls whether yaw is relative +int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, bool relative_yaw, struct vec start_from, float start_yaw); // start compressed trajectory. start_from param is ignored if relative == false. int plan_start_compressed_trajectory(struct planner *p, struct piecewise_traj_compressed* trajectory, bool relative, struct vec start_from); diff --git a/src/modules/interface/pptraj.h b/src/modules/interface/pptraj.h index 22616ff93a..0ae10f7629 100644 --- a/src/modules/interface/pptraj.h +++ b/src/modules/interface/pptraj.h @@ -137,7 +137,8 @@ bool is_traj_eval_valid(struct traj_eval const *ev); // evaluate a single polynomial piece struct traj_eval poly4d_eval(struct poly4d const *p, float t); - +// rotate and then translate a traj_eval object +void traj_eval_transform(struct traj_eval *ev, struct vec shift, float rotation); // ----------------------------------// // piecewise polynomial trajectories // @@ -148,6 +149,7 @@ struct piecewise_traj float t_begin; float timescale; struct vec shift; + float shift_yaw; unsigned char n_pieces; struct poly4d* pieces; }; diff --git a/src/modules/src/crtp_commander_high_level.c b/src/modules/src/crtp_commander_high_level.c index 7c0e3f4b9e..49f5a08ed9 100644 --- a/src/modules/src/crtp_commander_high_level.c +++ b/src/modules/src/crtp_commander_high_level.c @@ -98,6 +98,7 @@ static bool isInit = false; static struct planner planner; static uint8_t group_mask; static bool isBlocked; // Are we blocked to do anything by the supervisor +static bool isRelativeYaw; // Is yaw relative in relative mode? static struct vec pos; // last known setpoint (position [m]) static struct vec vel; // last known setpoint (velocity [m/s]) static float yaw; // last known setpoint yaw (yaw [rad]) @@ -637,7 +638,7 @@ int start_trajectory(const struct data_start_trajectory* data) trajectory.timescale = data->timescale; trajectory.n_pieces = trajDesc->trajectoryIdentifier.mem.n_pieces; trajectory.pieces = (struct poly4d*)&trajectories_memory[trajDesc->trajectoryIdentifier.mem.offset]; - result = plan_start_trajectory(&planner, &trajectory, data->reversed, data->relative, pos); + result = plan_start_trajectory(&planner, &trajectory, data->reversed, data->relative, isRelativeYaw, pos, yaw); xSemaphoreGive(lockTraj); } else if (trajDesc->trajectoryLocation == TRAJECTORY_LOCATION_MEM && trajDesc->trajectoryType == CRTP_CHL_TRAJECTORY_TYPE_POLY4D_COMPRESSED) { @@ -895,6 +896,11 @@ bool crtpCommanderHighLevelIsTrajectoryFinished() { */ PARAM_GROUP_START(hlCommander) +/** + * @brief Boolean whether to use relative yaw in relative trajectories. + */ +PARAM_ADD(PARAM_INT8, relativeYaw, &isRelativeYaw) + /** * @brief Default take off velocity (m/s) */ diff --git a/src/modules/src/planner.c b/src/modules/src/planner.c index 1f02a27256..a7de5342e9 100644 --- a/src/modules/src/planner.c +++ b/src/modules/src/planner.c @@ -206,7 +206,7 @@ int plan_go_to(struct planner *p, bool relative, struct vec hover_pos, float hov return plan_go_to_from(p, &setpoint, relative, hover_pos, hover_yaw, duration, t); } -int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, struct vec start_from) +int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, bool reversed, bool relative, bool relative_yaw, struct vec start_from, float start_yaw) { p->reversed = reversed; p->state = TRAJECTORY_STATE_FLYING; @@ -216,17 +216,28 @@ int plan_start_trajectory(struct planner *p, struct piecewise_traj* trajectory, if (relative) { struct traj_eval traj_init; trajectory->shift = vzero(); + trajectory->shift_yaw = 0; if (reversed) { traj_init = piecewise_eval_reversed(trajectory, trajectory->t_begin); } else { traj_init = piecewise_eval(trajectory, trajectory->t_begin); } + + // translate trajectory to starting point struct vec shift_pos = vsub(start_from, traj_init.pos); trajectory->shift = shift_pos; + + if (relative_yaw) { + // compute the shortest possible rotation towards trajectory start yaw to current yaw + float traj_yaw = normalize_radians(traj_init.yaw); + start_yaw = normalize_radians(start_yaw); + trajectory->shift_yaw = shortest_signed_angle_radians(traj_yaw, start_yaw); + } } else { trajectory->shift = vzero(); + trajectory->shift_yaw = 0; } return 0; @@ -244,6 +255,8 @@ int plan_start_compressed_trajectory( struct planner *p, struct piecewise_traj_c struct traj_eval traj_init = piecewise_compressed_eval( trajectory, trajectory->t_begin ); + + // translate trajectory to current position struct vec shift_pos = vsub(start_from, traj_init.pos); trajectory->shift = shift_pos; } else { @@ -251,4 +264,4 @@ int plan_start_compressed_trajectory( struct planner *p, struct piecewise_traj_c } return 0; -} +} \ No newline at end of file diff --git a/src/modules/src/pptraj.c b/src/modules/src/pptraj.c index ea7c4be493..9fb5dca19c 100644 --- a/src/modules/src/pptraj.c +++ b/src/modules/src/pptraj.c @@ -329,6 +329,20 @@ struct traj_eval poly4d_eval(struct poly4d const *p, float t) return out; } +void traj_eval_transform(struct traj_eval *ev, struct vec shift, float rotation) +{ + struct mat33 rotator = mrotz(normalize_radians(rotation)); + + // rotate position, velocity, acceleration + ev->pos = mvmul(rotator, ev->pos); + ev->vel = mvmul(rotator, ev->vel); + ev->acc = mvmul(rotator, ev->acc); + + // shift + ev->yaw += normalize_radians(rotation); + ev->pos = vadd(ev->pos, shift); +} + // // piecewise 4d polynomials // @@ -343,9 +357,15 @@ struct traj_eval piecewise_eval( struct poly4d const *piece = &(traj->pieces[cursor]); 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); + + // evaluate polynomial + struct traj_eval ev = poly4d_eval(&poly4d_tmp, t); + + // rotate and shift output of polynomial + traj_eval_transform(&ev, traj->shift, traj->shift_yaw); + + return ev; } t -= piece->duration * traj->timescale; ++cursor; @@ -353,7 +373,8 @@ struct traj_eval piecewise_eval( // if we get here, the trajectory has ended struct poly4d const *end_piece = &(traj->pieces[traj->n_pieces - 1]); struct traj_eval ev = poly4d_eval(end_piece, end_piece->duration); - ev.pos = vadd(ev.pos, traj->shift); + traj_eval_transform(&ev, traj->shift, traj->shift_yaw); + ev.vel = vzero(); ev.acc = vzero(); ev.jerk = vzero(); diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c index 0a3c1d8cfd..6ce6a4a584 100644 --- a/src/modules/src/stabilizer.c +++ b/src/modules/src/stabilizer.c @@ -77,6 +77,10 @@ static setpoint_t tempSetpoint; static StateEstimatorType estimatorType; static ControllerType controllerType; +// Controls whether the HL commander knows the state. If not, trajectories are +// initiated at the most recent setpoint. +static bool doHLTellState; + static STATS_CNT_RATE_DEFINE(stabilizerRate, 500); static rateSupervisor_t rateSupervisorContext; static bool rateWarningDisplayed = false; @@ -295,6 +299,10 @@ static void stabilizerTask(void* param) stateEstimator(&state, stabilizerStep); + if (doHLTellState) { + crtpCommanderHighLevelTellState(&state); + } + const bool areMotorsAllowedToRun = supervisorAreMotorsAllowedToRun(); // Critical for safety, be careful if you modify this code! @@ -370,6 +378,10 @@ PARAM_ADD_CORE(PARAM_UINT8, estimator, &estimatorType) * @brief Controller type Auto select(0), PID(1), Mellinger(2), INDI(3), Brescianini(4), Lee(5) (Default: 0) */ PARAM_ADD_CORE(PARAM_UINT8, controller, &controllerType) +/** + * @brief Whether high-level commander tells state (Default: 0) + */ +PARAM_ADD(PARAM_UINT8, hlTellState, &doHLTellState) PARAM_GROUP_STOP(stabilizer)