Payload Generation of Mavlink heartbeat message doesn't seem correct

[Peter Milani]

Hi All

I'm new to mavlink and I'm trying to integrate it with its ROS stack, which I catkinized. It compiles well, but I'm having trouble generating a HeartBeat message, particularly around the generation of the payload. In the code below I'm generating a mavlink_message_t heartbeatMessage, and then filling a mavlink_ros message from that and subsequently publishing it over ROS.

    mavlink_message_t heartbeatMessage;

    int length = mavlink_msg_heartbeat_pack(1, MAV_COMP_ID_ALL, &heartbeatMessage, MAV_TYPE_GENERIC, MAV_AUTOPILOT_GENERIC, MAV_MODE_MANUAL_DISARMED, 1, MAV_STATE_ACTIVE);
    ROS_INFO("Length %d", heartbeatMessage.len);
    mavlink_ros::Mavlink message;
    std_msgs::Header header;

    message.header.frame_id = "swarmbot";
    message.header.seq = 1;
    message.header.stamp = ros::Time::now();

    message.compid = heartbeatMessage.compid;
    message.len = heartbeatMessage.len;
    message.msgid = heartbeatMessage.msgid;
    message.sysid = heartbeatMessage.sysid;
    message.seq = heartbeatMessage.seq;

    ROS_INFO("Heartbeat Payload");

    for (int i =0; i<message.len/8; i++)
    {
        message.payload64.push_back(heartbeatMessage.payload64[i]);
        ROS_INFO("i: %d", heartbeatMessage.payload64[i]);
        ROS_INFO("RosMessage i %d,: %d", i, message.payload64[i]);
    }

I'm using the mavlink_msg_heartbeat_pack() to package the detail of my message into the payload. However only the first uint64_t byte of the payload is filled and it only contains the value for the custom_mode which is the first part of the mavlink_heartbeat_t structure. However the remainder of the mavlink_heartbeat_t message is not available.

The mavlink_heartbeat_message is 9 bytes long. It seems Mavlink uses memcpy to copy only 9 bytes to the unit64_t array, therefore, pretty much all of the mavlink_heartbeat_t message should be in that first item of the payload, but it isnt.

I thought it may have been due to the operating system (ubuntu 64 bit x86) allocating 64 bits for each entry in the structure regardless of size, but when copying  9 x 8 bytes in the memcpy, the result doesn't change and I cannot see any of the other data of the mavlink_heartbeat_t in subsequent memory locations via the ROS_INFO outputs shown above.

I'm sure someone has got this to work, but rightnow, I cannot see any data in the payload that means anything except custom_mode, and based on the code, I would have expected it to be bit shifted by the rest of the structure during the memcpy. I've also tried the non-aligned bit of code included but that doesn't make a difference.

If you've gotten the mavlink_hearbeat_pack() to work I'd be interested to find out what I'm doing wrong.

cheers

Peter

[Peter Milani]

Actually don't worry about this. I've decoded it at the other end of the ROS subscriber, and it decodes correctly, therefore I assume that it is being transmitted as planned. Only thing is that Mission planner cannot connect to me via the heartbeat messages I am sending.

[Vladimir Ermakov]

Hi Reter,

You can get my code as example: https://github.com/vooon/mavros/blob/master/mavros/src/plugins/sys_status.cpp#L569
Also seems that your copy might lose last payload64 item, my copy funcs: https://github.com/vooon/mavros/blob/master/mavros/include/mavros/utils.h#L90

вторник, 16 сентября 2014 г., 4:28:50 UTC+4 пользователь Peter Milani написал: