aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

SITL: fixed SITL for tilted motors

Browse Source 
vector math was done in the wrong order
This commit is contained in:
Andrew Tridgell 2024-02-17 19:26:49 +11:00
parent 9986fb9726
commit 388b04d671
1 changed files with 4 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/SITL/SIM_Motor.cpp
View File

@ -98,17 +98,17 @@ void Motor::calculate_forces(const struct sitl_input &input,
} }
last_change_usec = now; last_change_usec = now;
// calculate torque in newton-meters
torque = (position % thrust) + rotor_torque;
// possibly rotate the thrust vector and the rotor torque // possibly rotate the thrust vector and the rotor torque
if (!is_zero(roll) || !is_zero(pitch)) { if (!is_zero(roll) || !is_zero(pitch)) {
Matrix3f rotation; Matrix3f rotation;
rotation.from_euler(radians(roll), radians(pitch), 0); rotation.from_euler(radians(roll), radians(pitch), 0);
thrust = rotation * thrust; thrust = rotation * thrust;
torque = rotation * torque; rotor_torque = rotation * rotor_torque;
} }
// calculate total torque in newton-meters
torque = (position % thrust) + rotor_torque;
if (use_drag) { if (use_drag) {
// calculate momentum drag per motor // calculate momentum drag per motor
const float momentum_drag_factor = momentum_drag_coefficient * sqrtf(air_density * true_prop_area); const float momentum_drag_factor = momentum_drag_coefficient * sqrtf(air_density * true_prop_area);