diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-13 15:07:09 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-13 15:07:09 +0200 |
commit | e634c9fa99916f795773b60b8884f479fb4e3d36 (patch) | |
tree | 1aed91d4edc1e360b386199fef90a97b3b22b273 /src/modules/mavlink/mavlink_receiver.cpp | |
parent | 85eed22150e4a24098554992a6d77bce6f1ddf31 (diff) | |
parent | 54fc6aa6788a125b387926a45023844daa42ec48 (diff) | |
download | px4-firmware-e634c9fa99916f795773b60b8884f479fb4e3d36.tar.gz px4-firmware-e634c9fa99916f795773b60b8884f479fb4e3d36.tar.bz2 px4-firmware-e634c9fa99916f795773b60b8884f479fb4e3d36.zip |
Merge remote-tracking branch 'origin/master' into offboard2_externalsetpointmessages
Conflicts:
src/modules/mavlink/mavlink_receiver.cpp
Diffstat (limited to 'src/modules/mavlink/mavlink_receiver.cpp')
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 46 |
1 files changed, 46 insertions, 0 deletions
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index da651e4e7..7dd043bbe 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -110,6 +110,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) : _force_sp_pub(-1), _pos_sp_triplet_pub(-1), _vicon_position_pub(-1), + _vision_position_pub(-1), _telemetry_status_pub(-1), _rc_pub(-1), _manual_pub(-1), @@ -157,6 +158,11 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg) case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET: handle_message_set_attitude_target(msg); break; + + case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE: + handle_message_vision_position_estimate(msg); + break; + case MAVLINK_MSG_ID_RADIO_STATUS: handle_message_radio_status(msg); break; @@ -524,6 +530,45 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t } void +MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg) +{ + mavlink_vision_position_estimate_t pos; + mavlink_msg_vision_position_estimate_decode(msg, &pos); + + struct vision_position_estimate vision_position; + memset(&vision_position, 0, sizeof(vision_position)); + + // Use the component ID to identify the vision sensor + vision_position.id = msg->compid; + + vision_position.timestamp_boot = hrt_absolute_time(); + vision_position.timestamp_computer = pos.usec; + vision_position.x = pos.x; + vision_position.y = pos.y; + vision_position.z = pos.z; + + // XXX fix this + vision_position.vx = 0.0f; + vision_position.vy = 0.0f; + vision_position.vz = 0.0f; + + math::Quaternion q; + q.from_euler(pos.roll, pos.pitch, pos.yaw); + + vision_position.q[0] = q(0); + vision_position.q[1] = q(1); + vision_position.q[2] = q(2); + vision_position.q[3] = q(3); + + if (_vision_position_pub < 0) { + _vision_position_pub = orb_advertise(ORB_ID(vision_position_estimate), &vision_position); + + } else { + orb_publish(ORB_ID(vision_position_estimate), _vision_position_pub, &vision_position); + } +} + +void MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) { mavlink_set_attitude_target_t set_attitude_target; @@ -610,6 +655,7 @@ MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg) } } } + void MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg) { |