diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index 9b90bfbc5f..24ab70faa8 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/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 diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 9cd5f030f6..60b0a5fe39 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/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; diff --git a/libraries/SITL/SIM_Balloon.cpp b/libraries/SITL/SIM_Balloon.cpp index 6e9fb95394..25ad6a4378 100644 --- a/libraries/SITL/SIM_Balloon.cpp +++ b/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) { diff --git a/libraries/SITL/SIM_CRRCSim.cpp b/libraries/SITL/SIM_CRRCSim.cpp index c42b372cde..318df84056 100644 --- a/libraries/SITL/SIM_CRRCSim.cpp +++ b/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); diff --git a/libraries/SITL/SIM_FlightAxis.cpp b/libraries/SITL/SIM_FlightAxis.cpp index 171b8caa5d..80e880bf89 100644 --- a/libraries/SITL/SIM_FlightAxis.cpp +++ b/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; diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index e5a0cb3c41..6d88820f79 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/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; diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index d694df524c..ba2de33371 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/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]; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 0eadc4a6fe..2e83aea0b4 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/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; diff --git a/libraries/SITL/SIM_Plane.cpp b/libraries/SITL/SIM_Plane.cpp index 9638e348d5..f57647ac8c 100644 --- a/libraries/SITL/SIM_Plane.cpp +++ b/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); diff --git a/libraries/SITL/SIM_Plane.h b/libraries/SITL/SIM_Plane.h index 0df9133954..1cc03645c4 100644 --- a/libraries/SITL/SIM_Plane.h +++ b/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 diff --git a/libraries/SITL/SIM_QuadPlane.cpp b/libraries/SITL/SIM_QuadPlane.cpp index 19d89a8716..0261f695cb 100644 --- a/libraries/SITL/SIM_QuadPlane.cpp +++ b/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); diff --git a/libraries/SITL/SIM_last_letter.cpp b/libraries/SITL/SIM_last_letter.cpp index 308cff0194..965bb7ec3d 100644 --- a/libraries/SITL/SIM_last_letter.cpp +++ b/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;