diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-12-13 08:10:18 -0800 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-12-13 08:10:18 -0800 |
commit | 39dcda3996b7c988c66bb4678f35bd45e0cc5d49 (patch) | |
tree | 38cabb67cb31cd814255710925bc72c8d63058a7 /src/modules/mavlink | |
parent | 0b58c01dcc79ec82d701820dcfce3af15c5d67ce (diff) | |
parent | b3f1adc54bac36b8da16651a545835bb1877f883 (diff) | |
download | px4-firmware-39dcda3996b7c988c66bb4678f35bd45e0cc5d49.tar.gz px4-firmware-39dcda3996b7c988c66bb4678f35bd45e0cc5d49.tar.bz2 px4-firmware-39dcda3996b7c988c66bb4678f35bd45e0cc5d49.zip |
Merge pull request #541 from limhyon/master
SO(3) estimator has been debugged and cleaned.
Diffstat (limited to 'src/modules/mavlink')
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 15 |
1 files changed, 14 insertions, 1 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 241ee558a..c37c35a63 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -240,7 +240,7 @@ l_vehicle_attitude(const struct listener *l) att.rollspeed, att.pitchspeed, att.yawspeed); - + /* limit VFR message rate to 10Hz */ hrt_abstime t = hrt_absolute_time(); if (t >= last_sent_vfr + 100000) { @@ -250,6 +250,19 @@ l_vehicle_attitude(const struct listener *l) float throttle = armed.armed ? actuators_0.control[3] * 100.0f : 0.0f; mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz); } + + /* send quaternion values if it exists */ + if(att.q_valid) { + mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_0, + last_sensor_timestamp / 1000, + att.q[0], + att.q[1], + att.q[2], + att.q[3], + att.rollspeed, + att.pitchspeed, + att.yawspeed); + } } attitude_counter++; |