Copter: add 6DoF support

[IamPete1]

This adds support for copters that can fly in any direction and at any orientation.

https://youtu.be/CXIlZ8mCfGM

This adds a new attitude control and motor back-end and scripting bindings for each. Scripting can be used to set the true vehicle attitude while the existing navigation 'attitude' is used for the thrust vector. Scripting is also used to set the 6DoF matrix for the mixer. I did spend a some time trying to calculate the mixer directly from motor positions and trust vectors, but it all got too complex. The ideal mixer can be found and then loaded with the script. For my demo I used a PSO in MATLAB to find the mix matrix, the mix is not included.

Fixes #10117

[IamPete1]

For Copter + boosters rather than a dedicated 6DoF vehicle we might want some balance at the attitude control level, so it will let the vehicle lean a little in some cases or if it can only boost forwards but not backwards, I suspect all the edge cases there will be at least a complex as this support for dedicated 6DoF vehicles.

[IamPete1]

rebased, rates now use same offsets as angles. Added options to switch off lateral and forward control, so can now go 4DoF like a normal copter, 5DoF and 6DoF. Fixed get_frame_mav_type() so the correct number of motors for the new method, this fixes MP motor test which used the MAV_TYPE to determine the number of motors.

[tridge]

very cool! maybe use HAL_MINIMIZE_FEATURES?

[IamPete1]

rebased and included #15717 and #16204 also put new mixer and attitude control in ENABLE_SCRIPTING ifdefs the mixer requires scripting and that mixer is the only user of the new 6DoF lib.

[IamPete1]

some more flying on this PR https://youtu.be/0p9jmrf1eFM

[agentrfr]

Wouldnt it make more sense to have two contra rotating motors on each hardpoint instead of a single motor and relying on reversing prop direction? You could leave the motor/ESC at min throttle that is within the BLDC torque curve and avoid the giggling issue altogether

[auturgy]

this might interest the Sub guys too: @jaxxzer

[IamPete1]

rebased, now includes mixer

[IamPete1]

changed control priority to be in earth frame, previously it was applying body frame throttle first, but this might not be up. Also fixed a rotation issue for combined roll and pitch offsets.

[1ozzyabroad]

Great concept! Could it be adapted to use a servo controlling a variable pitch prop' to eliminate the "giggles'?

[IamPete1]

Wouldnt it make more sense to have two contra rotating motors on each hardpoint instead of a single motor and relying on reversing prop direction? You could leave the motor/ESC at min throttle that is within the BLDC torque curve and avoid the giggling issue altogether

Then you have to carry 6 extra motors and esc's. But yes that would work.

Great concept! Could it be adapted to use a servo controlling a variable pitch prop' to eliminate the "giggles'?

yes

[rmackay9]

It's an amazing vehicle and it is really great to see this PR. I've put in some comments which are really all just about naming and commenting in the hopes that that makes it easier for other developers to understand what's going on.

The elephant in the room is the mixup between target roll and pitch angles VS forward and lateral accelerations. I hope @lthall will provide some comments about this and whether it's acceptable to do what we've done in this PR or if we need to handle it more strategically.

[IamPete1]

@rmackay9 thanks to the review

re-target roll and pitch VS acceleration, the higher level code cannot tell because we set the targets based on the desired angle and then multiply back by throttle here . That does arbitrarily limit the vehicle but allows the higher level controllers to still apply. If we were un-couple lateral acceleration from vertical throttle so we could go full speed sideways with no vertical thrust, for example, we would then need a new PID to go from lateral acceleration to lateral thrust (Like the Z accel controller does for vertical thrust).

[lthall]

I have been planning for this class of aircraft for a long time now and what you are proposing is exactly what I was thinking we should implement. How we realise it is less clear but what I was thinking was a vehicle specific transformation from target acceleration to a 6dof attitude controller.

For multicopter that would be a roll, pitch, thrust command where we can do what we want with yaw.

For this aircraft it would be a transform of the earth frame acceleration vectors to a body frame thrust command where we can do what we want with roll, pitch, and yaw.

The question is how to best implement this.....
I was thinking a generic 6dof attitude controller that has not only Z thrust but also X and Y.
Then in the position controller the transformation from acceleration to actuators would be done based on the frame. This may mean that we move the acceleration controller for alt hold into the attitude controller. This approach is also supported by the need to run the throttle pid loop at the same rate as the rate controllers. When we did the onion rewrite I was not sure if the acceleration PID should be in the position controller or the attitude controller, I think I put it in the wrong place...

The other advantage of this approach is it would mean that each aircraft only needs to create a new attitude controller and motor mixer.

[IamPete1]

Fixed all review issues and re-tested in realflight.

[tridge]

@lthall is happy for input_quaternion to be removed on the understanding that @IamPete1 will help implement it for the 6DoF backend when it is implemented in the mavlink quaternion control

[rmackay9]

Merged, thanks!

[huangfuluya]

I am making such a copter recently, but I wonder how these values are calculated in the Motors_6DoF.lua
https://github.com/ArduPilot/ardupilot/blob/master/libraries/AP_Scripting/examples/Motors_6DoF.lua
Can someone give an example about how to calculate this matrix for a new copter?

[IamPete1]

@huangfuluya Have you seen the conference talk, https://youtu.be/QUDhnYvH66k ? If you start a thread over on the forum I can give you some pointers.

[huangfuluya]

Thanks，I have watched it many times, but I can't understand it very well. Maybe I haven't described my problem clearly. The matrix I calculated using my understanding is slightly different from the matrix in the demo, I want to know why these differences occur. You mentioned here that you used a PSO in MATLAB to find the mix matrix. Is there any paper or programs about this matrix calculation for reference?

[IamPete1]

@huangfuluya start a thread over on the forum, if you can explain how your calculations are done I can see if I can spot any issue. Or I could run your vehicle through my calculations. https://discuss.ardupilot.org/

[ryrobotics]

Hi Peter! What a great job! I am very interested in this kind of 6 DoF drone, and it can be a great tool for performing inspections. But I am not sure if it is possible to assemble extra visual sensors and onboard computers, like Xavier NX, on this frame. So could you share some drawings/detailed pictures of your drone or give me some hints? Thanks!