From 0d20167294ab990e0dd04967ef3d5a86b46415c1 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 25 May 2015 08:42:21 +1000 Subject: [PATCH] autotest: removed earth-frame rates usage work only with rates in body frame --- Tools/autotest/pysim/sim_tracker.py | 3 +-- Tools/autotest/pysim/sim_wrapper.py | 7 +++---- 2 files changed, 4 insertions(+), 6 deletions(-) diff --git a/Tools/autotest/pysim/sim_tracker.py b/Tools/autotest/pysim/sim_tracker.py index 0c952eb08b..c40631307c 100755 --- a/Tools/autotest/pysim/sim_tracker.py +++ b/Tools/autotest/pysim/sim_tracker.py @@ -12,14 +12,13 @@ def sim_send(a): '''send flight information to mavproxy''' from math import degrees - earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro) (roll, pitch, yaw) = a.dcm.to_euler() buf = struct.pack('<17dI', a.latitude, a.longitude, a.altitude, degrees(yaw), a.velocity.x, a.velocity.y, a.velocity.z, a.accelerometer.x, a.accelerometer.y, a.accelerometer.z, - degrees(earth_rates.x), degrees(earth_rates.y), degrees(earth_rates.z), + degrees(a.gyro.x), degrees(a.gyro.y), degrees(a.gyro.z), degrees(roll), degrees(pitch), degrees(yaw), math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), 0x4c56414f) diff --git a/Tools/autotest/pysim/sim_wrapper.py b/Tools/autotest/pysim/sim_wrapper.py index dc846f0df6..582a618a3b 100755 --- a/Tools/autotest/pysim/sim_wrapper.py +++ b/Tools/autotest/pysim/sim_wrapper.py @@ -11,7 +11,6 @@ def sim_send(m, a): global fdm from math import degrees - earth_rates = util.BodyRatesToEarthRates(a.dcm, a.gyro) (roll, pitch, yaw) = a.dcm.to_euler() fdm.set('latitude', a.latitude, units='degrees') @@ -20,9 +19,9 @@ def sim_send(m, a): fdm.set('phi', roll, units='radians') fdm.set('theta', pitch, units='radians') fdm.set('psi', yaw, units='radians') - fdm.set('phidot', earth_rates.x, units='rps') - fdm.set('thetadot', earth_rates.y, units='rps') - fdm.set('psidot', earth_rates.z, units='rps') + fdm.set('phidot', a.gyro.x, units='rps') + fdm.set('thetadot', a.gyro.y, units='rps') + fdm.set('psidot', a.gyro.z, units='rps') fdm.set('vcas', math.sqrt(a.velocity.x*a.velocity.x + a.velocity.y*a.velocity.y), units='mps') fdm.set('v_north', a.velocity.x, units='mps') fdm.set('v_east', a.velocity.y, units='mps')