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
passtrough.send_message(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,
Mike
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;
mavlink_msg_command_ack_pack(
passthrough.get_our_sysid(),
passthrough.get_our_compid(),
&_reply,
cmd_id,
mav_result,
255,
-1,
passthrough.get_target_sysid(),
passthrough.get_target_compid());
MavlinkPassthrough::Result _result = passthrough.send_message(_reply);
switch(_result)
{
case MavlinkPassthrough::Result::Unknown: /**< @brief Unknown error. */
cout << "passthrough.send_message()->Unknown" << endl;
break;
case MavlinkPassthrough::Result::Success: /**< @brief Success. */
cout << "passthrough.send_message()->Success" << endl;
break;
case MavlinkPassthrough::Result::ConnectionError: /**< @brief Connection error. */
cout << "passthrough.send_message()->ConnectionError" << endl;
break;
case MavlinkPassthrough::Result::CommandNoSystem: /**< @brief System not available. */
cout << "passthrough.send_message()->CommandNoSystem" << endl;
break;
case MavlinkPassthrough::Result::CommandBusy: /**< @brief System is busy. */
cout << "passthrough.send_message()->CommandBusy" << endl;
break;
case MavlinkPassthrough::Result::CommandDenied: /**< @brief Command has been denied. */
cout << "passthrough.send_message()->CommandDenied" << endl;
break;
case MavlinkPassthrough::Result::CommandUnsupported: /**< @brief Command is not supported. */
cout << "passthrough.send_message()->CommandUnsupported" << endl;
break;
case MavlinkPassthrough::Result::CommandTimeout: /**< @brief A timeout happened. */
cout << "passthrough.send_message()->CommandTimeout" << endl;
break;
}
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) 
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.
