multicopter hover thrust estimator use vehicle_thrust_setpoint (work in stabilized/manual mode)

[dagar]

Update the hover thrust estimator to use vehicle_thrust_setpoint (body frame) rather than vehicle_local_position_setpoint thrust (NED). This allows it to work in stabilized mode.

https://logs.px4.io/plot_app?log=6df3713e-f511-4e2e-854d-8fc7ab98ec65

TODO:

• review usability and safety in stabilized mode
• battery scaling?

[dagar]

Should we go even lower level and use the allocated_thrust from control_allocator_status?

[bresch]

Should we go even lower level and use the allocated_thrust from control_allocator_status?

Il would improve the estimate on setups that are often saturating, yes. For the other ones, it shouldn't change anything.
Should we maybe change to that once we switch to the new control allocation system completely?

[bresch]

Should we go even lower level and use the allocated_thrust from control_allocator_status?

Il would improve the estimate on setups that are often saturating, yes. For the other ones, it shouldn't change anything.
Should we maybe change to that once we switch to the new control allocation system completely?

[MaEtUgR]

Let's pick this up again. I've not seen the hover thrust estimator fail 👀

once we switch to the new control allocation system completely?

That's done 😃

[github-actions]

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: 728 byte (0.04 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +728  +0.0%    +728    .text
   +16%    +552   +16%    +552    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +2.6%    +176  +2.6%    +176    ../../src/modules/mc_att_control/mc_att_control_main.cpp
+0.0%    +196  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.5%     +18  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  +3.5%    +122  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+0.0%     +56  [ = ]       0    .debug_frame
+0.1% +15.6Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.6%    +662  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +22% +14.9Ki  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+0.0% +1.62Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +1.2%    +254  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +14% +1.40Ki  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  -0.3%      -3  [ = ]       0    task/task_cancelpt.c
+0.1% +4.54Ki  [ = ]       0    .debug_loc
  +1.0%    +414  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +41% +4.13Ki  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+0.1%    +770  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +0.8%     +88  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +15%    +688  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +3.1%      +2  [ = ]       0    task/task_cancelpt.c
+0.0%     +98  [ = ]       0    .debug_str
  +1.4%     +14  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  +0.2%     +21  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  +0.8%     +88  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.0%      +5  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  -0.2%     -30  [ = ]       0    ../../src/modules/mc_rate_control/MulticopterRateControl.cpp
+0.5%      +1  [ = ]       0    .shstrtab
+0.0%     +59  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +3.6%     +59  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +96  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +2.2%     +32  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  +6.8%     +96  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.0%     +16  [ = ]       0    [section .symtab]
-4.3%    -728  [ = ]       0    [Unmapped]
+0.0% +23.0Ki  +0.0%    +728    TOTAL

px4_fmu-v6x [Total VM Diff: 728 byte (0.04 %)]

    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%    +728  +0.0%    +728    .text
   +16%    +552   +16%    +552    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +2.6%    +176  +2.6%    +176    ../../src/modules/mc_att_control/mc_att_control_main.cpp
+0.0%    +196  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.5%     +18  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  +3.5%    +122  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+0.0%     +56  [ = ]       0    .debug_frame
+0.1% +15.6Ki  [ = ]       0    .debug_info
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  +0.6%    +662  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +23% +14.9Ki  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+0.0% +1.63Ki  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +1.2%    +254  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +14% +1.40Ki  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.4%      +5  [ = ]       0    task/task_cancelpt.c
+0.1% +4.54Ki  [ = ]       0    .debug_loc
  +1.0%    +414  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +41% +4.13Ki  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
+0.1%    +770  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +0.8%     +88  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
   +15%    +688  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +3.1%      +2  [ = ]       0    task/task_cancelpt.c
+0.0%     +98  [ = ]       0    .debug_str
  +1.4%     +14  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/FlightTask/FlightTask.cpp
  +0.2%     +21  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  +0.8%     +88  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.0%      +5  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  -0.2%     -30  [ = ]       0    ../../src/modules/mc_rate_control/MulticopterRateControl.cpp
+0.5%      +1  [ = ]       0    .shstrtab
+0.0%     +59  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +3.6%     +59  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
+0.0%     +96  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +2.2%     +32  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  +6.8%     +96  [ = ]       0    ../../src/modules/mc_hover_thrust_estimator/MulticopterHoverThrustEstimator.cpp
  +0.0%     +16  [ = ]       0    [section .symtab]
-1.0%    -728  [ = ]       0    [Unmapped]
+0.0% +23.0Ki  +0.0%    +728    TOTAL

Updated: 2025-01-08T12:47:45

[mahimayoga]

Tests in SITL:

1. Switch from position mode to stabilized while hovering.
   

2. For a heavier vehicle, check if thrust setpoint tracks allocated thrust (stabilized mode).