Option to use SI units in controller/power-distribution interface

[jpreiss]

This issue refers to: master...jpreiss:si-units. The code is not ready to merge into master yet, but it's enough to give an idea.

I propose to add a new interface between controller and power distribution using SI units. In the existing firmware, the units are related to the fixed-point motor power input unit. This is propagated into the controller gain parameters, which do not have any clear physical meaning.

In the new interface, the controller outputs a desired angular acceleration and a desired normalized thrust (thrust divided by mass). The power distribution section is now aware of the Crazyflie's mass, geometry, moments of inertia, and the torque:thrust ratio of the propellers. It uses this system-ID information to compute how much thrust each motor should produce in Newtons.

The result is that the PID controller gains have meaningful non-dimensionalized units; for example the position P gain is (m/s^2)/m or just 1/s^2 - it tells how fast we should accelerate per unit of distance away from the setpoint.

The code is rough:

• I only applied the changes to the Mellinger controller in trajectory-tracking mode. There are assert(false)s in the other branches.
• I'm not sure if platform.h is the right place to put the system-ID info.
• I feel that the code for switching between SI and normal modes is clunky, but we could get rid of that if we convert all the controllers to SI.

If other people want this feature, I hope that other contributors who are more familiar with the other controllers could apply the same process. The main challenge is preserving the controller gain values so that the observable behavior of the CF doesn't change. I documented the process of how I did that here.

[krichardsson]

Yes please! We have been discussing this for years, but have never got started.
I like the approach of adding a parallell API, even-though it might add some noise and duplication to the code. It will enable an orderly transition.

I'm not sure if platform.h is the right place to put the system-ID info.
First some background; The platform concept was added when we released the Roadrunner (a tag) and was intended to create an abstraction for multiple hardware platforms with different functionality, hw implementations, sensors and other properties.
In our lingo we currently have two platforms: tag and CF (quad rotor). Within each platform there are multiple deviceTypes: for instance CF2.0, CF2.1 and the Bolt.
A FW that is built for one platform, should work on all deviceTypes of that platform, so the same CF FW works on all our quads.
The deviceType is stored permanently in the physical device and is used when booting the device to identify the device type, verify that the FW is compatible and pick the correct configuration.

I'm not completely sure where to put the physical properties either, but I think it makes sense to add them to the platform like you did, and that it is a good step forward. Collecting them in one place will simplify changes in the future, if we discover that it does not work out. There is a slight complication on the horizon though, in that the Bolt will require physical properties per individual as opposed to per deviceType, but let's ignore that for now.

[jpreiss]

Re: physical properties: platform.h is already written in OOP style, i.e. not exposing platformConfig_t directly but instead giving access via platformConfigGet*() functions. I followed that design for the new data. That should make it easy to add support for diverse Bolt builds in the future without changing any of the dependent code in the stabilizer. It would also be useful to add ability to compute a rough estimate the moments of inertia and the torque:thrust ratio from the mass and arm length, since those are hard to measure.

Re: parallel APIs: Since the data type passed from controller to power distribution is different, and the user is allowed to change controller types at runtime, added complexity seems unavoidable. One alternative to my attempt would be to make a tagged union containing both control_t and control_si_t, but that just pushes the same added complexity into the power distribution function.

Seems like once we resolve these issues, there won't be too much remaining work besides converting the other modes of the Mellinger controller to SI units. I can talk to @whoenig about that since I'm not 100% clear on the intended behavior for all the different possible modes.

[whoenig]

The other modes are just for manual flight and can be tested using cfclient, for example. I think this change is pretty important (and I do have a "hacky" version of that with an improved force model of the propellers for that in a local branch for our ICRA2020 paper.) I would be even willing helping to tune the mellinger controller again if that's the only thing that is holding us back.

[jpreiss]

Sorry if my original post wasn't clear - I already was able to preserve the original controller gains as documented in https://github.com/jpreiss/crazyflie-fix-units. My problem is that I don't understand the intended semantics of the manual control modes.

If setpoint->mode.x != modeAbs then we determine the x and y components of target_thrust using the roll and pitch components setpoint->attitude. Then:

• If we aren't in altitude-hold mode, we set target_thrust.z to 1. Why? If the stick is meant to control absolute roll and pitch angles, shouldn't we be setting target_thrust.z to sqrt(1 - (target_thrust.x)^2 - (target_thrust.y)^2)?
• If we are in altitude-hold mode, we set target_thrust.z according to a PID + feedforward control law. But this implies that the stick is not controlling absolute roll and pitch angles: Suppose the set roll and pitch are both nonzero. In the case when the true altitude is exactly as desired, target_thrust.z will be small. In the case when the true altitude is much lower than desired, target_thrust.z will grow, so the direction of the target_thrust vector will be closer to straight up. This implies that the stick is more controlling "amount of thrust in the world x and y axes" rather than actual angles.

Is my understanding correct?

[whoenig]

This code was never meant to distinguish altitude-hold mode. Instead, the if-condition refers to the timeout condition in the commander.

• For the first case (no timeout), I agree that this should not be 1 and your equation instead.
• For the second case (timeout), the assumption was that the desired roll/pitch angles are zero (set by the commander). I agree that we have a problem here if somebody tries to use those modes in another way.

In general the mellinger controller is not well tested for any "modes" other then the one used by the high-level commander. We mostly implemented the other cases in order to simplify gain tuning for new quadrotors (where one can tune the attitude gains with manual flight first).

[jpreiss]

OK, makes sense. It would be useful to document the intended semantics of the stab_mode_t members of setpoint_t somewhere.

We have some hardware issues with our Vicon system at the moment, but once those are resolved I should be able to test promptly.

[jpreiss]

FYI, I am still working on this. I tested this feature and there was a firmware crash. I will try to debug it in the coming weeks.

[krichardsson]

Thanks for the info!

[jonasdn]

Hello! Is there any new progress on this?

[whoenig]

I have a prototype of this as well, if needed. However, I didn't bother updating the gains, so my version allows (new) controllers to use SI units.

[jpreiss]

I would really like to finish this soon. It's lower priority because it's not part of a research project, but i would like to leave open if that's ok.

[whoenig]

@jpreiss I had a look at your code linked at the beginning at the issue, as I also need this feature to get upstream. As I said, I have an implementation also, with a few pro's and con's. I updated control_t to be:

typedef enum control_mode_e {
  controlModeLegacy      = 0, // legacy mode with int16_t roll, pitch, yaw and float thrust
  controlModeForceTorque = 1,
} control_mode_t;


typedef struct control_s {
  union {
    struct {
      // legacy part
      int16_t roll;
      int16_t pitch;
      int16_t yaw;
      float thrust;
    };
    struct {
      float thrustSI;  // N
      float torque[3]; // Nm
    };
  };
  control_mode_t controlMode;
} control_t;

This simplifies the logic a bit, since existing controllers don't need to change at all. Note that for the new control mode I used N, rather than normalized output. I am not sure if that has any important implications for tuning or numerical stability?

The other big change I have is a proper model for PWM->Force and estimate of the current achievable maximum force (depends on the battery state of life). These are both pretty important for better flight performance.

Let me know how I can help to move this forward:-)

[knmcguire]

I will restart the discussion on this, as my work on the simulation has some effect on this too, especially if I want to do software in the loop.

[jpreiss]

I disagree with this design choice in whoenig's updated control_t:

struct {
  float thrustSI;  // N
  float torque[3]; // Nm
};

I believe the units should be normalized thrust and angular acceleration, as described in my earlier write-up. Otherwise we push an unnecessary dependence on system-ID information into the controller.

Sorry I am too busy writing my dissertation to do any development or testing for this right now, but I can contribute to design discussions.

[whoenig]

Good point, especially for the Bolt support this is a better solution. I originally used SI units for easier interpretability when comparing to a simulator (that was using SI units, also).

[knmcguire]

Probably normalization is indeed a good middle ground here and I would say that makes also sense from a simulation point of view, as scaling will always be required anyway, especially if we want to use the same controller for different simulators using different thrust models.

Anyway.. the triage meeting is unfortunately postponed for a few weeks but me and wolfgang might be able to discuss this in more detail during my visit in Berlin!

[jpreiss]

For purposes of testing we might want the simulator/firmware interface to be motor thrusts anyway, so we also test power distribution. Right now power distribution is simple but I can imagine wanting to make it more complex to achieve best performance (e.g. with box-constrained convex optimization). Also we may want to test controllers that directly map states to thrusts, e.g. in some reinforcement learning setups.

[whoenig]

Discussion with @knmcguire and @Khaledwahba1994:

• Lets start with 2 modes: Legacy and normalized thrust/torque
• We will provide a draft PR for the power distribution and later a new controller (e.g., Lee), that uses the normalized thrust/torque mode
• Later, this new controller will replace the current Mellinger implementation
• Later, the PID controller should be changed to use the new mode and the legacy mode be removed to reduce the maintenance effort.