vtol: reduce schedule frequency, which causes DSHOT150 problems

[alexcekay]

Solved Problem

Specific ADP ESCs lost communication when switching from MC -> FW in case of using DSHOT150. When switching back from FW -> MC the motors would still not spin.

The communication loss is due to incorrect DSHOT messages on the wire. As can be seen multiple DSHOT frames got packed into one large stream:

The cause of this is that a DSHOT transmission is started while an old one was still in progress. This occured only on DSHOT150 as the other ones are quick enough for this to not happen. The reason for this is a extremly fast scheduling of the DSHOT driver:

All this could be traced back to vtol_att_control being called too often due to it having configured 4 topic callbacks which are triggered by MulticopterRateControl and FixedwingRateControl which are triggered by an update of vehicle_angular_velocity. So one update of vehicle_angular_velocity causes a burst of 4 schedules of vtol_att_control.

Solution

Depending on the VTOL mode only the relevant callbacks are registered. In addition only the torque topics are subscribed as they are published after the thrust topics.

Changelog Entry

For release notes:

Bugfix: Reduce vtol schedule frequency to avoid DSHOT150 problems

Test coverage

• On the bench with an ADP F3 80 and DSHOT
• Flight tested on a Loong VTOL with PWM output

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 40 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +40  +0.0%     +40    .text
  +0.9%     +36  +0.9%     +36    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.1%      -4  -0.1%      -4    ../../src/modules/vtol_att_control/vtol_type.cpp
+0.0%     +38  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  -0.5%     -18  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
+0.0%      +8  [ = ]       0    .debug_frame
+0.0%    +326  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.4%    +330  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
+0.0%    +204  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +2.5%    +235  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  -0.5%      -6  [ = ]       0    task/task_cancelpt.c
+0.0%    +219  [ = ]       0    .debug_loc
  +4.2%    +368  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  -0.0%    -149  [ = ]       0    [section .debug_loc]
+0.0%    +209  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +9.4%    +232  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  -0.0%     -16  [ = ]       0    [section .debug_ranges]
  +1.5%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +100  [ = ]       0    .debug_str
  +0.3%    +100  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
-0.5%      -1  [ = ]       0    .shstrtab
+0.0%     +13  [ = ]       0    .strtab
  +4.6%     +16  [ = ]       0    ../../src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  -0.2%      -3  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.1%     +32  [ = ]       0    [section .strtab]
-0.0%     -16  [ = ]       0    .symtab
  +4.2%     +16  [ = ]       0    ../../src/drivers/magnetometer/memsic/mmc5983ma/mmc5983ma_i2c.cpp
  -5.4%     -48  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.8%     -32  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.1%     +32  [ = ]       0    [section .symtab]
-0.1%     -40  [ = ]       0    [Unmapped]
+0.0% +1.07Ki  +0.0%     +40    TOTAL

px4_fmu-v6x [Total VM Diff: 40 byte (0 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +40  +0.0%     +40    .text
  +0.9%     +36  +0.9%     +36    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.0%      +5  +0.0%      +5    [section .text]
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.1%      -4  -0.1%      -4    ../../src/modules/vtol_att_control/vtol_type.cpp
+0.0%     +38  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  -0.5%     -18  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
+0.0%      +8  [ = ]       0    .debug_frame
+0.0%    +326  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.4%    +330  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
+0.0%    +212  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +2.5%    +235  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.2%      +2  [ = ]       0    task/task_cancelpt.c
+0.0%    +219  [ = ]       0    .debug_loc
  +4.2%    +368  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  -0.0%    -149  [ = ]       0    [section .debug_loc]
+0.0%    +209  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +9.4%    +232  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  -0.0%     -16  [ = ]       0    [section .debug_ranges]
  +1.6%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +100  [ = ]       0    .debug_str
  +0.3%    +100  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
-0.5%      -1  [ = ]       0    .shstrtab
+0.0%     +13  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.7%     +13  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.0%     -16  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.9%     -16  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
-0.1%     -40  [ = ]       0    [Unmapped]
+0.0% +1.08Ki  +0.0%     +40    TOTAL

Updated: 2025-04-17T15:54:47

[PetervdPerk-NXP]

Nice work, @alexcekay, I think my issue #24230 might be a bit related to this but then it affects the fixed wing (fw).

[sfuhrer]

This looks reasonable, and thanks for the in-depth investigation!

• I see no case where we send only thrust and not torque, so having the callback on torque only is ok
• In transition mode the virtual_mc_torque is always published, so no need to have the additional callback on the virtual_fw_torque

Did you by chance make a CPU load comparison w/ and w/o this fix?

[alexcekay]

Sure, did a first CPU load test with the following setup

• v6x
• Generic VTOL
• Test steps: Stabilized mode -> Arm -> MC2FW transition -> FW2MC transition -> Disarm

Without the patch:

With the patch:

So the peak CPU usage is reduced by ~2%.

A second CPU load test with the following setup

• v6x
• Generic VTOL
• Test steps: Stabilized mode -> Arm -> Leave it in MC for ~1 minute -> MC2FW transition -> Leave it in FW for ~1 minute -> FW2MC transition -> Disarm

Without the patch:

With the patch

In the longer run the average CPU usage is also reduced by ~2%.
It can also be seen nicely that the transition does lead to a permanent CPU load increase without the patch. With the patch the load is not affected by the transition.

[alexcekay]

We will do some additional flight testing. Afterwards we can merge this.

[dagar]

I agree with fixing the VTOL scheduling in the first place, but shouldn't we also have some protection in the dshot driver?

[alexcekay]

Hi @dagar,
yes good point. This is a first start to get it fixed in the higher layer (which also helps to reduce CPU load).
For additional robustness a check in the DSHOT driver would also be nice.
I can add this to my list of things to do. Already had a look at it and it will not be super easy as the DMA logic got quite complex with BDSHOT on both STM32 and imxrt.

[PetervdPerk-NXP]

Hi @dagar, yes good point. This is a first start to get it fixed in the higher layer (which also helps to reduce CPU load). For additional robustness a check in the DSHOT driver would also be nice. I can add this to my list of things to do. Already had a look at it and it will not be super easy as the DMA logic got quite complex with BDSHOT on both STM32 and imxrt.

FYI IMXRT doesn't do DMA for dshot and bdshot, it's just "smart" shifter that implements the protocol.

[MaEtUgR]

Schedules the task loop at the beginning once, then initializes the callback at the beginning once and then updates whenever the VTOL mode changes. Registration is only on torque setpoint (thrust is published together with it, is it published before torque?) and only on the fixed-wing one while not hovering or transitioning. Should do what was intended.

[MaEtUgR]

With the risk you'll hate me I added my nit-pick refactors. Take them or not I'm fine either way.

[alexcekay]

Hi @MaEtUgR,
yes looks good. Just one point: We need to initialize _previous_vtol_mode also here:

	if (_vtol_type->init()) {
			update_callbacks();
			_initialized = true;

Otherwise the enum is most likely zero-initialized as it is in the class scope (and thus not containing a valid mode). It would be nice to have it explicit. Alternatively we can init it to in the header file to a defined value.

(thrust is published together with it, is it published before torque?)

Yes exactly it is published before torque.

[dagar]

Hi @dagar, yes good point. This is a first start to get it fixed in the higher layer (which also helps to reduce CPU load). For additional robustness a check in the DSHOT driver would also be nice. I can add this to my list of things to do. Already had a look at it and it will not be super easy as the DMA logic got quite complex with BDSHOT on both STM32 and imxrt.

Ideally we'd protect the timer DMA directly and not clobber anything existing already in route, but even if we just caught the schedule early in the Run() and pushed it back a few millseconds it would probably be good enough.

[dakejahl]

Hi @dagar, yes good point. This is a first start to get it fixed in the higher layer (which also helps to reduce CPU load). For additional robustness a check in the DSHOT driver would also be nice. I can add this to my list of things to do. Already had a look at it and it will not be super easy as the DMA logic got quite complex with BDSHOT on both STM32 and imxrt.

@alexcekay I can help with the dshot driver. Feel free to ping me on Discord and we can discuss it