SITL: fixed rotations of motors by large angles

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;
     }