diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-14 13:34:06 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-07-14 13:34:06 +0200 |
commit | 7a064174110d827182820960b245031e0b4d42ab (patch) | |
tree | 6673fb2dc74150ae4729a39dc55b700b865fd03d /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 747eda92b9396e38746d505e5d79a7528e117c89 (diff) | |
download | px4-firmware-7a064174110d827182820960b245031e0b4d42ab.tar.gz px4-firmware-7a064174110d827182820960b245031e0b4d42ab.tar.bz2 px4-firmware-7a064174110d827182820960b245031e0b4d42ab.zip |
mavlink: external setpoint feed trough functionality
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 15 |
1 files changed, 15 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index bd1296f62..9df843e58 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -451,6 +451,21 @@ MavlinkReceiver::handle_message_local_ned_position_setpoint_external(mavlink_mes } else { orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp); } + + /* If we are in offboard control mode and offboard control loop through is enabled + * also publish the setpoint topic which is read by the controller */ + if (_mavlink->get_forward_externalsp()) { + bool updated; + orb_check(_control_mode_sub, &updated); + if (updated) { + orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode); + } + + if (_control_mode.flag_control_offboard_enabled) { + //XXX: copy to and publish setpoint triplet here + } + + } } } |