[WIP]: Rate control: introduce torque_setpoint, angular_accel_setpoint, and inertia matrix

[jlecoeur]

This is a first step towards more unit-aware controllers.

As of now, the output of the rate controller does not have unit (this is a normalized torque setpoint). Although this makes things simple and has always worked like it, this has a few limitations:

• This will limit the use of actual vehicle properties in a (future) control allocation scheme.
• There were a few requests for the possibility to provide torque setpoints in N.m.
• Not having a model of the vehicle inertia may be problematic for applications where inertia changes over time (with fuel consumption, package delivery, etc).
• Model-based control is difficult because of the lack of units.

I introduced several new things in this PR. I would like to hear your opinion on a few additions. (Note that with the default parameters, it should not break anything.)

• computation of vehicle_angular_acceleration (in rad/s^2) and publication on a dedicated uORB topic.
  @dagar I did this in the sensor module although it is not a real sensor because I think this can be useful for more things than the MC controller, it is disabled by default.
  @MaEtUgR we may use this data for the D term in the rate controller instead of computing the derivative inside the controller. This may save some computation in VTOLs because both the MC and the FW controller compute rate derivatives.
• addition of vehicle_angular_acceleration_setpoint, vehicle_thrust_setpoint, vehicle_torque_setpoint topics, with proper units.
• the inertia matrix is included in the controller module.
  Eventually this could be moved into a separate module. Either MC or FW or VTOL controller would publish an angular acceleration setpoint and a feedforward torque setpoint. The new module would subscribe to these topics and then compute a torque setpoint based on its model of the vehicle.
• split of the PD terms and I-FF terms (see the changes in RateControl.cpp).
  I use the P and D term to generate an angular acceleration setpoint, which is later mapped to torque via the inertia matrix. The integral and feed-forward terms are directly generating torque values (called torque_feedforward).
  If the I term was used to generate acceleration setpoints, you would have -- for an unbalanced system in equilibrium -- a steady-state angular acceleration setpoint, which does not make sense.

From there, I multiplied the P and D rate gains by 120 (roll), 120 (pitch) and 35 (yaw), and divided the inertia by the same amount.
This way the behaviour is not changed, but units are now OK (see on the graphs below how the angular acceleration somewhat matches the angular acceleration setpoint). I obtain this during steps on roll, pitch and yaw:

The values of 120 and 35 were not chosen arbitrarily, we can in fact estimate them.
From a gazebo log, we can compute the time derivative of the vehicle angular acceleration and of the torque setpoint:

A quick eye fit gave me the inertia matrix

I = [1/120      0        0   ]
    [  0      1/120      0   ]
    [  0        0      1/35  ]

Let me know your opinion, shoot!

[dagar]

computation of vehicle_angular_acceleration (in rad/s^2) and publication on a dedicated uORB topic.
@dagar I did this in the sensor module although it is not a real sensor because I think this can be useful for more things than the MC controller, it is disabled by default.

One possibility I'm going to consider (if we ever manage to kill off DriverFramework - #12783) is moving the filtering to vehicle_angular_velocity and vehicle_acceleration rather than in each driver (we only need it for the primaries). Newer drivers using FIFOs (arrays of 4 or 8 raw samples) can publish that data to process downstream (#13103). I mention this now because it might be worth considering using that raw data to compute angular acceleration.

Related comment from @priseborough - PX4/PX4-ECL#621

The acceleration returned is at the sensor frame. The only way to return acceleration at the body origin would be to use angular accelerations which is problematic due to the issues associated with differentiating rate gyro noise. This was tried in the past using the downsampled data available to the EKF but the resulting data product was noisy when engine vibration was present. It may be possible if we do the differentiation and filtering in the sensor driver on the high rate data before downsampling. This rate acceleration would then be need to be added to the sensor topic used by the EKF.

[zarathustr]

This may be useful for a satellite but seems not to be very necessary for the micro air vehicle. The reason is two-fold: how to measure the accurate inertia matrix; how to adapt this concept for multiple kinds of frames. I think this point is more worthy of consideration when a system identification is taken into account. By then the inertia matrix can be approximately obtained.

[jlecoeur]

Hi @zarathustr, thanks for the comments. I do not agree that this is useless for small UAVs. There were numerous requests for proper units in the controller outputs. Also, there are systems with varying mass and inertia, like delivery drones, that are now being tested or even used in production and that could benefit from an inertia model. Regarding the system identification, I gave a simplified example, but there are much more advanced and accurate techniques available. Finally, you should not worry if you do not want/need or know the inertia of your robot because this is opt-in: the default inertia matrix is identity and thus has no impact.

[jlecoeur]

I cherry-picked commits from #13351

Let's try to get this in before it rots.

@RomanBapst can you check that the toque_sp and thrust_sp publication in FW and MC controllers is the same as what you have tested this morning?

[jlecoeur]

fmu-v2's flash overflows by 3437 bytes after a rebase to master :-(

There is not much left to remove on fmu-v2. I removed px4flow and pwm_out_sim, which is not great.

[jlecoeur]

Holding this off for now, until work has progressed in #13351

[LorenzMeier]

I'm closing this as stale - please open a new rebased PR. Thanks!