Skip to content

Remove MPC_USE_HTE and always use hover thrust for altitude control #24751

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 1 commit into
base: main
Choose a base branch
from

Conversation

MaEtUgR
Copy link
Member

@MaEtUgR MaEtUgR commented Apr 22, 2025

Solved Problem

When we discussed #24710 in the dev call and agreed once again we need to reduce the configuration space, I realized I would never change this parameter to 0 or advise someone to do so. It was introduced beginning 2020 to experimentally introduce the hover thrust estimator but was soon after enabled by default and I have not seen anyone turn it off.

References:
Hover thrust estimator introduction: #13981
Addition of the parameter (and many improvements): #14182
Enabling use of the estimate the default: #14419

Solution

Note the separate mc_hover_thrust_estimator module can still be not run but I have not seen a case where the hover thrust estimate is not useful for altitude control hence I don't expect anyone to have the parameter disabled or planning on not running the module.

Please review the cases where the hover thrust is not available or gets invalid. My intention is to keep on using the last valid estimate like for the stick rescaling in Stabilized mode. This is in my eyes better than jumping back to the hover thrust parameter. I left the safety logic to make landing detection checks more strict with invalid hover thrust.

Changelog Entry

Remove `MPC_USE_HTE` and always use hover thrust for altitude control

Test coverage

I have not tested this pr. But can do so if we decide to go forward with it.

Note the separate module can still be not run but I have not seen a case where the hover thrust estimate is not useful for altitude control hence I don't expect anyone to have the parameter disabled or planning on not running the module.

This parameter was used to experimentally introduce the hover thrust estimator. First it was just logging its status and you could opt in to use it:
f9794e9
but soon after it became the default and you had to opt out of using it:
a8063ac
AFAIK we haven't seen problems requiring to disable it in the last 5 years and hence I suggest to remove the parameter to reduce the configuration space.
Copy link

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -56 byte (-0 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.1%     +16  +0.1%     +16    .ramfunc
  +2.0%     +12  +2.0%     +12    ../../src/lib/parameters/parameters.cpp
  +0.6%      +4  +0.6%      +4    [section .ramfunc]
+0.0%     +74  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.6%     +18  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +36  [ = ]       0    .debug_frame
-0.0% -1.25Ki  [ = ]       0    .debug_info
  -0.0%      -8  [ = ]       0    ../../src/drivers/batt_smbus/batt_smbus.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/dshot/DShot.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/heater/heater.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled/rgbled.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled_is31fl3195/rgbled_is31fl3195.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226_main.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228_main.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238_main.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/pwm_out/PWMOut.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/rc/crsf_rc/CrsfRc.cpp
 -100.0% -1.10Ki  [ = ]       0    [172 Others]
+0.0%     +88  [ = ]       0    .debug_line
  +0.3%     +66  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +2.4%    +135  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.3%     -92  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.4%      +4  [ = ]       0    task/task_cancelpt.c
-0.0%     -92  [ = ]       0    .debug_loc
  -0.0%     -15  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  +0.0%     +28  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  -0.2%     -52  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -0.0%     -16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +4.0%    +282  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.5%    -266  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  -0.1%     -16  [ = ]       0    ../../src/modules/navigator/navigator_main.cpp
  +0.0%      +9  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  -0.0%     -26  [ = ]       0    [section .debug_loc]
  -0.0%     -20  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
+0.0%     +31  [ = ]       0    .debug_ranges
  +0.4%     +56  [ = ]       0    ../../src/lib/parameters/parameters.cpp
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +8.8%    +112  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.9%    -128  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  -1.5%      -1  [ = ]       0    task/task_cancelpt.c
-0.0%   -1019  [ = ]       0    .debug_str
  -0.0%     -12  [ = ]       0    ../../src/drivers/batt_smbus/batt_smbus.cpp
  -3.7%     -75  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.1%     -14  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  -3.0%    -918  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
-0.9%      -2  [ = ]       0    .shstrtab
-0.0%     -86  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  -2.8%     -86  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.1%     +32  [ = ]       0    [section .strtab]
-0.0%     -32  [ = ]       0    .symtab
  -5.4%     -48  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.3%     -32  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.1%     +32  [ = ]       0    [section .symtab]
+0.1%     +56  [ = ]       0    [Unmapped]
-0.0%     -72  -0.0%     -72    .text
  +0.3%     +64  +0.3%     +64    ../../src/lib/parameters/parameters.cpp
  +2.2%     +48  +2.2%     +48    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.0%     -40  -0.0%     -40    [section .text]
  -0.0%     -56  -0.0%     -56    ROMFS/nsh_romfsimg.c
  -0.7%     -88  -0.7%     -88    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
-0.0% -2.24Ki  -0.0%     -56    TOTAL

px4_fmu-v6x [Total VM Diff: -136 byte (-0.01 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +74  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
  +0.6%     +18  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
-0.0%      -8  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
+0.0%     +24  [ = ]       0    .debug_frame
-0.0% -1.16Ki  [ = ]       0    .debug_info
  -0.0%      -8  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/distance_sensor/lightware_laser_i2c/lightware_laser_i2c.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/dshot/DShot.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/heater/heater.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled/rgbled.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled_is31fl3195/rgbled_is31fl3195.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled_lp5562/rgbled_lp5562.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/lights/rgbled_ncp5623c/rgbled_ncp5623c.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/osd/msp_osd/msp_osd.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina226/ina226_main.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina228/ina228_main.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/ina238/ina238_main.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/power_monitor/pm_selector_auterion/PowerMonitorSelectorAuterion.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/pwm_out/PWMOut.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/px4io/px4io.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/rc_input/RCInput.cpp
  -0.0%      -8  [ = ]       0    ../../src/drivers/uavcan/actuators/esc.cpp
 -100.0% -1.01Ki  [ = ]       0    [161 Others]
+0.0%     +20  [ = ]       0    .debug_line
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  +0.0%      +1  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  +2.4%    +135  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.3%     -92  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.1%      +1  [ = ]       0    task/task_cancelpt.c
+0.0%    +335  [ = ]       0    .debug_loc
  +0.0%      +3  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  -0.0%      -1  [ = ]       0    ../../src/modules/commander/HealthAndArmingChecks/checks/externalChecks.cpp
  +0.6%    +180  [ = ]       0    ../../src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp
  +4.0%    +282  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.5%    -266  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  -0.0%      -2  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +0.1%     +29  [ = ]       0    ../../src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  +0.1%     +75  [ = ]       0    ../../src/modules/sensors/vehicle_imu/VehicleIMU.cpp
  +0.0%      +2  [ = ]       0    [section .debug_loc]
  +0.0%     +33  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
-0.0%     -24  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +8.8%    +112  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.9%    -128  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
-0.0%   -1019  [ = ]       0    .debug_str
  -0.0%     -12  [ = ]       0    ../../src/drivers/cdcacm_autostart/cdcacm_autostart.cpp
  -3.7%     -75  [ = ]       0    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  -0.1%     -14  [ = ]       0    ../../src/modules/mc_att_control/mc_att_control_main.cpp
  -3.0%    -918  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
-0.9%      -2  [ = ]       0    .shstrtab
-0.0%     -86  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  -2.8%     -86  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.0%     -32  [ = ]       0    .symtab
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.3%     -32  [ = ]       0    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+0.2%    +136  [ = ]       0    [Unmapped]
-0.0%    -136  -0.0%    -136    .text
  +2.2%     +48  +2.2%     +48    ../../src/modules/land_detector/MulticopterLandDetector.cpp
  +0.2%      +3  +0.2%      +3    ../../src/systemcmds/ver/ver.cpp
  -0.0%      -8  -0.0%      -8    ../../src/lib/parameters/parameters.cpp
  -0.0%     -13  -0.0%     -13    [section .text]
  -0.0%     -78  -0.0%     -78    ROMFS/nsh_romfsimg.c
  -0.7%     -88  -0.7%     -88    ../../src/modules/mc_pos_control/MulticopterPositionControl.cpp
-0.0% -1.86Ki  -0.0%    -136    TOTAL

Updated: 2025-04-22T17:08:31

@DronecodeBot
Copy link

This pull request has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-sync-q-a-april-23-2025/45198/3

@dakejahl
Copy link
Contributor

dakejahl commented Apr 23, 2025

I flight tested this yesterday and all seemed well.
https://review.px4.io/plot_app?log=ddbf2f12-14a9-4400-815c-2520cd347313

Copy link

No flaws found

@MaEtUgR
Copy link
Member Author

MaEtUgR commented Apr 25, 2025

@dakejahl Thanks for testing! I had a quick glance and it looks good from my perspective with this pull request in mind:
image

Copy link
Member

@bresch bresch left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just a small comment. All good otherwise!

@@ -98,12 +97,12 @@ void MulticopterLandDetector::_update_topics()
_flag_control_climb_rate_enabled = vehicle_control_mode.flag_control_climb_rate_enabled;
}

if (_params.useHoverThrustEstimate) {
if (_hover_thrust_estimate_sub.updated()) {
hover_thrust_estimate_s hte;

if (_hover_thrust_estimate_sub.update(&hte)) {
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
if (_hover_thrust_estimate_sub.update(&hte)) {
if (_hover_thrust_estimate_sub.copy(&hte)) {

Since you already have the updated above.

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants