void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_local_position_ned_t pos_ned;
		mavlink_msg_local_position_ned_decode(msg, &pos_ned);

		auto position = UAS::transform_frame_ned_enu(Eigen::Vector3d(pos_ned.x, pos_ned.y, pos_ned.z));
		auto orientation = uas->get_attitude_orientation();

		auto pose = boost::make_shared<geometry_msgs::PoseStamped>();

		pose->header = uas->synchronized_header(frame_id, pos_ned.time_boot_ms);

		tf::pointEigenToMsg(position, pose->pose.position);

		// XXX FIX ME #319
		tf::quaternionTFToMsg(orientation, pose->pose.orientation);

		local_position.publish(pose);

		if (tf_send) {
			geometry_msgs::TransformStamped transform;

			transform.header.stamp = pose->header.stamp;
			transform.header.frame_id = tf_frame_id;
			transform.child_frame_id = tf_child_frame_id;

			transform.transform.rotation = pose->pose.orientation;
			tf::vectorEigenToMsg(position, transform.transform.translation);

			tf2_broadcaster.sendTransform(transform);
		}
	}

	void command_int(bool broadcast,
			uint8_t frame, uint16_t command,
			uint8_t current, uint8_t autocontinue,
			float param1, float param2,
			float param3, float param4,
			int32_t x, int32_t y,
			float z)
	{
		mavlink_message_t msg;
		const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();

		mavlink_msg_command_int_pack_chan(UAS_PACK_CHAN(uas), &msg,
				tgt_sys_id,
				tgt_comp_id,
				frame,
				command,
				current,
				autocontinue,
				param1, param2,
				param3, param4,
				x, y, z);
		UAS_FCU(uas)->send_message(&msg);
	}

	void command_long(bool broadcast,
			uint16_t command, uint8_t confirmation,
			float param1, float param2,
			float param3, float param4,
			float param5, float param6,
			float param7)
	{
		mavlink_message_t msg;
		const uint8_t tgt_sys_id = (broadcast) ? 0 : uas->get_tgt_system();
		const uint8_t tgt_comp_id = (broadcast) ? 0 :
			(use_comp_id_system_control) ?
				MAV_COMP_ID_SYSTEM_CONTROL : uas->get_tgt_component();
		const uint8_t confirmation_fixed = (broadcast) ? 0 : confirmation;

		mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg,
				tgt_sys_id,
				tgt_comp_id,
				command,
				confirmation_fixed,
				param1, param2,
				param3, param4,
				param5, param6,
				param7);
		UAS_FCU(uas)->send_message(&msg);
	}

    void connection_cb(bool connected) {
        // if connection changes, start delayed version request
        version_retries = RETRIES_COUNT;
        if (connected)
            autopilot_version_timer.start();
        else
            autopilot_version_timer.stop();

        // add/remove APM diag tasks
        if (connected && disable_diag && uas->is_ardupilotmega()) {
#ifdef MAVLINK_MSG_ID_MEMINFO
            UAS_DIAG(uas).add(mem_diag);
#endif
#ifdef MAVLINK_MSG_ID_HWSTATUS
            UAS_DIAG(uas).add(hwst_diag);
#endif
#if !defined(MAVLINK_MSG_ID_MEMINFO) || !defined(MAVLINK_MSG_ID_HWSTATUS)
            ROS_INFO_NAMED("sys", "SYS: APM detected, but mavros uses different dialect. "
                           "Extra diagnostic disabled.");
#else
            ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics enabled.");
#endif
        }
        else {
            UAS_DIAG(uas).removeByName(mem_diag.getName());
            UAS_DIAG(uas).removeByName(hwst_diag.getName());
            ROS_DEBUG_NAMED("sys", "SYS: APM extra diagnostics disabled.");
        }
    }

	/*
	 * Arming/disarming the UAV
	 */
	void arming(const mms::Arm::ConstPtr msg){
		mavlink_message_t msg_mav;
		if (msg->arm_disarm){
			enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
			float param1 = 1;      //1-->arm 0-->disarm
			float param2 = 0;      //not used
			float param3 = 0;      //not used
			float param4 = 0;      //not used
			float param5 = 0;      //not used
			float param6 = 0;      //not used
			float param7 = 0;      //not used
			uint8_t confirmation = 1;
			mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
								uas->get_tgt_system(),
								uas->get_tgt_component(),
								command,
								confirmation,
								param1, param2,
								param3, param4,
								param5, param6,
								param7);
			UAS_FCU(uas)->send_message(&msg_mav);
			ROS_INFO("Arming UAV");
		} else {
			enum MAV_CMD command = MAV_CMD_COMPONENT_ARM_DISARM;
			float param1 = 0;      //1-->arm 0-->disarm
			float param2 = 0;      //not used
			float param3 = 0;      //not used
			float param4 = 0;      //not used
			float param5 = 0;      //not used
			float param6 = 0;      //not used
			float param7 = 0;      //not used
			uint8_t confirmation = 1;
			mavlink_msg_command_long_pack_chan(UAS_PACK_CHAN(uas), &msg_mav,
								uas->get_tgt_system(),
								uas->get_tgt_component(),
								command,
								confirmation,
								param1, param2,
								param3, param4,
								param5, param6,
								param7);
			UAS_FCU(uas)->send_message(&msg_mav);                        //TODO decide if send or not disarm by software
			ROS_INFO("Disarming UAV");
		}
	}

    bool set_mode_cb(mavros::SetMode::Request &req,
                     mavros::SetMode::Response &res) {
        mavlink_message_t msg;
        uint8_t base_mode = req.base_mode;
        uint32_t custom_mode = 0;

        if (req.custom_mode != "") {
            if (!uas->cmode_from_str(req.custom_mode, custom_mode)) {
                res.success = false;
                return true;
            }

            base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
        }

        mavlink_msg_set_mode_pack_chan(UAS_PACK_CHAN(uas), &msg,
                                       uas->get_tgt_system(),
                                       base_mode,
                                       custom_mode);
        UAS_FCU(uas)->send_message(&msg);
        res.success = true;
        return true;
    }

	void handle_global_position_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_global_position_int_t gpos;
		mavlink_msg_global_position_int_decode(msg, &gpos);

		auto pose_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
		auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
		auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
		auto relative_alt = boost::make_shared<std_msgs::Float64>();
		auto compass_heading = boost::make_shared<std_msgs::Float64>();

		auto header = uas->synchronized_header(frame_id, gpos.time_boot_ms);

		// Global position fix
		fix->header = header;

		fill_lla(gpos, fix);

		// fill GPS status fields using GPS_RAW data
		auto raw_fix = uas->get_gps_fix();
		if (raw_fix) {
			fix->status.service = raw_fix->status.service;
			fix->status.status = raw_fix->status.status;
			fix->position_covariance = raw_fix->position_covariance;
			fix->position_covariance_type = raw_fix->position_covariance_type;
		}
		else {
			// no GPS_RAW_INT -> fix status unknown
			fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
			fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;

			// we don't know covariance
			fill_unknown_cov(fix);
		}

		/* Global position velocity
		 *
		 * No transform needed:
		 * X: latitude m/s
		 * Y: longitude m/s
		 * Z: altitude m/s
		 */
		vel->header = header;
		tf::vectorEigenToMsg(
				Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
				vel->twist.linear);

		relative_alt->data = gpos.relative_alt / 1E3;	// in meters
		compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN;	// in degrees

		// local position (UTM) calculation
		double northing, easting;
		std::string zone;

		/** @note Adapted from gps_umd ROS package @http://wiki.ros.org/gps_umd
		 *  Author: Ken Tossell <ken AT tossell DOT net>
		 */
		UTM::LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);

		pose_cov->header = header;
		pose_cov->pose.pose.position.x = easting;
		pose_cov->pose.pose.position.y = northing;
		pose_cov->pose.pose.position.z = relative_alt->data;

		// XXX FIX ME #319, We really need attitude on UTM?
		tf::quaternionTFToMsg(uas->get_attitude_orientation(), pose_cov->pose.pose.orientation);

		// Use ENU covariance to build XYZRPY covariance
		UAS::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
		UAS::EigenMapCovariance6d cov_out(pose_cov->pose.covariance.data());
		cov_out <<
			gps_cov(0, 0) , gps_cov(0, 1) , gps_cov(0, 2) , 0.0     , 0.0     , 0.0     ,
			gps_cov(1, 0) , gps_cov(1, 1) , gps_cov(1, 2) , 0.0     , 0.0     , 0.0     ,
			gps_cov(2, 0) , gps_cov(2, 1) , gps_cov(2, 2) , 0.0     , 0.0     , 0.0     ,
			0.0           , 0.0           , 0.0           , rot_cov , 0.0     , 0.0     ,
			0.0           , 0.0           , 0.0           , 0.0     , rot_cov , 0.0     ,
			0.0           , 0.0           , 0.0           , 0.0     , 0.0     , rot_cov ;

		// publish
		gp_fix_pub.publish(fix);
		gp_pos_pub.publish(pose_cov);
		gp_vel_pub.publish(vel);
		gp_rel_alt_pub.publish(relative_alt);
		gp_hdg_pub.publish(compass_heading);

		// TF
		if (tf_send) {
			geometry_msgs::TransformStamped transform;

			transform.header.stamp = pose_cov->header.stamp;
			transform.header.frame_id = tf_frame_id;
			transform.child_frame_id = tf_child_frame_id;

			// XXX do we really need rotation in UTM coordinates?
			// setRotation()
			transform.transform.rotation = pose_cov->pose.pose.orientation;

			// setOrigin(), why to do transform_frame?
			transform.transform.translation.x = pose_cov->pose.pose.position.x;
			transform.transform.translation.y = pose_cov->pose.pose.position.y;
			transform.transform.translation.z = pose_cov->pose.pose.position.z;
//.........这里部分代码省略.........

	/**
	 * Receive distance sensor data from FCU.
	 */
	void handle_distance_sensor(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_distance_sensor_t dist_sen;
		mavlink_msg_distance_sensor_decode(msg, &dist_sen);

		auto it = sensor_map.find(dist_sen.id);
		if (it == sensor_map.end()) {
			ROS_ERROR_NAMED("distance_sensor",
					"DS: no mapping for sensor id: %d, type: %d, orientation: %d",
					dist_sen.id, dist_sen.type, dist_sen.orientation);
			return;
		}

		auto sensor = it->second;
		if (sensor->is_subscriber) {
			ROS_ERROR_NAMED("distance_sensor",
					"DS: %s (id %d) is subscriber, but i got sensor data for that id from FCU",
					sensor->topic_name.c_str(), sensor->sensor_id);
			return;
		}

		if (sensor->orientation >= 0 && dist_sen.orientation != sensor->orientation) {
			ROS_ERROR_NAMED("distance_sensor",
					"DS: %s: received sensor data has different orientation (%s) than in config (%s)!",
					sensor->topic_name.c_str(),
					UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation)).c_str(),
					UAS::str_sensor_orientation(static_cast<MAV_SENSOR_ORIENTATION>(sensor->orientation)).c_str());
		}

		auto range = boost::make_shared<sensor_msgs::Range>();

		range->header = uas->synchronized_header(sensor->frame_id, dist_sen.time_boot_ms);

		range->min_range = dist_sen.min_distance * 1E-2; // in meters
		range->max_range = dist_sen.max_distance * 1E-2;
		range->field_of_view = sensor->field_of_view;

		if (dist_sen.type == MAV_DISTANCE_SENSOR_LASER) {
			range->radiation_type = sensor_msgs::Range::INFRARED;
		}
		else if (dist_sen.type == MAV_DISTANCE_SENSOR_ULTRASOUND) {
			range->radiation_type = sensor_msgs::Range::ULTRASOUND;
		}
		else {
			ROS_ERROR_NAMED("distance_sensor",
					"DS: %s: Wrong/undefined type of sensor (type: %d). Droping!...",
					sensor->topic_name.c_str(), dist_sen.type);
			return;
		}

		range->range = dist_sen.current_distance * 1E-2;	// in meters

		if (sensor->send_tf) {
			/* variables init */
			auto q = UAS::sensor_orientation_matching(static_cast<MAV_SENSOR_ORIENTATION>(dist_sen.orientation));

			geometry_msgs::TransformStamped transform;

			transform.header = uas->synchronized_header("fcu", dist_sen.time_boot_ms);
			transform.child_frame_id = sensor->frame_id;

			/* rotation and position set */
			tf::quaternionEigenToMsg(q, transform.transform.rotation);
			tf::vectorEigenToMsg(sensor->position, transform.transform.translation);

			/* transform broadcast */
			uas->tf2_broadcaster.sendTransform(transform);
		}

		sensor->pub.publish(range);
	}

	void handle_timesync(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_timesync_t tsync;
		mavlink_msg_timesync_decode(msg, &tsync);

		uint64_t now_ns = ros::Time::now().toNSec();

		if (tsync.tc1 == 0) {
			send_timesync_msg(now_ns, tsync.ts1);
			return;
		}
		else if (tsync.tc1 > 0) {
			int64_t offset_ns = (tsync.ts1 + now_ns - tsync.tc1 * 2) / 2;
			int64_t dt = time_offset_ns - offset_ns;

			if (std::abs(dt) > 10000000) {		// 10 millisecond skew
				time_offset_ns = offset_ns;	// hard-set it.
				uas->set_time_offset(time_offset_ns);

				dt_diag.clear();
				dt_diag.set_timestamp(tsync.tc1);

				ROS_WARN_THROTTLE_NAMED(10, "time", "TM: Clock skew detected (%0.9f s). "
						"Hard syncing clocks.", dt / 1e9);
			}
			else {
				average_offset(offset_ns);
				dt_diag.tick(dt, tsync.tc1, time_offset_ns);

				uas->set_time_offset(time_offset_ns);
			}
		}
	}

    void handle_heartbeat(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        if (!uas->is_my_target(sysid)) {
            ROS_DEBUG_NAMED("sys", "HEARTBEAT from [%d, %d] dropped.", sysid, compid);
            return;
        }

        mavlink_heartbeat_t hb;
        mavlink_msg_heartbeat_decode(msg, &hb);

        // update context && setup connection timeout
        uas->update_heartbeat(hb.type, hb.autopilot);
        uas->update_connection_status(true);
        timeout_timer.stop();
        timeout_timer.start();

        // build state message after updating uas
        auto state_msg = boost::make_shared<mavros::State>();
        state_msg->header.stamp = ros::Time::now();
        state_msg->armed = hb.base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
        state_msg->guided = hb.base_mode & MAV_MODE_FLAG_GUIDED_ENABLED;
        state_msg->mode = uas->str_mode_v10(hb.base_mode, hb.custom_mode);

        state_pub.publish(state_msg);
        hb_diag.tick(hb.type, hb.autopilot, state_msg->mode, hb.system_status);
    }

	void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_gps_raw_int_t raw_gps;
		mavlink_msg_gps_raw_int_decode(msg, &raw_gps);

		auto fix = boost::make_shared<sensor_msgs::NavSatFix>();

		fix->header = uas->synchronized_header(frame_id, raw_gps.time_usec);

		fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
		if (raw_gps.fix_type > 2)
			fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
		else {
			ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
			fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
		}

		fill_lla(raw_gps, fix);

		float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
		float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;

		if (!isnan(eph)) {
			const double hdop = eph;

			// From nmea_navsat_driver
			fix->position_covariance[0 + 0] = \
				fix->position_covariance[3 + 1] = std::pow(hdop, 2);
			fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
			fix->position_covariance_type =
					sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
		}
		else {
			fill_unknown_cov(fix);
		}

		// store & publish
		uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
		raw_fix_pub.publish(fix);

		if (raw_gps.vel != UINT16_MAX &&
				raw_gps.cog != UINT16_MAX) {
			double speed = raw_gps.vel / 1E2;				// m/s
			double course = angles::from_degrees(raw_gps.cog / 1E2);	// rad

			auto vel = boost::make_shared<geometry_msgs::TwistStamped>();

			vel->header.frame_id = frame_id;
			vel->header.stamp = fix->header.stamp;

			// From nmea_navsat_driver
			vel->twist.linear.x = speed * std::sin(course);
			vel->twist.linear.y = speed * std::cos(course);

			raw_vel_pub.publish(vel);
		}
	}

	/**
	 * Common function for command service callbacks.
	 *
	 * NOTE: success is bool in messages, but has unsigned char type in C++
	 */
	bool send_command_long_and_wait(uint16_t command, uint8_t confirmation,
			float param1, float param2,
			float param3, float param4,
			float param5, float param6,
			float param7,
			unsigned char &success, uint8_t &result) {
		unique_lock lock(mutex);

		/* check transactions */
		for (auto it = ack_waiting_list.cbegin();
				it != ack_waiting_list.cend(); it++)
			if ((*it)->expected_command == command) {
				ROS_WARN_THROTTLE_NAMED(10, "cmd", "Command %u alredy in progress", command);
				return false;
			}

		//! @note APM always send COMMAND_ACK, while PX4 never.
		bool is_ack_required = (confirmation != 0 || uas->is_ardupilotmega()) && !uas->is_px4();
		if (is_ack_required)
			ack_waiting_list.push_back(new CommandTransaction(command));

		command_long(command, confirmation,
				param1, param2,
				param3, param4,
				param5, param6,
				param7);

		if (is_ack_required) {
			auto it = ack_waiting_list.begin();
			for (; it != ack_waiting_list.end(); it++)
				if ((*it)->expected_command == command)
					break;

			if (it == ack_waiting_list.end()) {
				ROS_ERROR_NAMED("cmd", "CommandTransaction not found for %u", command);
				return false;
			}

			lock.unlock();
			bool is_not_timeout = wait_ack_for(*it);
			lock.lock();

			success = is_not_timeout && (*it)->result == MAV_RESULT_ACCEPTED;
			result = (*it)->result;

			delete *it;
			ack_waiting_list.erase(it);
		}
		else {
			success = true;
			result = MAV_RESULT_ACCEPTED;
		}

		return true;
	}

	void uas_store_attitude(tf::Quaternion &orientation,
			geometry_msgs::Vector3 &gyro_vec,
			geometry_msgs::Vector3 &acc_vec)
	{
		tf::Vector3 angular_velocity;
		tf::Vector3 linear_acceleration;
		tf::vector3MsgToTF(gyro_vec, angular_velocity);
		tf::vector3MsgToTF(acc_vec, linear_acceleration);

		uas->set_attitude_orientation(orientation);
		uas->set_attitude_angular_velocity(angular_velocity);
		uas->set_attitude_linear_acceleration(linear_acceleration);
	}

	/* -*- diagnostics -*- */
	void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
		int fix_type, satellites_visible;
		float eph, epv;

		uas->get_gps_epts(eph, epv, fix_type, satellites_visible);

		if (satellites_visible <= 0)
			stat.summary(2, "No satellites");
		else if (fix_type < 2)
			stat.summary(1, "No fix");
		else if (fix_type == 2)
			stat.summary(0, "2D fix");
		else if (fix_type >= 3)
			stat.summary(0, "3D fix");

		stat.addf("Satellites visible", "%zd", satellites_visible);
		stat.addf("Fix type", "%d", fix_type);

		if (!isnan(eph))
			stat.addf("EPH (m)", "%.2f", eph);
		else
			stat.add("EPH (m)", "Unknown");

		if (!isnan(epv))
			stat.addf("EPV (m)", "%.2f", epv);
		else
			stat.add("EPV (m)", "Unknown");
	}

	/**
	 * Send transform to FCU position controller
	 *
	 * Note: send only XYZ, Yaw
	 */
	void send_setpoint_transform(const tf::Transform &transform, const ros::Time &stamp) {
		// ENU frame
		tf::Vector3 origin = transform.getOrigin();
		tf::Quaternion q = transform.getRotation();

		/* Documentation start from bit 1 instead 0,
		 * Ignore velocity and accel vectors, yaw rate
		 */
		uint16_t ignore_all_except_xyz_y = (1 << 11) | (7 << 6) | (7 << 3);

		if (uas->is_px4()) {
			/**
			 * Current PX4 has bug: it cuts throttle if there no velocity sp
			 * Issue #273.
			 *
			 * @todo Revesit this quirk later. Should be fixed in firmware.
			 */
			ignore_all_except_xyz_y = (1 << 11) | (7 << 6);
		}

		// ENU->NED. Issue #49.
		set_position_target_local_ned(stamp.toNSec() / 1000000,
				MAV_FRAME_LOCAL_NED,
				ignore_all_except_xyz_y,
				origin.y(), origin.x(), -origin.z(),
				0.0, 0.0, 0.0,
				0.0, 0.0, 0.0,
				tf::getYaw(q), 0.0);
	}

    void autopilot_version_cb(const ros::TimerEvent &event) {
        bool ret = false;

        try {
            auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");

            mavros::CommandLong cmd{};
            cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
            cmd.request.confirmation = false;
            cmd.request.param1 = 1.0;

            ROS_DEBUG_NAMED("sys", "VER: Sending request.");
            ret = client.call(cmd);
        }
        catch (ros::InvalidNameException &ex) {
            ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
        }

        ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");

        if (version_retries > 0) {
            version_retries--;
            ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
                                "VER: request timeout, retries left %d", version_retries);
        }
        else {
            uas->update_capabilities(false);
            autopilot_version_timer.stop();
            ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
                           "switched to default capabilities");
        }
    }

    void handle_statustext(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        mavlink_statustext_t textm;
        mavlink_msg_statustext_decode(msg, &textm);

        std::string text(textm.text,
                         strnlen(textm.text, sizeof(textm.text)));

        if (uas->is_ardupilotmega())
            process_statustext_apm_quirk(textm.severity, text);
        else
            process_statustext_normal(textm.severity, text);
    }

	void handle_cam_trig(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_camera_trigger_t ctrig;
		mavlink_msg_camera_trigger_decode(msg, &ctrig);

		auto sync_msg = boost::make_shared<mavros_extras::CamIMUStamp>();

		sync_msg->frame_stamp = uas->synchronise_stamp(ctrig.time_usec);
		sync_msg->frame_seq_id = ctrig.seq;

		cam_imu_pub.publish(sync_msg);
		ROS_INFO("Sent timestamp %u.%u:",sync_msg->frame_stamp.sec,sync_msg->frame_stamp.nsec);
	}

    void handle_vibration(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
        mavlink_vibration_t vibration;
        mavlink_msg_vibration_decode(msg, &vibration);

        auto vibe_msg = boost::make_shared<mavros_msgs::Vibration>();

        vibe_msg->header = uas->synchronized_header(frame_id, vibration.time_usec);

        // TODO no transform_frame?
        vibe_msg->vibration.x = vibration.vibration_x;
        vibe_msg->vibration.y = vibration.vibration_y;
        vibe_msg->vibration.z = vibration.vibration_z;
        vibe_msg->clipping[0] = vibration.clipping_0;
        vibe_msg->clipping[1] = vibration.clipping_1;
        vibe_msg->clipping[2] = vibration.clipping_2;

        vibration_pub.publish(vibe_msg);
    }

	void handle_local_position_ned(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		mavlink_local_position_ned_t pos_ned;
		mavlink_msg_local_position_ned_decode(msg, &pos_ned);

		tf::Transform transform;
		transform.setOrigin(tf::Vector3(pos_ned.y, pos_ned.x, -pos_ned.z));
		transform.setRotation(uas->get_attitude_orientation());

		geometry_msgs::PoseStampedPtr pose = boost::make_shared<geometry_msgs::PoseStamped>();

		tf::poseTFToMsg(transform, pose->pose);
		pose->header.frame_id = fixed_frame_id;
		pose->header.stamp = ros::Time();

		publish_vehicle_marker(); 
		publish_vis_marker(pose);

	}