C++ UAS::is_ardupilotmega方法代码示例

    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.");
        }
    }

	/**
	 * 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 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_raw_imu(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
		if (has_hr_imu || has_scaled_imu)
			return;

		sensor_msgs::ImuPtr imu_msg = boost::make_shared<sensor_msgs::Imu>();
		mavlink_raw_imu_t imu_raw;
		mavlink_msg_raw_imu_decode(msg, &imu_raw);

		std_msgs::Header header;
		header.stamp = ros::Time::now();
		header.frame_id = frame_id;

		/* NOTE: APM send SCALED_IMU data as RAW_IMU */
		fill_imu_msg_raw(imu_msg,
				imu_raw.xgyro * MILLIRS_TO_RADSEC,
				-imu_raw.ygyro * MILLIRS_TO_RADSEC,
				-imu_raw.zgyro * MILLIRS_TO_RADSEC,
				imu_raw.xacc * MILLIG_TO_MS2,
				-imu_raw.yacc * MILLIG_TO_MS2,
				-imu_raw.zacc * MILLIG_TO_MS2);

		if (!uas->is_ardupilotmega()) {
			ROS_WARN_THROTTLE_NAMED(60, "imu", "RAW_IMU: linear acceleration known on APM only");
			linear_accel_vec.x = 0.0;
			linear_accel_vec.y = 0.0;
			linear_accel_vec.z = 0.0;
		}

		imu_msg->header = header;
		imu_raw_pub.publish(imu_msg);

		/* -*- magnetic vector -*- */
		sensor_msgs::MagneticFieldPtr magn_msg = boost::make_shared<sensor_msgs::MagneticField>();

		// Convert from local NED plane to ENU
		magn_msg->magnetic_field.x = imu_raw.ymag * MILLIT_TO_TESLA;
		magn_msg->magnetic_field.y = imu_raw.xmag * MILLIT_TO_TESLA;
		magn_msg->magnetic_field.z = -imu_raw.zmag * MILLIT_TO_TESLA;

		magn_msg->magnetic_field_covariance = magnetic_cov;

		magn_msg->header = header;
		magn_pub.publish(magn_msg);
	}