Using MavlinkPassthrough to collect MAV_CMD_DO_MOTOR_TEST messages

I need to write a piece of code to receive (and ACK) a MAV_CMD_DO_MOTOR_TEST message.
I’m using MavlinkPassthrough plugin since I have not found a better way to do it.
I’m using this code:

        auto passtrough = MavlinkPassthrough{gcs_system};

	passtrough.subscribe_message_async(MAVLINK_MSG_ID_COMMAND_INT, [](const __mavlink_message& message)
			__mavlink_command_int_t cmd;

			cout << "Received from: " << (int)message.sysid << " compid: " << (int)message.compid << " msgid: " << message.msgid << endl;

			mavlink_msg_command_int_decode(&message, &cmd);

			if(cmd.command == MAV_CMD_DO_MOTOR_TEST)
				cout << "COMMAND_INT.cmd: " << (int)cmd.command << endl;
				cout << "\t autocontinue: " << (int)cmd.autocontinue << endl;
				cout << "\t current: " << (int)cmd.current << endl;
				cout << "\t frame: " << (int)cmd.frame << endl;
				cout << "\t param1: " << cmd.param1 << endl;
				cout << "\t param2: " << cmd.param2 << endl;
				cout << "\t param3: " << cmd.param3 << endl;
				cout << "\t param4: " << cmd.param4 << endl;
				cout << "\t target_component: " << (int)cmd.target_component << endl;
				cout << "\t target_system: " << (int)cmd.target_system << endl;
				cout << "\t x: " << cmd.x << endl;
				cout << "\t y: " << cmd.y << endl;
				cout << "\t z: " << cmd.z << endl;

				__mavlink_message reply;
				//TODO: fill reply

This works fine since it receive all COMMAND_INT messages but then I need to send ACK reply and I cannot find a sample code about filling up raw messages… I think there’s some undocumented feature like mavlink_msg_command_int_decode function I used is this code but I’m not able to find it…

P.S. Maybe is a better approach to extend Action/ActionServer plugins? It doesn’t sound like the better container for such kind of commands… maybe writing up a new plugin from scratch?

Best regards,

Does this example help? Instead of the battery message, I guess you would send the command ack.

hallo, yes it pointed me in the right direction (MAVSDK/connection.cpp at main · mavlink/MAVSDK · GitHub), so I worked out my solution:

                const auto cmd_id = cmd.command;
	            auto mav_result = MAV_RESULT_ACCEPTED;

	            mavlink_message_t _reply;


	            MavlinkPassthrough::Result _result = passthrough.send_message(_reply);

					case MavlinkPassthrough::Result::Unknown: /**< @brief Unknown error. */
						cout << "passthrough.send_message()->Unknown" << endl;
					case MavlinkPassthrough::Result::Success: /**< @brief Success. */
						cout << "passthrough.send_message()->Success" << endl;
					case MavlinkPassthrough::Result::ConnectionError: /**< @brief Connection error. */
						cout << "passthrough.send_message()->ConnectionError" << endl;
					case MavlinkPassthrough::Result::CommandNoSystem: /**< @brief System not available. */
						cout << "passthrough.send_message()->CommandNoSystem" << endl;
					case MavlinkPassthrough::Result::CommandBusy: /**< @brief System is busy. */
						cout << "passthrough.send_message()->CommandBusy" << endl;
					case MavlinkPassthrough::Result::CommandDenied: /**< @brief Command has been denied. */
						cout << "passthrough.send_message()->CommandDenied" << endl;
					case MavlinkPassthrough::Result::CommandUnsupported: /**< @brief Command is not supported. */
						cout << "passthrough.send_message()->CommandUnsupported" << endl;
					case MavlinkPassthrough::Result::CommandTimeout: /**< @brief A timeout happened. */
						cout << "passthrough.send_message()->CommandTimeout" << endl;

UNLUCKLY the recipient of the ACK message stil return timeout here:

mavsdk::MavlinkPassthrough::Result _result = _passtrough->send_command_int(_command);

Hmm that’s weird. Are you sending the ACK right away on the server side?

Hi Jonas, as you can see in the code above I’m sending MAV_RESULT_ACCEPTED just upon message reception (on server side), but matching send_command_int call in client side still report TIMEOUT
Please note that the server side .send_message() returns SUCCESS instead.
Anyway I opted for a different approach extending Action and ActionServer plugins to implement MSG_DO_MOTOR_TEST send and receive. Probably it’s not the most correct plugin but I didn’t find a more fitting one.

I guess you would need to debug it on the client side and see if the command message arrives (e.g. with Wireshark) :+1:

why? I suppose mavdsk is able to deliver the packet…

Just to understand what is happening where, and to see if that’s a bug in your code or in MAVSDK, I would say.