diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-26 22:13:49 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-26 22:13:49 +0400 |
commit | 2967763b160bb12b8e13e72219fcb689da015ab5 (patch) | |
tree | ca42f8055142465bee7cabd5dd862632aaa394f6 /src/modules/mavlink/mavlink_messages.cpp | |
parent | 355715a05414726d13f41b4881fa57df2fe13ac0 (diff) | |
download | px4-firmware-2967763b160bb12b8e13e72219fcb689da015ab5.tar.gz px4-firmware-2967763b160bb12b8e13e72219fcb689da015ab5.tar.bz2 px4-firmware-2967763b160bb12b8e13e72219fcb689da015ab5.zip |
mavlink: heartbeat message bug fixed
Diffstat (limited to 'src/modules/mavlink/mavlink_messages.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_messages.cpp | 6 |
1 files changed, 3 insertions, 3 deletions
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index df73581f0..0880e8285 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -38,7 +38,7 @@ protected: status = (struct vehicle_status_s *)status_sub->get_data(); pos_sp_triplet_sub = mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet), sizeof(position_setpoint_triplet_s)); - pos_sp_triplet = (struct position_setpoint_triplet_s *)status_sub->get_data(); + pos_sp_triplet = (struct position_setpoint_triplet_s *)pos_sp_triplet_sub->get_data(); } void send(const hrt_abstime t) { @@ -62,6 +62,7 @@ protected: /* main state */ mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + union px4_custom_mode custom_mode; custom_mode.data = 0; if (pos_sp_triplet->nav_state == NAV_STATE_NONE) { @@ -96,7 +97,6 @@ protected: custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_LAND; } } - mavlink_custom_mode = custom_mode.data; /* set system state */ if (status->arming_state == ARMING_STATE_INIT @@ -119,7 +119,7 @@ protected: mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, - mavlink_custom_mode, + custom_mode.data, mavlink_state); } |