SITL: fixed mag field in JSBSim and last_letter

This commit is contained in:
Andrew Tridgell 2016-06-20 11:07:34 +10:00
parent ba2382c409
commit 5b7b799046
2 changed files with 6 additions and 0 deletions

View File

@ -423,6 +423,9 @@ void JSBSim::recv_fdm(const struct sitl_input &input)
airspeed = fdm.vcas * FEET_TO_METERS;
airspeed_pitot = airspeed;
// update magnetic field
update_mag_field_bf();
rpm1 = fdm.rpm[0];
rpm2 = fdm.rpm[1];

View File

@ -125,6 +125,9 @@ void last_letter::recv_fdm(const struct sitl_input &input)
airspeed = pkt.airspeed;
airspeed_pitot = pkt.airspeed;
// update magnetic field
update_mag_field_bf();
// auto-adjust to last_letter frame rate
uint64_t deltat_us = pkt.timestamp_us - last_timestamp_us;
time_now_us += deltat_us;