SITL: Motor: acount for airflow due to vehicle rotation

libraries/SITL/SIM_Frame.cpp

@@ -590,13 +590,14 @@ void Frame::calculate_forces(const Aircraft &aircraft,
     Vector3f torque;
 
     const float air_density = get_air_density(aircraft.get_location().alt*0.01);
+    const Vector3f gyro = aircraft.get_gyro();
 
     Vector3f vel_air_bf = aircraft.get_dcm().transposed() * aircraft.get_velocity_air_ef();
 
     float current = 0;
     for (uint8_t i=0; i<num_motors; i++) {
         Vector3f mtorque, mthrust;
-        motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, air_density, battery->get_voltage());
+        motors[i].calculate_forces(input, motor_offset, mtorque, mthrust, vel_air_bf, gyro, air_density, battery->get_voltage());
         current += motors[i].get_current();
         torque += mtorque;
         thrust += mthrust;
@@ -615,7 +616,6 @@ void Frame::calculate_forces(const Aircraft &aircraft,
 
     if (terminal_rotation_rate > 0) {
         // rotational air resistance
-        const Vector3f &gyro = aircraft.get_gyro();
         rot_accel.x -= gyro.x * radians(400.0) / terminal_rotation_rate;
         rot_accel.y -= gyro.y * radians(400.0) / terminal_rotation_rate;
         rot_accel.z -= gyro.z * radians(400.0) / terminal_rotation_rate;

libraries/SITL/SIM_Motor.cpp

@@ -27,6 +27,7 @@ void Motor::calculate_forces(const struct sitl_input &input,
                              Vector3f &torque,
                              Vector3f &thrust,
                              const Vector3f &velocity_air_bf,
+                             const Vector3f &gyro,
                              float air_density,
                              float voltage)
 {
@@ -58,13 +59,19 @@ void Motor::calculate_forces(const struct sitl_input &input,
     // the yaw torque of the motor
     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.projected(thrust_vector).z);
+    // velocity of motor through air
+    Vector3f motor_vel = velocity_air_bf;
+
+    // add velocity of motor about center due to vehicle rotation
+    motor_vel += -(position % gyro);
+
+    // calculate velocity into prop, clipping at zero
+    float velocity_in = MAX(0, -motor_vel.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 in bodyframe NED
     thrust = thrust_vector * motor_thrust;
 
     // work out roll and pitch of motor relative to it pointing straight up

libraries/SITL/SIM_Motor.h

@@ -88,9 +88,10 @@ public:
 
     void calculate_forces(const struct sitl_input &input,
                           uint8_t motor_offset,
-                          Vector3f &rot_accel, // rad/sec
-                          Vector3f &body_thrust, // Z is down
+                          Vector3f &torque, // Newton meters
+                          Vector3f &thrust, // Z is down, Newtons
                           const Vector3f &velocity_air_bf,
+                          const Vector3f &gyro, // rad/sec
                           float air_density,
                           float voltage);