New mixing system

[JMoore5353]

In the current system, the controller outputs (in Nm or N) are mixed using the mixing matrices defined in mixer.h. These matrices combine the output but do not take into account any of the propeller dynamics or parameters.

This means that we are currently taking forces and moments and converting them directly to PWM values by saturation. A better approach might be to use the propeller dynamic equations from the textbook to convert the desired forces and moments to input voltages and then to a throttle setting.

[JMoore5353]

@bsutherland333 @iandareid I have some thoughts I'd like your input on.

I implemented a mixing system that dynamically takes in the number of motors and the motor parameters to create the mixing matrix, using the Eigen library. It works well in sim. However, it dynamically allocates memory on the heap to do this (since the size of the matrices are defined at runtime). We can't use (or would strongly prefer not to use) a heap on the microcontroller, which makes this challenging.

A couple ideas:

• Precompute and hard code some mixing matrices for common frame types.
  • Pros:
    • Doesn't use Eigen / dynamically allocated memory
    • Doesn't need to compute the mixing matrix on the microcontroller
  • Cons:
    • Can't adapt for different motor positions (i.e., if we have a big quad frame vs a small quad frame -- this would change the mixing matrix, but we wouldn't be able to update it unless we recompile).
    • Restricted to frames / configurations that we decide, needing a recompile for other configurations.
• Compute the mixing matrix in rosflight_io node and send each entry in the matrix as a parameter to the firmware
  • Pros:
    • No dynamic memory allocation / extra computation in the firmware.
    • Completely flexible since we can do everything dynamically on the computer
    • Can take advantage of ROS2 array parameters to pass in motor locations (instead of passing them one by one to the firmware)
    • Cons:
      • Have to add >=40 mixing matrix parameters (4 x NUM_MIXER_OUTPUTS) to the firmware param list

I think my vote would be to do both of the above, meaning we have precomputed, hard coded matrices for specific configurations (like it is now), and then add a custom_mixer option that is loaded from params from rosflight_io.

What are your thoughts?

[JMoore5353]

Update: I can use Eigen statically by computing the pseudoinverse using the SVD. I just allocate matrices of the max size and do it that way. It didn't work before because I was trying to use the built in method pseudoInverse(), which computes the pseudoinverse in a different method. I couldn't figure out how to get that other method to allocate memory statically. This SVD method should work well, however.

I think the best course of action then is to keep the computation in the firmware. I will still precompute some mixing matrices for some standard frames, but also give the option for a custom matrix with the values specific to the frame (which should be the recommended route).

[bsutherland333]

I like the idea of keeping the mixing on the firmware. The less responsibilities we assign to rosflight_io the better, since it's supposed to be an io node only. I'm glad you were able to figure out how to compute that without needing heap memory.

How does the user set custom matrices? It would be nice to have this ability for fixedwings as well. STM processors have PWM outputs in common groups, where the output rate for each group must be the same, making mixing more complicated there as well. Having a highly flexible and easy to configure method of changing channels for the mixing will be very useful.

[JMoore5353]

@bsutherland333 That's a great question.

It is still up in the air for now. One task that I have not solved is what the mixing matrix looks like for servo commands (which relates to fixedwing aircraft, but also multirotor configurations that have servos--i..e, quadplane, tricopter, etc.). Servo commands would seem to be mixed fundamentally differently than servo commands would. This is partly because we probably shouldn't be computing forces and moments -> actual commanded servo in the firmware, and that is a highly platform-specific computation. The output of ROSplane is currently throttle setting + control surface deflection, which is easier to mix than, say, desired forces and moments. However, it does need to be customizable.

The current implementation for the custom mixing matrix takes in information about each motor's position relative to the CG and propeller information to compute the mixing matrix. This is admittedly multirotor-focused. To create the custom mixer for fixedwing aircraft, we'd need to create a way that the user could denote which channels (i.e., 1-10) should be at what PWM rate, which channels should be servos vs motors, and the desired mixes for the servo channels.

I agree with you about the rosflight_io node having as few responsibilities as possible. However, one advantage of moving some parameters management to the rosflight_io node is that we can take advantage of ROS2's vector parameters... Which is a very convenient way to store a lot of these parameters that the mixer depends on. (There is no mavlink message I've seen that sends vectors)

[bsutherland333]

@avtoku has some thoughts about this as well, in regards to the fxyz channel input.

[JMoore5353]

Sounds good. There are times we will need more than just the fxyz values, i.e., a quadplane, where we have an fx and a fz, in addition to the xyz moments.

[bsutherland333]

Yeah that's what he was talking about. A mixer that had 6 inputs instead of 4 would be better. Apparently multi rotors with 6 or more motors can be configured to have force input in all three directions (by making the motors non-planar), making it a 6 degree of freedom vehicle which would also benefit from an expanded mixer.

[JMoore5353]

It would be good to move the mixer to the Jetson for flexibility.

Thoughts if we mixed on the Jetson:

• For a dynamically changing matrix, we might want to send mixed commands... Otherwise we'd suffer from latency as we sent 60 parameters to the firmware at each timestep
• We'll add ~60 more parameters to firmware
• Would have to add mixer functionality to rosflight_io (or another node?)

[bsutherland333]

What situations would require a dynamic matrix? And if we're concerned about passing over that much data as parameters, we could add support for matrix or array parameters to the io. Most of our mavlink messages are already custom, so needing to create another custom one wouldn't be a terrible thing.

I think I'm worried about how we could keep the minimum functionality required for a safety pilot to fly an aircraft on the firmware if the mixing isn't happening on there.

[JMoore5353]

Airframes like a tricopter, tiltrotor, etc., where the motors are physically moving would require a matrix that changes based on the angle of the motors.

Depending on how it gets implemented, we might want to add a mavlink message that supports vectors.

The safety pilot concern is something I've been concerned about as well. I am going to look into how PX4 and other platforms mix the tricopter or tilt rotor frames to understand a bit more what they are doing. It might be possible to make some simplifying assumptions for an RC pilot to be able to calculate a mixer (I'm thinking we could assume all yaw comes from the rear motor being rotated in a tricopter, for example).