From da5d5c99367e5f61e0d99b8f9650652b08c90120 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 23 May 2015 11:24:10 +1000 Subject: [PATCH] SITL: fixed airspeed pass-thru from simulators --- libraries/SITL/SIM_Aircraft.cpp | 2 +- libraries/SITL/SIM_Aircraft.h | 1 + libraries/SITL/SIM_CRRCSim.cpp | 3 +++ libraries/SITL/SIM_Helicopter.cpp | 3 +++ libraries/SITL/SIM_JSBSim.cpp | 1 + libraries/SITL/SIM_Multicopter.cpp | 3 +++ libraries/SITL/SIM_last_letter.cpp | 3 +++ 7 files changed, 15 insertions(+), 1 deletion(-) diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp index c545da43dc..f6d681ad02 100644 --- a/libraries/SITL/SIM_Aircraft.cpp +++ b/libraries/SITL/SIM_Aircraft.cpp @@ -233,7 +233,7 @@ void Aircraft::fill_fdm(struct sitl_fdm &fdm) const fdm.rollDeg = degrees(r); fdm.pitchDeg = degrees(p); fdm.yawDeg = degrees(y); - fdm.airspeed = velocity_ef.length(); + fdm.airspeed = airspeed; fdm.magic = 0x4c56414f; } diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h index 508688cd20..aaa8878a2f 100644 --- a/libraries/SITL/SIM_Aircraft.h +++ b/libraries/SITL/SIM_Aircraft.h @@ -77,6 +77,7 @@ protected: Vector3f position; // meters, NED from origin float mass; // kg Vector3f accel_body; // m/s/s NED, body frame + float airspeed; // m/s, apparent airspeed uint64_t time_now_us; diff --git a/libraries/SITL/SIM_CRRCSim.cpp b/libraries/SITL/SIM_CRRCSim.cpp index 988d76adfa..bc20c54538 100644 --- a/libraries/SITL/SIM_CRRCSim.cpp +++ b/libraries/SITL/SIM_CRRCSim.cpp @@ -129,6 +129,9 @@ void CRRCSim::recv_fdm(const struct sitl_input &input) position.y = posdelta.y; position.z = -pkt.altitude; + // assume zero wind for now + airspeed = velocity_ef.length(); + dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw); // auto-adjust to crrcsim frame rate diff --git a/libraries/SITL/SIM_Helicopter.cpp b/libraries/SITL/SIM_Helicopter.cpp index 0a311a37e3..e135dbfb18 100644 --- a/libraries/SITL/SIM_Helicopter.cpp +++ b/libraries/SITL/SIM_Helicopter.cpp @@ -131,6 +131,9 @@ void Helicopter::update(const struct sitl_input &input) Vector3f old_position = position; position += velocity_ef * delta_time; + // assume zero wind for now + airspeed = velocity_ef.length(); + // constrain height to the ground if (on_ground(position)) { if (!on_ground(old_position)) { diff --git a/libraries/SITL/SIM_JSBSim.cpp b/libraries/SITL/SIM_JSBSim.cpp index 5022f160ff..9a357a7ac0 100644 --- a/libraries/SITL/SIM_JSBSim.cpp +++ b/libraries/SITL/SIM_JSBSim.cpp @@ -405,6 +405,7 @@ void JSBSim::recv_fdm(const struct sitl_input &input) location.lng = degrees(fdm.longitude) * 1.0e7; location.alt = fdm.agl*100 + home.alt; dcm.from_euler(fdm.phi, fdm.theta, fdm.psi); + airspeed = fdm.vcas * FEET_TO_METERS; // assume 1kHz for now time_now_us += 1000; diff --git a/libraries/SITL/SIM_Multicopter.cpp b/libraries/SITL/SIM_Multicopter.cpp index 0ef3e0784d..59a10f42b4 100644 --- a/libraries/SITL/SIM_Multicopter.cpp +++ b/libraries/SITL/SIM_Multicopter.cpp @@ -204,6 +204,9 @@ void MultiCopter::update(const struct sitl_input &input) Vector3f old_position = position; position += velocity_ef * delta_time; + // assume zero wind for now + airspeed = velocity_ef.length(); + // constrain height to the ground if (on_ground(position)) { if (!on_ground(old_position)) { diff --git a/libraries/SITL/SIM_last_letter.cpp b/libraries/SITL/SIM_last_letter.cpp index abcaf56a84..c4fd65e250 100644 --- a/libraries/SITL/SIM_last_letter.cpp +++ b/libraries/SITL/SIM_last_letter.cpp @@ -107,6 +107,9 @@ void last_letter::recv_fdm(const struct sitl_input &input) location.alt = pkt.altitude*1.0e2; dcm.from_euler(pkt.roll, pkt.pitch, pkt.yaw); + // assume zero wind for now + airspeed = velocity_ef.length(); + // auto-adjust to last_letter frame rate uint64_t deltat_us = pkt.timestamp_us - last_timestamp_us; time_now_us += deltat_us;