void autopilot_version_cb(const ros::TimerEvent &event) {
bool ret = false;
try {
auto client = nh.serviceClient<mavros::CommandLong>("cmd/command");
mavros::CommandLong cmd{};
cmd.request.command = MAV_CMD_REQUEST_AUTOPILOT_CAPABILITIES;
cmd.request.confirmation = false;
cmd.request.param1 = 1.0;
ROS_DEBUG_NAMED("sys", "VER: Sending request.");
ret = client.call(cmd);
}
catch (ros::InvalidNameException &ex) {
ROS_ERROR_NAMED("sys", "VER: %s", ex.what());
}
ROS_ERROR_COND_NAMED(!ret, "sys", "VER: command plugin service call failed!");
if (version_retries > 0) {
version_retries--;
ROS_WARN_COND_NAMED(version_retries != RETRIES_COUNT - 1, "sys",
"VER: request timeout, retries left %d", version_retries);
}
else {
uas->update_capabilities(false);
autopilot_version_timer.stop();
ROS_WARN_NAMED("sys", "VER: your FCU don't support AUTOPILOT_VERSION, "
"switched to default capabilities");
}
}
void handle_autopilot_version(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_autopilot_version_t apv;
mavlink_msg_autopilot_version_decode(msg, &apv);
autopilot_version_timer.stop();
uas->update_capabilities(true, apv.capabilities);
// Note based on current APM's impl.
// APM uses custom version array[8] as a string
ROS_INFO_NAMED("sys", "VER: Capabilities 0x%016llx", (long long int)apv.capabilities);
ROS_INFO_NAMED("sys", "VER: Flight software: %08x (%*s)",
apv.flight_sw_version,
8, apv.flight_custom_version);
ROS_INFO_NAMED("sys", "VER: Middleware software: %08x (%*s)",
apv.middleware_sw_version,
8, apv.middleware_custom_version);
ROS_INFO_NAMED("sys", "VER: OS software: %08x (%*s)",
apv.os_sw_version,
8, apv.os_custom_version);
ROS_INFO_NAMED("sys", "VER: Board hardware: %08x", apv.board_version);
ROS_INFO_NAMED("sys", "VER: VID/PID: %04x:%04x", apv.vendor_id, apv.product_id);
ROS_INFO_NAMED("sys", "VER: UID: %016llx", (long long int)apv.uid);
}