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

	/**
	 * 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);
	}

	/**
	 * 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;
	}