SITL: added basic wind support in multicopter, plane and quadplane

this adds non-turbulent wind support for the built-in simulators. I
added it primarily for quadplane testing, but it should also be useful
for multicopter navigation testing.

libraries/SITL/SIM_Aircraft.cpp

@@ -287,7 +287,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) const
     fdm.rollDeg  = degrees(r);
     fdm.pitchDeg = degrees(p);
     fdm.yawDeg   = degrees(y);
-    fdm.airspeed = airspeed;
+    fdm.airspeed = airspeed_pitot;
     fdm.battery_voltage = battery_voltage;
     fdm.battery_current = battery_current;
     fdm.rpm1 = rpm1;
@@ -360,9 +360,18 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
     Vector3f old_position = position;
     position += velocity_ef * delta_time;
 
-    // assume zero wind for now
-    airspeed = velocity_ef.length();
+    // velocity relative to air mass, in earth frame
+    velocity_air_ef = velocity_ef - wind_ef;
+    
+    // velocity relative to airmass in body frame
+    velocity_air_bf = dcm.transposed() * velocity_air_ef;
+    
+    // airspeed 
+    airspeed = velocity_air_ef.length();
 
+    // airspeed as seen by a fwd pitot tube (limited to 120m/s)
+    airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1, 0, 0), 0, 120);
+    
     // constrain height to the ground
     if (on_ground(position)) {
         if (!on_ground(old_position) && AP_HAL::millis() - last_ground_contact_ms > 1000) {
@@ -372,5 +381,14 @@ void Aircraft::update_dynamics(const Vector3f &rot_accel)
         position.z = -(ground_level + frame_height - home.alt*0.01f);
     }
 }
+
+/*
+  update wind vector
+*/
+void Aircraft::update_wind(const struct sitl_input &input)
+{
+    // wind vector in earth frame
+    wind_ef = Vector3f(cosf(radians(input.wind.direction)), sinf(radians(input.wind.direction)), 0) * input.wind.speed;
+}
     
 } // namespace SITL

libraries/SITL/SIM_Aircraft.h

@@ -89,6 +89,10 @@ public:
         return velocity_ef;
     }
 
+    const Vector3f &get_velocity_air_ef(void) const {
+        return velocity_air_ef;
+    }
+    
     const Matrix3f &get_dcm(void) const {
         return dcm;
     }
@@ -103,10 +107,14 @@ protected:
     Matrix3f dcm;  // rotation matrix, APM conventions, from body to earth
     Vector3f gyro; // rad/s
     Vector3f velocity_ef; // m/s, earth frame
+    Vector3f wind_ef; // m/s, earth frame
+    Vector3f velocity_air_ef; // velocity relative to airmass, earth frame
+    Vector3f velocity_air_bf; // velocity relative to airmass, body frame
     Vector3f position; // meters, NED from origin
     float mass; // kg
     Vector3f accel_body; // m/s/s NED, body frame
     float airspeed; // m/s, apparent airspeed
+    float airspeed_pitot; // m/s, apparent airspeed, as seen by fwd pitot tube
     float battery_voltage = -1;
     float battery_current = 0;
     float rpm1 = 0;
@@ -157,6 +165,9 @@ protected:
     // update attitude and relative position
     void update_dynamics(const Vector3f &rot_accel);
 
+    // update wind vector
+    void update_wind(const struct sitl_input &input);
+    
 private:
     uint64_t last_time_us = 0;
     uint32_t frame_counter = 0;

libraries/SITL/SIM_Balloon.cpp

@@ -34,6 +34,9 @@ Balloon::Balloon(const char *home_str, const char *frame_str) :
  */
 void Balloon::update(const struct sitl_input &input)
 {
+    // get wind vector setup
+    update_wind(input);
+
     if (!released && input.servos[6] > 1800) {
         ::printf("Balloon released\n");
         released = true;
@@ -48,7 +51,7 @@ void Balloon::update(const struct sitl_input &input)
     Vector3f rot_accel = -gyro * radians(400) / terminal_rotation_rate;
 
     // air resistance
-    Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
+    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
 
     float lift_accel = 0;
     if (!burst && released) {

libraries/SITL/SIM_CRRCSim.cpp

@@ -130,6 +130,7 @@ void CRRCSim::recv_fdm(const struct sitl_input &input)
     position.z = -pkt.altitude;
 
     airspeed = pkt.airspeed;
+    airspeed_pitot = pkt.airspeed;
 
     dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
 

libraries/SITL/SIM_FlightAxis.cpp

@@ -287,6 +287,7 @@ void FlightAxis::update(const struct sitl_input &input)
     accel_body.z = constrain_float(accel_body.z, -16, 16);
 
     airspeed = state.m_airspeed_MPS;
+    airspeed_pitot = state.m_airspeed_MPS;
 
     battery_voltage = state.m_batteryVoltage_VOLTS;
     battery_current = state.m_batteryCurrentDraw_AMPS;

libraries/SITL/SIM_Helicopter.cpp

@@ -54,6 +54,9 @@ Helicopter::Helicopter(const char *home_str, const char *frame_str) :
  */
 void Helicopter::update(const struct sitl_input &input)
 {
+    // get wind vector setup
+    update_wind(input);
+
     float rsc = constrain_float((input.servos[7]-1000) / 1000.0f, 0, 1);
     // ignition only for gas helis
     bool ignition_enabled = gas_heli?(input.servos[5] > 1500):true;
@@ -145,7 +148,7 @@ void Helicopter::update(const struct sitl_input &input)
     rot_accel.z += torque_effect_accel;
 
     // air resistance
-    Vector3f air_resistance = -velocity_ef * (GRAVITY_MSS/terminal_velocity);
+    Vector3f air_resistance = -velocity_air_ef * (GRAVITY_MSS/terminal_velocity);
 
     // scale thrust to newtons
     thrust *= thrust_scale;

libraries/SITL/SIM_JSBSim.cpp

@@ -420,6 +420,7 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
     location.alt = fdm.agl*100 + home.alt;
     dcm.from_euler(fdm.phi, fdm.theta, fdm.psi);
     airspeed = fdm.vcas * FEET_TO_METERS;
+    airspeed_pitot = airspeed;
 
     rpm1 = fdm.rpm[0];
     rpm2 = fdm.rpm[1];

libraries/SITL/SIM_Multicopter.cpp

@@ -168,7 +168,7 @@ void Frame::calculate_forces(const Aircraft &aircraft,
 
     if (terminal_velocity > 0) {
         // air resistance
-        Vector3f air_resistance = -aircraft.get_velocity_ef() * (GRAVITY_MSS/terminal_velocity);
+        Vector3f air_resistance = -aircraft.get_velocity_air_ef() * (GRAVITY_MSS/terminal_velocity);
         body_accel += aircraft.get_dcm().transposed() * air_resistance;
     }
 
@@ -195,6 +195,9 @@ void MultiCopter::calculate_forces(const struct sitl_input &input, Vector3f &rot
  */
 void MultiCopter::update(const struct sitl_input &input)
 {
+    // get wind vector setup
+    update_wind(input);
+
     // how much time has passed?
     Vector3f rot_accel;
 

libraries/SITL/SIM_Plane.cpp

@@ -205,15 +205,12 @@ void Plane::calculate_forces(const struct sitl_input &input, Vector3f &rot_accel
     float thrust     = throttle;
 
     // calculate angle of attack
-    angle_of_attack = atan2f(velocity_bf.z, velocity_bf.x);
-    beta = atan2f(velocity_bf.y,velocity_bf.x);
+    angle_of_attack = atan2f(velocity_air_bf.z, velocity_air_bf.x);
+    beta = atan2f(velocity_air_bf.y,velocity_air_bf.x);
     
     Vector3f force = getForce(aileron, elevator, rudder);
     rot_accel = getTorque(aileron, elevator, rudder, force);
 
-    // velocity in body frame
-    velocity_bf = dcm.transposed() * velocity_ef;
-    
     // scale thrust to newtons
     thrust *= thrust_scale;
 
@@ -231,6 +228,8 @@ void Plane::update(const struct sitl_input &input)
 {
     Vector3f rot_accel;
 
+    update_wind(input);
+    
     calculate_forces(input, rot_accel, accel_body);
     
     update_dynamics(rot_accel);

libraries/SITL/SIM_Plane.h

@@ -43,7 +43,6 @@ protected:
     const float air_density = 1.225; // kg/m^3 at sea level, ISA conditions
     float angle_of_attack;
     float beta;
-    Vector3f velocity_bf;
 
     struct {
         // from last_letter skywalker_2013/aerodynamics.yaml

libraries/SITL/SIM_QuadPlane.cpp

@@ -65,6 +65,9 @@ QuadPlane::QuadPlane(const char *home_str, const char *frame_str) :
  */
 void QuadPlane::update(const struct sitl_input &input)
 {
+    // get wind vector setup
+    update_wind(input);
+
     // first plane forces
     Vector3f rot_accel;
     calculate_forces(input, rot_accel, accel_body);

libraries/SITL/SIM_last_letter.cpp

@@ -123,6 +123,7 @@ void last_letter::recv_fdm(const struct sitl_input &input)
     dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw);
 
     airspeed = pkt.airspeed;
+    airspeed_pitot = pkt.airspeed;
 
     // auto-adjust to last_letter frame rate
     uint64_t deltat_us = pkt.timestamp_us - last_timestamp_us;