From c54529aaf850390f7cc3d1583541e0055fc4c7ad Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Sun, 18 Feb 2024 13:12:35 +0000 Subject: [PATCH] SITL: SIM_Motor: include momentum drag in derived torque --- libraries/SITL/SIM_Motor.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index 9c82d1ac0b..00740aba02 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -106,9 +106,6 @@ void Motor::calculate_forces(const struct sitl_input &input, rotor_torque = rotation * rotor_torque; } - // calculate total torque in newton-meters - torque = (position % thrust) + rotor_torque; - if (use_drag) { // calculate momentum drag per motor const float momentum_drag_factor = momentum_drag_coefficient * sqrtf(air_density * true_prop_area); @@ -124,6 +121,9 @@ void Motor::calculate_forces(const struct sitl_input &input, thrust -= momentum_drag; } + // calculate total torque in newton-meters + torque = (position % thrust) + rotor_torque; + // calculate current float power = power_factor * fabsf(motor_thrust); current = power / MAX(voltage, 0.1);