From b4d24d8e033132852927128953d9f35f8f1978d8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 22 Apr 2016 09:55:22 +1000 Subject: [PATCH] SITL: fixed rotations of motors by large angles --- libraries/SITL/SIM_Motor.cpp | 19 ++++++++++++++----- 1 file changed, 14 insertions(+), 5 deletions(-) diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp index d4e092035d..0e0cffef36 100644 --- a/libraries/SITL/SIM_Motor.cpp +++ b/libraries/SITL/SIM_Motor.cpp @@ -35,18 +35,27 @@ void Motor::calculate_forces(const Aircraft::sitl_input &input, rot_accel.z = yaw_factor * motor_speed * radians(400.0); thrust(0, 0, motor_speed * thrust_scale); // newtons if (roll_servo >= 0) { - float roll = constrain_float(roll_min + (input.servos[roll_servo]-1000)*0.001*(roll_max-roll_min), roll_min, roll_max); + float roll; + if (roll_min < roll_max) { + roll = constrain_float(roll_min + (input.servos[roll_servo]-1000)*0.001*(roll_max-roll_min), roll_min, roll_max); + } else { + roll = constrain_float(roll_max + (2000-input.servos[roll_servo])*0.001*(roll_min-roll_max), roll_max, roll_min); + } Matrix3f rotation; - rotation.identity(); - rotation.rotate(Vector3f(radians(roll), 0, 0)); + rotation.from_euler(radians(roll), 0, 0); rot_accel = rotation * rot_accel; thrust = rotation * thrust; } if (pitch_servo >= 0) { - float pitch = constrain_float(pitch_min + (input.servos[pitch_servo]-1000)*0.001*(pitch_max-pitch_min), pitch_min, pitch_max); + float pitch; + if (pitch_min < pitch_max) { + pitch = constrain_float(pitch_min + (input.servos[pitch_servo]-1000)*0.001*(pitch_max-pitch_min), pitch_min, pitch_max); + } else { + pitch = constrain_float(pitch_max + (2000-input.servos[pitch_servo])*0.001*(pitch_min-pitch_max), pitch_max, pitch_min); + } Matrix3f rotation; rotation.identity(); - rotation.rotate(Vector3f(0, radians(pitch), 0)); + rotation.from_euler(0, radians(pitch), 0); rot_accel = rotation * rot_accel; thrust = rotation * thrust; }