ArduPilot - Problem identified that may be causing the mission module to fail (fly_mission example)

[trolledbypro]

Hello, I have found a possible cause of this issue of the mission module failing in ArduPilot.
Using Wireshark, I analyzed the network traffic between my ArduPilot SITL (running inside a WSL2 Docker container that runs the latest ArduPilot Docker image) and the MAVSDK example (running inside WSL2 as I was having issues running it in Windows 10). My MAVSDK library and my fly_mission binary were built in debug mode using CMake through CMake Tools in VSCode (using VSCode WSL Remote session).

Here is my terminal output:

There is a debug message saying the gimbal version is v1, and that the mission is unsupported.
Examining Wireshark, the last message sent before the MISSION_ACK message containing the MAV_MISSION_UNSUPPORTED payload was MAV_CMD_DO_MOUNT_CONFIGURE (204).

When gimbal angles are defined in a mission_item, the gimbal version is verified:

MAVSDK/src/mavsdk/plugins/mission/mission_impl.cpp

Line 460 in fadfe22

const auto temp_gimbal_protocol = _gimbal_protocol.load();

We then use the version above and send a version specific message containing the gimbal configuration. Below, we see that the unsupported message is sent if the gimbal version is assumed to be v1, as is the case in my console output:

MAVSDK/src/mavsdk/plugins/mission/mission_impl.cpp

Lines 1188 to 1221 in fadfe22

	void MissionImpl::add_gimbal_items_v1(
	std::vector<MavlinkMissionTransfer::ItemInt>& int_items,
	unsigned item_i,
	float pitch_deg,
	float yaw_deg)
	{
	if (_enable_absolute_gimbal_yaw_angle) {
	// We need to configure the gimbal to use an absolute angle.
	// Current is the 0th waypoint
	uint8_t current = ((int_items.size() == 0) ? 1 : 0);
	uint8_t autocontinue = 1;
	MavlinkMissionTransfer::ItemInt next_item{
	static_cast<uint16_t>(int_items.size()),
	MAV_FRAME_MISSION,
	MAV_CMD_DO_MOUNT_CONFIGURE,
	current,
	autocontinue,
	MAV_MOUNT_MODE_MAVLINK_TARGETING,
	0.0f, // stabilize roll
	0.0f, // stabilize pitch
	1.0f, // stabilize yaw, FIXME: for now we use this for an absolute yaw angle,
	// because it works.
	0,
	0,
	2.0f, // eventually this is the correct flag to set absolute yaw angle.
	MAV_MISSION_TYPE_MISSION};
	_mission_data.mavlink_mission_item_to_mission_item_indices.push_back(item_i);
	int_items.push_back(next_item);
	}

It appears that MAVSDK is not finding out the supported gimbal version. Gimbal v1 is deprecated in ArduPilot as of version 4.3. Wireshark shows that no GIMBAL_MANAGER_INFORMATION (280) messages were sent from the AP and no MAV_CMD_REQUEST_MESSAGE (512) message from MAVSDK requesting the gimbal version was sent. The GIMBAL_MANAGER_INFORMATION message is what the gimbal module uses to set the gimbal version.

I propose the following two solutions. I think the latter is more inclusive to more ArduPilot versions but the former could be implemented quicker:

• default to v2 if ArduPilot is detected
• send a request for GIMBAL_MANAGER_INFORMATION if ArduPilot is detected

What do you all think?

[JonasVautherin]

Thanks @trolledbypro for looking into this! 🚀

I think I would be in favour of a third version: drop gimbal v1 entirely and always assume v2. There are many problems with the detection anyway.

One that I learn here is that the mission module needs to know about the gimbal protocol, so either we need to duplicate the detection that we already have in the gimbal module, or we need the mission module to depend on the gimbal one somehow (which we have never done with any plugin before).

But on top of that, the detection we have in the gimbal module is fundamentally flawed (and the root cause is in the MAVLink design): MAVLink does not offer a mechanism to differentiate between a timeout and an unsupported message (many modules just ignore commands they don't support, and the MAVLink specs allow a broadcast command which require this behavior). So we can be sure that a drone supports Gimbal v2 when everything works ("yay I received a v2 message, I know it supports v2"), but we can never be sure that it does not support v2 ("I did not receive a v2 message, is it because the drone does not support v2, or is it because the messages were lost?"). We could probably do more convoluted heuristics (like "I got 3 timeouts for the gimbal v2 requests while I received X messages from the drone, so I will say that I am reasonably confident that it was not a timeout"), but... well is it worth it?

I don't know of a single drone that uses Gimbal v1, and v2 has been out there for years. Maybe it is time to drop v1 entirely.

@julianoes what do you think?

[trolledbypro]

Hey @JonasVautherin,

By looking into the Gimbal module code and at my debugging, it seems that the Gimbal module init and enable methods are not being called. I believe this is the case because Wireshark does not detect a MAV_CMD_REQUEST_MESSAGE being sent from to the AP. Even if the message is unsupported, it would still be sent.

I am not sure which thread calls these methods, I don't think it's the main thread because I could not step into these methods:

MAVSDK/src/mavsdk/plugins/gimbal/gimbal_impl.cpp

Lines 29 to 49 in fadfe22

	void GimbalImpl::init()
	{
	_system_impl->register_mavlink_message_handler(
	MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION,
	[this](const mavlink_message_t& message) { process_gimbal_manager_information(message); },
	this);
	}
	void GimbalImpl::deinit() {}
	void GimbalImpl::enable()
	{
	_system_impl->register_timeout_handler(
	[this]() { receive_protocol_timeout(); }, 1.0, &_protocol_cookie);
	MavlinkCommandSender::CommandLong command{};
	command.command = MAV_CMD_REQUEST_MESSAGE;
	command.params.maybe_param1 = {static_cast<float>(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION)};
	command.target_component_id = 0; // any component
	_system_impl->send_command_async(command, nullptr);
	}

These methods establish the gimbal version. The absence of these methods being called is causing a timeout which creates an assumption that gimbal v1 is present:
Here is the code that makes the main thread wait for the version. These methods are being called by the mission module. The timeout is created by the MissionImp module at creation (?)

MAVSDK/src/mavsdk/plugins/gimbal/gimbal_impl.cpp

Lines 197 to 208 in fadfe22

	void GimbalImpl::wait_for_protocol()
	{
	while (_gimbal_protocol == nullptr) {
	std::this_thread::sleep_for(std::chrono::milliseconds(100));
	}
	}
	void GimbalImpl::wait_for_protocol_async(std::function<void()> callback)
	{
	wait_for_protocol();
	callback();
	}

Here is the timeout code that sets it to v1.

MAVSDK/src/mavsdk/plugins/mission/mission_impl.cpp

Lines 126 to 131 in fadfe22

	void MissionImpl::receive_protocol_timeout()
	{
	LogDebug() << "Falling back to gimbal protocol v1";
	_gimbal_protocol = GimbalProtocol::V1;
	_gimbal_protocol_cookie = nullptr;
	}

[JonasVautherin]

Hmm even if you instantiate a Gimbal object in your code? The plugins are initialized lazily, maybe you need to call one of the gimbal functions for it to start 🤔

[trolledbypro]

I edited set_mission example to instantiate a Gimbal object and run set_mode and it still failed. Both the gimbal and mission modules experienced a gimbal mode timeout and defaulted to gimbal v1:

[JonasVautherin]

Both the gimbal and mission modules experienced a gimbal mode timeout and defaulted to gimbal v1

Hmm, so the way we currently detect gimbal v2 is maybe not working with Ardupilot?

But that brings be back to my question above: no need to fix the detection for Ardupilot if we just default to v2 and drop v1 entirely. @julianoes what do you say?

[JonasVautherin]

I talked with Julian, and he agrees that we should drop gimbal v1. I won't be able to do that before 2 weeks because I am going on holidays, so... @trolledbypro if you want to do it before then, feel free! Just remove the detection and default to v2, and you can also remove the v1 implementation files. Really it shouldn't be very hard.

Otherwise I'll give it a shot when I'm back. Does that work for you @trolledbypro?

[trolledbypro]

@JonasVautherin I'll make a branch in my fork and get to work, should be doable for me (especially within 2 weeks)

[JonasVautherin]

Amazing, keep me posted!

[trolledbypro]

I did more debugging, and setting up a gimbal on SITL allowed gimbal version 2 to be successfully detected (I am dumb and forgot the -M flag :( ). However, I still am getting a mission unsupported message even with gimbal version 2.

It appears that gimbal being v1 was not the reason that the mission is failing.
Gimbal v2 sends MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE (1001) as a mission item. DO_GIMBAL_MANAGER_CONFIGURE has only recently been implemented in ArduPilot ArduPilot/ardupilot#23737. This command is used here

MAVSDK/src/mavsdk/plugins/mission/mission_impl.cpp

Lines 1288 to 1312 in e7657c2

	void MissionImpl::acquire_gimbal_control_v2(
	std::vector<MavlinkMissionTransfer::ItemInt>& int_items, unsigned item_i)
	{
	uint8_t current = ((int_items.size() == 0) ? 1 : 0);
	uint8_t autocontinue = 1;
	MavlinkMissionTransfer::ItemInt next_item{
	static_cast<uint16_t>(int_items.size()),
	MAV_FRAME_MISSION,
	MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE,
	current,
	autocontinue,
	-2.0f, // primary control sysid: set itself (autopilot) when running mission
	-2.0f, // primary control compid: itself (autopilot) when running mission
	-1.0f, // secondary control sysid: unchanged
	-1.0f, // secondary control compid: unchanged
	0, // reserved
	0, // reserved
	0, // all devices
	MAV_MISSION_TYPE_MISSION};
	_mission_data.mavlink_mission_item_to_mission_item_indices.push_back(item_i);
	int_items.push_back(next_item);
	}

and is called here

MAVSDK/src/mavsdk/plugins/mission/mission_impl.cpp

Lines 459 to 480 in e7657c2

	if (std::isfinite(item.gimbal_yaw_deg) || std::isfinite(item.gimbal_pitch_deg)) {
	const auto temp_gimbal_protocol = _gimbal_protocol.load();
	switch (temp_gimbal_protocol) {
	case GimbalProtocol::V1:
	add_gimbal_items_v1(
	int_items, item_i, item.gimbal_pitch_deg, item.gimbal_yaw_deg);
	break;
	case GimbalProtocol::V2:
	if (!_mission_data.gimbal_v2_in_control) {
	acquire_gimbal_control_v2(int_items, item_i);
	_mission_data.gimbal_v2_in_control = true;
	}
	add_gimbal_items_v2(
	int_items, item_i, item.gimbal_pitch_deg, item.gimbal_yaw_deg);
	break;
	case GimbalProtocol::Unknown:
	// This should not happen because we wait until we know the protocol version.
	LogErr() << "Unknown gimbal protocol, skipping gimbal commands.";
	break;
	}
	}

It looks like it has not yet been implemented to work as a mission item. This is something I maybe able to fix by adding the support into ArduPilot. I will discuss with the devs there.

[trolledbypro]

These are two issues I made in ArduPilot that should make gimbal control work with MAVSDK (until I find new bugs):
ArduPilot/ardupilot#24691
ArduPilot/ardupilot#24691
ArduPilot has yet to implement acquiring and removing gimbal control as defined in gimbal v2 protocol, so my solution is to accept MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE as a mission_item and have it do nothing until gimbal control is implemented or I get the time to have it modify the gimbal control struct added by the ArduPilot PR mentioned in my previous comment (that is presently being unused). The MAV_CMD_IMAGE_STOP_CAPTURE is also not implemented, it looks like image handling with a photo interval is done differently in ArduPilot. I have also added that command as a do nothing command for now.