diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-23 08:33:42 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-23 08:33:42 +0200 |
commit | 29b926db1bbbb86b4c384d15788a6f9bf8962cb7 (patch) | |
tree | 8bbc8f146ff53172aae03877c1d08674237f54db /src/modules/mavlink/orb_listener.c | |
parent | 5f1004117f8086c4bba5b4031f3aebd73411682c (diff) | |
parent | 330908225e5fcb1731df20e740dbfe403a7b30b9 (diff) | |
download | px4-firmware-29b926db1bbbb86b4c384d15788a6f9bf8962cb7.tar.gz px4-firmware-29b926db1bbbb86b4c384d15788a6f9bf8962cb7.tar.bz2 px4-firmware-29b926db1bbbb86b4c384d15788a6f9bf8962cb7.zip |
Merged seatbelt_multirotor_new
Diffstat (limited to 'src/modules/mavlink/orb_listener.c')
-rw-r--r-- | src/modules/mavlink/orb_listener.c | 8 |
1 files changed, 4 insertions, 4 deletions
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index c711b1803..dcdc03281 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -666,12 +666,12 @@ l_airspeed(const struct listener *l) orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed); float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy); + uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f; float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1); - float alt = global_pos.alt; - float climb = global_pos.vz; + float alt = global_pos.relative_alt; + float climb = -global_pos.vz; - mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, - ((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb); + mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb); } static void * |