fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-24 14:00:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

vfr_hud: fix throttle display for FW and show magnitude for 3D thrust (#22154)

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2023-09-28 17:15:26 +02:00
committed by GitHub
parent 8ea04b0f8f
commit 0df5134156
1 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mavlink/streams/VFR_HUD.hpp
+9 -3
View File
@@ -94,12 +94,18 @@ private:
_vehicle_thrust_setpoint_0_sub.copy(&vehicle_thrust_setpoint_0);
_vehicle_thrust_setpoint_1_sub.copy(&vehicle_thrust_setpoint_1);
const float thrust_magnitude_0 = sqrtf(vehicle_thrust_setpoint_0.xyz[0] * vehicle_thrust_setpoint_0.xyz[0] +
vehicle_thrust_setpoint_0.xyz[1] * vehicle_thrust_setpoint_0.xyz[1] +
vehicle_thrust_setpoint_0.xyz[2] * vehicle_thrust_setpoint_0.xyz[2]);
const float thrust_magnitude_1 = sqrtf(vehicle_thrust_setpoint_1.xyz[0] * vehicle_thrust_setpoint_1.xyz[0] +
vehicle_thrust_setpoint_1.xyz[1] * vehicle_thrust_setpoint_1.xyz[1] +
vehicle_thrust_setpoint_1.xyz[2] * vehicle_thrust_setpoint_1.xyz[2]);
// VFR_HUD throttle should only be used for operator feedback.
// VTOLs switch between vehicle_thrust_setpoint_0 and vehicle_thrust_setpoint_1. During transition there isn't a
// a single throttle value, but this should still be a useful heuristic for operator awareness.
msg.throttle = 100 * math::max(
-vehicle_thrust_setpoint_0.xyz[2],
vehicle_thrust_setpoint_1.xyz[0]);
msg.throttle = 100.f * math::max(thrust_magnitude_0, thrust_magnitude_1);
} else {
msg.throttle = 0.0f;