Added support for fully-relative trajectories

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);

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;
 };

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)
  */

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,11 +255,13 @@ 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 {
 		trajectory->shift = vzero();
 	}
 
 	return 0;
-}
+}

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,17 +357,24 @@ 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;
 	}
 	// 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();

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)