Modify yaw behaviour of high level commander for take off and landing

[krichardsson]

When the high level commander performs a take off or landing operation, it goes to yaw=0.
There might be uses cases where the user is interested in taking off to a different yaw or just taking off without changing the yaw.
When landing it also means that it "unwinds" any yawing that has been done during the flight, thus forcing the user to fly trajectories without accumulated yaw. It could be useful to have the option of landing with the current yaw.

The main point of interest seems to be

crazyflie-firmware/src/modules/src/planner.c

Lines 48 to 50 in f574561

	piecewise_plan_7th_order_no_jerk(&p->planned_trajectory, duration,
	pos, yaw, vzero(), 0, vzero(),
	takeoff_pos, 0, vzero(), 0, vzero());

One solution could be to add a yaw parameter to takeoff() and land() with the target yaw. Not sure how to handle "use current yaw" though.

@whoenig @jpreiss Do you have any input?

Changing this behaviour is a breaking change of the current behaviour and we need to be a bit careful about what we do.

[whoenig]

One way to do this would be to add two new commands that include yaw in

crazyflie-firmware/src/modules/src/crtp_commander_high_level.c

Lines 104 to 105 in f574561

	COMMAND_TAKEOFF = 1,
	COMMAND_LAND = 2,

. If the desired value is outside -pi ... pi, we keep the current setpoint yaw. Then, we could deprecate the older commands.

[krichardsson]

Agree, we were also discussing adding new commands.

I think we should not limit the target yaw to +-pi though. If you have yawed 6pi when flying for instance, and land with yaw=pi, the Crazyflie would unwind 5pi while landing. You probably would like to be able to set it to a value higher than pi.

I is maybe better to have a second bool parameter that is "use current yaw"

[whoenig]

This unwinding would actually not work for all controllers. Any SO3 controller (that computes error on quaternion or rotation matrix) would always find the shortest rotation in order to move to the desired yaw. If you want this kind of spinning behavior, I would argue that one should plan a trajectory with that upfront and then use the low-level interface for landing instead.

That being said, I don't see any particular downside from having another bool parameter instead of this implicit encoding I suggested.

[jpreiss]

When the planner transitions into landing state, it uses the current yaw:

crazyflie-firmware/src/modules/src/crtp_commander_high_level.c

Line 326 in f574561

result = plan_land(&planner, pos, yaw, data->height, data->duration, t);

where, under normal conditions, the static variable yaw in crtp_commander_high_level.c has very recently been updated according to the current setpoint:

void crtpCommanderHighLevelGetSetpoint(setpoint_t* setpoint, const state_t *state)
{
  ...
  struct traj_eval ev = plan_current_goal(&planner, t);
  ...
  if (is_traj_eval_valid(&ev)) {
	...
	// store the last setpoint
	pos = ev.pos;
	yaw = ev.yaw;
  }
}

and in pptraj.c, we can see that the traj_eval.yaw member is set by directly evaluating the trajectory polynomial:

struct traj_eval poly4d_eval(struct poly4d const *p, float t)
{
    ...
    out.yaw = polyval_yaw(p, t);
    ...
}

so if user has executed high-level trajectories with a yaw polynomial that "winds" up to 6pi, then the planned landing trajectory will have a yaw polynomial that unwinds all the way back to 0.

In my opinion, the current design of always landing at 0 yaw is an overly restrictive assumption on what users will want to do (probably my own fault!). Instead of adding the single new option of landing at the current yaw, I would go all the way and make the target yaw a float parameter.

We can always use default values, overloads, etc. in the software that is sending high level commands (e.g. pycrazyswarm) to make the common cases easy.