Send messages from callback

mavros_extras/src/plugins/mount_control.cpp

@@ -7,7 +7,7 @@
  * @{
  */
 /*
- * Copyright 2018 Jaeyoung Lim.
+ * Copyright 2019 Jaeyoung Lim.
  *
  * This file is part of the mavros package and subject to the license terms
  * in the top-level LICENSE file of the mavros repository.
@@ -22,10 +22,8 @@ namespace mavros {
 namespace extra_plugins {
 
 //! Mavlink enumerations
-using mavlink::common::MAV_TYPE;
-using mavlink::common::MAV_STATE;
-using mavlink::common::MAV_COMPONENT;
 using mavlink::common::MAV_MOUNT_MODE;
+using mavlink::common::MAV_CMD;
 using utils::enum_value;
 
 /**
@@ -59,11 +57,27 @@ class MountControlPlugin : public plugin::PluginBase {
 	/**
 	 * @brief Send mount control commands to vehicle
 	 *
-	 * Message specification: https://mavlink.io/en/messages/common.html#DO_MOUNT_CONTROL
+	 * Message specification: https://mavlink.io/en/messages/common.html#MAV_CMD_DO_MOUNT_CONTROL
 	 * @param req	received MountControl msg
 	 */
 	void command_cb(const mavros_msgs::MountControl::ConstPtr &req)
 	{
+		mavlink::common::msg::COMMAND_LONG cmd {};
+
+		const uint8_t tgt_sys_id = m_uas->get_tgt_system();
+		const uint8_t tgt_comp_id = m_uas->get_tgt_component();
+		cmd.target_system = tgt_sys_id;
+		cmd.target_component = tgt_comp_id;
+		cmd.command = enum_value(MAV_CMD::DO_MOUNT_CONTROL);
+		cmd.param1 = req->pitch;
+		cmd.param2 = req->roll;
+		cmd.param3 = req->yaw;
+		cmd.param4 = req->altitude; // 
+		cmd.param5 = req->latitude; // lattitude in degrees * 1E7
+		cmd.param6 = req->longitude; // longitude in degrees * 1E7
+		cmd.param7 = req->mode; // MAV_MOUNT_MODE
+
+		UAS_FCU(m_uas)->send_message_ignore_drop(cmd);
 	}
 };
 }	// namespace extra_plugins