diff --git a/libraries/SITL/SIM_Motor.cpp b/libraries/SITL/SIM_Motor.cpp
index da41cd0341..5050be8ab2 100644
--- a/libraries/SITL/SIM_Motor.cpp
+++ b/libraries/SITL/SIM_Motor.cpp
@@ -56,20 +56,16 @@ void Motor::calculate_forces(const struct sitl_input &input,
     last_command = command;
 
     // the yaw torque of the motor
-    Vector3f rotor_torque(0, 0, yaw_factor * command * yaw_scale * voltage_scale);
+    Vector3f rotor_torque = thrust_vector * yaw_factor * command * yaw_scale * voltage_scale * -1.0;
 
     // calculate velocity into prop, clipping at zero, assumes zero roll/pitch
-    float velocity_in = MAX(0, -velocity_air_bf.z);
+    float velocity_in = MAX(0, -velocity_air_bf.projected(thrust_vector).z);
 
     // get thrust for untilted motor
     float motor_thrust = calc_thrust(command, air_density, velocity_in, voltage_scale);
 
     // thrust in NED
-    thrust = {0, 0, -motor_thrust};
-
-    // define the arm position relative to center of mass
-    Vector3f arm(cosf(radians(angle)), sinf(radians(angle)), 0);
-    arm *= diagonal_size;
+    thrust = thrust_vector * motor_thrust;
 
     // work out roll and pitch of motor relative to it pointing straight up
     float roll = 0, pitch = 0;
@@ -96,7 +92,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
     last_change_usec = now;
 
     // calculate torque in newton-meters
-    torque = (arm % thrust) + rotor_torque;
+    torque = (position % thrust) + rotor_torque;
 
     // possibly rotate the thrust vector and the rotor torque
     if (!is_zero(roll) || !is_zero(pitch)) {
@@ -155,11 +151,14 @@ void Motor::setup_params(uint16_t _pwm_min, uint16_t _pwm_max, float _spin_min,
     mot_spin_max = _spin_max;
     mot_expo = _expo;
     slew_max = _slew_max;
-    diagonal_size = _diagonal_size;
     power_factor = _power_factor;
     voltage_max = _voltage_max;
     effective_prop_area = _effective_prop_area;
     max_outflow_velocity = _velocity_max;
+
+    position.x = cosf(radians(angle)) * _diagonal_size;
+    position.y =  sinf(radians(angle)) * _diagonal_size;
+    position.z = 0;
 }
 
 /*
diff --git a/libraries/SITL/SIM_Motor.h b/libraries/SITL/SIM_Motor.h
index 2cbdf8f5f9..bdf657db1c 100644
--- a/libraries/SITL/SIM_Motor.h
+++ b/libraries/SITL/SIM_Motor.h
@@ -49,7 +49,15 @@ public:
         angle(_angle), // angle in degrees from front
         yaw_factor(_yaw_factor), // positive is clockwise
         display_order(_display_order) // order for clockwise display
-    {}
+    {
+        position.x = cosf(radians(angle));
+        position.y =  sinf(radians(angle));
+        position.z = 0;
+
+        thrust_vector.x = 0;
+        thrust_vector.y = 0;
+        thrust_vector.z = -1;
+    }
 
     /*
       alternative constructor for tiltable motors
@@ -67,7 +75,15 @@ public:
         pitch_servo(_pitch_servo),
         pitch_min(_pitch_min),
         pitch_max(_pitch_max)
-    {}
+    {
+        position.x = cosf(radians(angle));
+        position.y =  sinf(radians(angle));
+        position.z = 0;
+
+        thrust_vector.x = 0;
+        thrust_vector.y = 0;
+        thrust_vector.z = -1;
+    }
 
     void calculate_forces(const struct sitl_input &input,
                           uint8_t motor_offset,
@@ -109,7 +125,6 @@ private:
     float mot_spin_max;
     float mot_expo;
     float slew_max;
-    float diagonal_size;
     float current;
     float power_factor;
     float voltage_max;
@@ -118,6 +133,9 @@ private:
 
     float last_command;
     uint64_t last_calc_us;
+
+    Vector3f position;
+    Vector3f thrust_vector;
 };
 
 }