Enable 3D mode in quadcopters

[jessweakly]

Enable 3D mode for quad copters
I know that there is a DShot_3D_enabled (#17488), however this does not seem to be fully implemented to me. The throttle seems to still be in the range 0 to 1, but with the lower half being reverse thrust. I set this up, however, I cannot get the quad flying stably with this mode. One of the main issues seems to be that despite using the DShot, the motors do not share a common zero - there is no way for them to all be stopped at the same time, which makes the quad highly unstable.

I also tried configuring my ESCs in the BLHeli Configurator to use bidirectional PWM instead of bidirectional DShot. The difficulty here is that still, the ESC do not all have a common zero. I have tried using the ESC calibration in PX4 to help with this, but the behavior is the same. I also do not see any parameter that will allow me to specify to the autopilot that the ESC are reversible.

Preferred Solution
What I would like is a parameter that I can use to allow the system to accept mavlink attitude_setpoint commands with thrust in the range -1 to 1. The ideal solution (for my application) would also allow me to restrict the range of the RC to 0 to 1 for handpiloting, and to only allow 3D when in offboard mode. A sufficient solution would have a parameter that allows the RC to arm while at mid-throttle and does not use negative thrust commands in the attitude controller (unless negative thrust is specified).

Possible Approaches

• make the DShot work fully, so that each motor behaves similarly
• a way to specify to the robot that the PWM ESC are bidirectional
• alternatively, a button to quickly reverse the thrust direction of all the motors via a mavlink command.

Additional context
I know that rovers support this type of behavior, so I believe it to be possible for quads. Additionally, I have seen mention of reversing the thrust instantaneously for VTOL back transitioning https://docs.px4.io/v1.9.0/en/config_vtol/vtol_back_transition_tuning.html. For my application, I want to be able to do something fairly similar, instantaneously applying reverse thrust. However, I want to be able to do this for a quad.

I am not really sure if this is a feature request or a bug, but I will greatly appreciate any insight. Additionally, I am willing to implement this feature myself, but would greatly appreciate insight on how best to approach this problem from someone more familiar with the flight stack. I am hoping to receive guidance on where in the code I should start looking, as well as whether there is some parameter I missed that already solves this.

Thanks in advance for your consideration!

[jessweakly]

Just to follow up, I analyzed the data from some flight tests. In 3D mode, I noticed that the actuator commands plot in flight review looks very similar to when 3D mode is disabled, which caused the quad to immediately crash. In both cases, the actuators are being send commands in the full range 1000-2000 when the quad is supposed to hover. However, for the 3D mode, I would like the actuator commands to be restricted to the range 1500-2000 for hovering, since reverse thrust should only be used when specified directly.
3D mode flight review: https://review.px4.io/plot_app?log=eb8344a7-f91a-47ec-9293-a5bc50c0e501
Normal flight review: https://review.px4.io/plot_app?log=ff7d2063-13bc-40e3-9fba-ddd12a3b9fe4

Some more searching led me to this forum which suggests I need to modify the mixer (https://discuss.px4.io/t/how-to-modify-the-mixer-for-a-quadrotor-to-account-for-bidirectional-motors/6571/8 ). From the documentation on mixers I am not sure how to modify it. Is there a way to specify more parameters for R: besides the geometry?

[bkueng]

Hi

However, for the 3D mode, I would like the actuator commands to be restricted to the range 1500-2000 for hovering, since reverse thrust should only be used when specified directly

For pwm this is only implemented with the new mixing system (https://docs.px4.io/master/en/config/actuators.html, setting motors to be bi-directional), for DShot it should work if you set DSHOT_3D_ENABLE. What do you mean by common zero? Do you have a log with DShot?

The currently main missing part is handling in the upper layers, in particular negative thrust setpoints, depending on how you want to control the drone.

[jessweakly]

Hi,

Thanks for your reply. It seems that the bi-directional DShot is working well in my bench tests (https://review.px4.io/plot_app?log=bce64554-ac3a-4d9d-bb98-e1bf9d8b19ca), but during flight, it is as you say that the upper layers are not correct. I was mistaken regarding their common zero - the issue seems to be that the attitude controller is sending both negative and positive directions to the motors which is undesireable.

I have found that the thrust is expected to always be in the range 0 to 1 with 0.5 being the actual no movement value of the reversible motors. Here is the data from an offboard control test where I command the robot to hover at a certain point, while holding the robot by hand with the props off, scaling the commanded throttle appropriately (https://review.px4.io/plot_app?log=00688ce0-b8e3-4028-aceb-294abc3b4ebf). During the test, I move the robot around its setpoint It seems that when the throttle commanded is 0.5, the motors quickly alternate between forward and backwards rather than turning off. This is not what I want.

What I would like to be able to do is to modify the firmware such that if DSHOT_3D_ENABLED is true and the thrust is > 0.5 then no motor should ever output a negative throttle (or alternatively, I could rescale the thrust to be in the range -1 to 1).

Alternatively, I wonder if I can add a parameter into the firmware similar to the one used for Betaflight's 3D on a switch (betaflight/betaflight@d1a197f#:~:text=%7D-,if%20(feature(FEATURE_3D)%20%26%26%20isModeActivationConditionPresent(BOX3DONASWITCH)%20%26%26%20!failsafeIsActive())%20%7B,%7D,-if%20(FLIGHT_MODE(HEADFREE_MODE ). I would be fine with having my offboard controller send system('rosrun mavros mavparam set NEW_3D_PARAM') when needed. Or, more elegantly, to flip this parameter when throttle < 0 and then rescale the throttle.

I would greatly appreciate any feedback on these ideas, especially regarding which may be the most straightforward to implement. If you could give me a pointer to the "correct" place to adjust behavior of the upper layers that would be fantastic. I would love to make these changes in such a way that they may be useful to others in the future.

[bkueng]

That makes sense. I suggest you switch to the new mixing system (https://docs.px4.io/master/en/config/actuators.html), which is in good shape for multirotors (and others too). That way you avoid doing work for something that's going to be replaced.
In particular the thrust range is well-defined: instead of 0.5 mapping to 0, [0,1] always maps to upwards thrust, whereas if you want downwards thrust you need to use [-1,0] (the thrust vector being reversed though, as it's in NED).

To avoid having different directions for individual motors you need to look at the desaturation logic. Currently for bi-directional motors the actuator min/max is set to -1/1, but what you want (if I understand correctly) is basically to limit the actuator range to [0,1] and [-1, 0] depending on the sign of the collective thrust. It probably makes sense to add a new class for such an implementation.

[jessweakly]

Thanks so much for your guidance, the new mixing system sounds really helpful and I think you have me pointed in the right direction. I am a bit confused on how to get to the new mixing system though. I still have the motors tab in QGC rather than the actuators tab. Do I need to build my firmware from a different commit to get it? Or do I need a different firmware target (currently using px4fmu-v4_default for a Pixracer)? Sorry to trouble you with what is likely a basic question but it will be a great help to me if you can clarify this. Thanks again!

…

On Wed, Mar 2, 2022 at 3:16 AM Beat Küng ***@***.***> wrote: That makes sense. I suggest you switch to the new mixing system ( https://docs.px4.io/master/en/config/actuators.html), which is in good shape for multirotors (and others too). That way you avoid doing work for something that's going to be replaced. In particular the thrust range is well-defined: instead of 0.5 mapping to 0, [0,1] always maps to upwards thrust, whereas if you want downwards thrust you need to use [-1,0] (the thrust vector being reversed though, as it's in NED). To avoid having different directions for individual motors you need to look at the desaturation logic <https://github.com/PX4/PX4-Autopilot/blob/master/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp>. Currently for bi-directional motors the actuator min/max is set to -1/1, but what you want (if I understand correctly) is basically to limit the actuator range to [0,1] and [-1, 0] depending on the sign of the collective thrust. It probably makes sense to add a new class for such an implementation. — Reply to this email directly, view it on GitHub <#19252 (comment)>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/AEC2MMOE73LM5PUYPTN3UJLU54PV5ANCNFSM5PH3NOTQ> . Triage notifications on the go with GitHub Mobile for iOS <https://apps.apple.com/app/apple-store/id1477376905?ct=notification-email&mt=8&pt=524675> or Android <https://play.google.com/store/apps/details?id=com.github.android&referrer=utm_campaign%3Dnotification-email%26utm_medium%3Demail%26utm_source%3Dgithub>. You are receiving this because you authored the thread.Message ID: ***@***.***>

[bkueng]

You need recent master for both PX4 and QGC and then enable it via SYS_CTRL_ALLOC.

[jessweakly]

Great, thank you!!

[jessweakly]

The new mixer seems to take care of some of the smaller issues I had been facing (it seems that I should be able to fly normally now with thrust in the range 0 to 1), but I am having trouble taking advantage of the reversible thrust.

what you want (if I understand correctly) is basically to limit the actuator range to [0,1] and [-1, 0] depending on the sign of the collective thrust

This is exactly what I want. I am still struggling to figure out how to allow the system to receive a negative thrust.
I am sending commands using setpoint_raw/attitude, which sends a msg of type AttitudeTarget.
So far, I have made the following modifications to the firmware:

• Line 94 of ATTITUDE_TARGET.hpp: replace the norm with using the last element of the vector.
• Mc_att_control_main line 214: thrust range lower bound shifted from 0 to -1
• MulticopterRateControl 184: thrust range lower bound shifted from 0 to -1

However, when I send an AttitudeTarget with negative collective thrust command to the robot, it is still showing it as 0 in vehicle_attitude_setpoint_0, vehicle_rates_setpoint_0, and vehicle_thrust_setpoint_0. However, the positive values I am sending working very well.

Can you give me any guidance regarding how the robot may receive a negative thrust value?

Thanks

[jessweakly]

I figured it out! Still need to verify that the robot works in flight tests but benchtop ones are very promising. Thanks again for your help.

For anyone who is interested, it turns out that the remaining issue was in the offboard control mavros side of things. On my laptop sending the setpoints I had to make a small modification to the mavros repo file: https://github.com/mavlink/mavros/blob/c035b8ff1ffb042db235648371f5d7d2e531fae4/mavros/src/plugins/setpoint_raw.cpp
line 306 thrust = std::min(1.0, std::max(0.0, req->thrust * thrust_scaling));
Replace the 0.0 with -1.0

[bkueng]

Cool!
A word of advice when you get to the desaturation changes: be careful, things can go wrong in bad ways.

[jessweakly]

Thanks for the words of caution - I will be careful.

I am wondering if an alternative, perhaps less risky method would be to modify the bounds of the mixer module based on the sign of the thrust instead (MixingOutput::setAllMinValues(uint16_t value)). I tried doing this from dshot.cpp each time a custom binary PX4 param was modified. However, this was with a modified version of the latest stable release. The response of the motors when the parameter was changed seemed to be as I desired. The problem is that I was modifying the parameter by system('rosrun mavros mavparam set MY_PARAM' val) from the my offboard station's script, and it seemed to cause a large delay (~0.7 seconds), probably due to comms and system call overhead. That leaves me with two questions:

1. would setting a custom PX4 parameter from the mc_att_control when the sign of the thrust has changed be fast enough? I think this should be true if my issue was indeed comms related but false if modifying PX4 params is inherently too slow for mid-flight
2. Would you foresee anything problematic with this approach? I am hoping in this way I might avoid the desaturation changes.

[bkueng]

I would not do that, it just won't be reliable. If you do it properly we can add it upstream, so it gets maintained and others can make use of it as well.