Add Lee controller

[Khaledwahba1994]

This adds the Lee controller from

Taeyoung Lee, Melvin Leok, and N. Harris McClamroch
Geometric Tracking Control of a Quadrotor UAV on SE(3)
CDC 2010

which is similar to Mellinger, but has provable stability guarantees.

Tested with manual flight (cfclient), trajectory tracking (crazyswarm2), and simulation (crazyswarm2 simulator).

[knmcguire]

Thanks for this PR! It is a big change so we will need time to review this, so hopefully that will be possible in the next couple of weeks. Also I think we need to discuss internally as this is now the 5th controller as well in the firmware so I'll mark it for triage

[knmcguire]

I'll soon have time to test this out, however I see that the dwm1000 lib submodule has changed commit hash... was this the intention? I see something similar with the system id PR as well.

[knmcguire]

I also was able to test this out and it seems to work, however it is very aggressively tuned. If I use a crazyflie without the thrust upgrade kit it flips immediately upon take off from the cfclient. Any chance that this can be tuned down a bit as a default value?

Also it would be nice to add the controller to the controller page of the documentation (source in the /doc folder)

[whoenig]

• submodule: certainly not intentional and I am also not sure how this even happened. I should be able to revert this.
• Flight tests: How did you fly? Manually with a controller in cfclient? That's at least how we also tested it (but we are very poor pilots, so perhaps we didn't notice how aggressive this is). Otherwise our main testing was with a mocap and trajectory tracking. Everything we tested is on a "stock" Crazyflie with the standard battery and standard motors. We have slightly different gains for the upgraded motors.
• Documentation: yes, good idea!

[knmcguire]

I tested it with the command based flight control panel with just a takeoff and a forward and up and down motion.

I'll try again with another crazyflie perhaps see if it is not a motor issue, but then it makes sense that it's a bit wobbly with the upgraded motors if those needed to be updated

[jpreiss]

Is there any outside contribution that could help move this along? Having a "geometric PID" controller with SI outputs in the firmware would be useful for a current research project. Would also prefer a lower-gain tune.

[whoenig]

I did the "smaller" changes (docs, reverting the unrelated submodule, fixing a build error caused by recent changes in master). @Khaledwahba1994 and @den-schmidt, could you double check the gains and perhaps fly with cfclient and the flow deck (no mocap) to verify the observed flipping behavior? My guess is that something went wrong when merging our internal config files to the default values in the firmware.

[knmcguire]

Thanks for the changes. I'll be able to test it out this wednesday again to double check any hardware faults. Could you perhaps also show me where I can find/know about the gains for the upgraded motors?

[knmcguire]

Alright I've done another test where I use the cfclient command-based flight controller on the flight control tab for a simple take-off and landing (uses high-level commander functions) with lighthouse

This is the PID controller on the same firmware on this PR:

20240228_133218.mp4

This is the Lee controller

20240228_133244.mp4

It doesn't flip at take off but it has a weird hop after landing and it flips afterwards. This is pretty repeatable for every landing for my crazyflie. Also the position hold it also shakes a little, indicating that the gains are still high.

[jpreiss]

I am really excited to start using this controller and forget about massThrust forever! But I am concerned that these lines introduce an unnecessary coupling between the quadrotor's moments of inertia and the controller gains:

crazyflie-firmware/src/modules/src/controller/controller_lee.c

Lines 245 to 254 in 8268236

	self->u = vadd4(
	vneg(veltmul(self->KR, eR)),
	vneg(veltmul(self->Komega, omega_error)),
	vneg(veltmul(self->KI, self->i_error_att)),
	vcross(self->omega, veltmul(self->J, self->omega)));
	control->controlMode = controlModeForceTorque;
	control->torque[0] = self->u.x;
	control->torque[1] = self->u.y;
	control->torque[2] = self->u.z;

Here we define torque = gain * error, so the gains will need to be scaled a lot for larger quadrotors.

As I argued in #537 (comment) and #537 (comment), using mass/inertia-normalized units means the PID gains have the interpretation of "aggressiveness" independent of the quadrotor properties. For example, the KR gain means "if I am 1 radian away from the desired attitude, I should accelerate at KR radians/sec/sec towards the desired attitude."

Even though the Bitcraze team decided to go with SI units, we can still retain most of the benefits by working with the normalized model within the controller and converting to thrust/torque only at the interface boundaries. Note that you are already doing this for the thrust part:

crazyflie-firmware/src/modules/src/controller/controller_lee.c

Lines 139 to 148 in 8268236

	struct vec F_d = vadd4(
	acc_d,
	veltmul(self->Kpos_D, vel_e),
	veltmul(self->Kpos_P, pos_e),
	veltmul(self->Kpos_I, self->i_error_pos));
	struct quat q = mkquat(state->attitudeQuaternion.x, state->attitudeQuaternion.y, state->attitudeQuaternion.z, state->attitudeQuaternion.w);
	struct mat33 R = quat2rotmat(q);
	struct vec z = vbasis(2);
	control->thrustSi = self->mass*vdot(F_d , mvmul(R, z));

I propose that the attitude part should look similar.

[knmcguire]

@jpreiss we have indeed expressed our interest of going to go for SI units but implementation has been quite on hold and currently there are other priorities, so it's fine of adding it in legacy way.

What is currently blocking this PR is the weird behavior we see that still needed to be investigated. The bare minimum we'd like to see is a stable take off and landing from the cfclient's command flight panel with all positioning systems, and currently we are seeing issues with the weird hop and flip at the end with lighthouse.

[jpreiss]

Sorry if my previous comment sounded like a request for a big change - I only meant to request a couple of lines change so this controller sets torque = inertia_matrix * (KR * error + ...) instead of torque = (KR * error + ...).

As a data point, I tested this controller in a mocap system and it worked OK, but I did see find the attitude control a bit jittery. I think it's unusual that the attitude integral gain KI is larger than the proportional gain KR - this is very different from the Mellinger controller tuning, even though the controllers are nearly the same architecture. But I wasn't able to significantly improve upon this tuning with few rounds of trial and error.

[knmcguire]

Ah oke. I'll mark this for our next triage then to see about the next steps. I'm not sure how to fix it either, but if mocap works maybe we can be a bit more lenient. However, we are a bit worried about the surplus of controllers that is now difficult for us to maintain...

We'll let you know the outcome!

[knmcguire]

So sorry for the delay.

We have agreed internally that we will merge this as this is a very good valued addition to the firmware. We will put a pin in the dicussion on how to handle the current overload of controllers but we won't bring that worry in for now. Just be aware to maintain this controller to test it out every once in a while if it still works well in master.

Also, just let us know before if you'd like to implement another controller to see what we have decided to do for the controllers.

Thanks and sorry again for the wait!