C++ MAVLink Set_Position_Target_Local_NED Coordinate Frame Issue

Ground Control Software MAVLink
,
1

Hello all,

So, I’ve coded up an extremely simple C++ MAVLink command emitter, that sends heartbeats and command_long messages over UDP (driving a gimbal from a custom Windows-based ground element.) The receiving end is a Raspberry Pi Zero, running Ser2Net to bridge the hardware serial port to the UDP socket. I’ve got an Orange Cube, running Arducopter 4.3.3 (admittedly, a little out-of-date). So far, this all works quite well, the AP responds as expected, and connecting via Mission Planner over USB, I can see the expected traffic on the MAVLink Inspector.

However, I’m trying to add Set_Position_Target_Local_NED commands, and this has gone extremely poorly - the messages wouldn’t show up in MAVLink Inspector, but doing some packet sniffing on the RPi serial port, and manually decoding the resulting hex dumps via the Mission Planner utility, and correlating them back to different inputs, I found that if the coordinate frame parameter is non-zero, the packet will consistently fail to decode (“NULL”), whereas an otherwise-invalid coordinate frame value of 0 will always produce readable hex dumps (and the messages appear in MAVLink Inspector). Interestingly, I’ve also found this is the case for Set_Position_Target_Global_Int.

I feel like I’m missing something absurdly obvious - any suggestions would be greatly appreciated. I’ve attached the Set_Position and UDP sender functions below.

Thanks!

Regards,
-Luke

int sendSetPositionLocalVelocity(float vx, float vy, float vz, float yaw_rate)
{
int bytes_sent;
mavlink_message_t mavlink_message_vel;

mavlink_set_position_target_local_ned_t set_vel_local = { 0 };
set_vel_local.time_boot_ms = currentTime();
set_vel_local.target_system = 0;
//Pack the message
set_vel_local.target_component = 0;
set_vel_local.coordinate_frame = 9; //Also tried MAV_FRAME_BODY_OFFSET_NED
set_vel_local.type_mask = 1497; //Velocity + yaw rate (see below)
set_vel_local.x = 0;
set_vel_local.y = 0;
set_vel_local.z = 0;
set_vel_local.vx = vx;
set_vel_local.vy = vy;
set_vel_local.vz = vz;
set_vel_local.afx = 0;
set_vel_local.afy = 0;
set_vel_local.afz = 0;
set_vel_local.yaw = 0;
set_vel_local.yaw_rate = yaw_rate;

//Bitmask (0 = do not ignore, 1 = ignore)
//bit12:yaw rate, bit11:yaw, bit10: FZ vs AccZ, bit9:AccZ, bit8:AccY, bit7:AccX, bit6:VelZ, bit5:VelY, bit4:VelX, bit3:PosZ, bit2:PosY, bit1:PosX
//For velocity control with yaw rate: 010111000111 → 1479

mavlink_msg_set_position_target_local_ned_encode(SOURCE_SYSID, SOURCE_COMPID, &mavlink_message_vel, &set_vel_local); //Encode message for transmission
bytes_sent = sendUDP(mavlink_message_vel); //Send over UDP socket

return bytes_sent;
}

int sendUDP(const mavlink_message_t& message)
{
char mavlink_buffer[300]; //Buffer to send
unsigned length; //Length of message in the buffer
int length_sent; //Length of message actually sent

length = mavlink_msg_to_send_buffer((uint8_t*)mavlink_buffer, &message);
//Convert to character buffer for serial transmission
length_sent = sendto(socket_udp, mavlink_buffer, length, 0, (struct sockaddr*)&udp_socket_add, sizeof(udp_socket_add));
//Send the message over the socket, to the specified address

return length_sent; //Return the message length sent
}

2

Just for reference, I picked through the hex dumps on the hardware serial port, and found the problem (though not the source): I’m losing one of the CRC bytes when I set the coordinate frame to a non-zero number:

For instance, here’s with a 0:
FD 34 00 00 00 FE 01 54 00 00 E8 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80 3F 00 00 00 40 00 00 40 40 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80 40 D9 05 02 01 24 2E
The payload length is 0x34 (52 bytes), which it has.

Here’s with a 9:
FD 35 00 00 00 FE 01 54 00 00 E8 03 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80 3F 00 00 00 40 00 00 40 40 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 00 80 40 D9 05 02 01 09 1D
The payload length is 0x35 (53 bytes), which it has, but that leaves only one byte for the CRC.

I’m picking through my code now - looks like I send the correct 65 bytes (a local pre-send hex-dump produces valid messages), but only 64 bytes come out of the other end of the UDP link (observing with netcat and hexdump), with the last byte missing. (In fact, even if I just try to send 65 bytes with a stand-alone UDP utility directly to netcat, only 64 bytes show up… This is over a wired ethernet connection, too.)

On closer inspection, this doesn’t seem to be the case - with proper netcat and hexdump usage, I receive the complete 65-byte sequence.

I did try MAVP2P, and it also truncated the last character, just like Ser2Net, so no easy fix there.

3

Just by way of closing this out (and hopefully helping somebody else out down the road) - after a lot of poking, I conclusively determined that the point of failure was the UDP to serial bridge - up until that point, the messages were making it through intact. I replacing both Ser2Net and MAVP2P with a Netcat line
( nc -u -l 14300 < /dev/ttyAMA0 > /dev/ttyAMA0) - it’s a bidirectional serial-UDP bridge, with no buffering, no MAVLink layers - it isn’t great, but it also doesn’t truncate 65-byte strings down to 64 bytes, and it works with Mission Planner, so for now, it’ll be OK.

Regards,
-Luke

1 Like