Hello.
I want to add custom mavlink message to ArduCopter firmware with my ErleBrain2.
I’ve added this code to ardupilotmega.xml in modules/mavlink/message_definitions/
<message id="239" name="MYDEBUG">
<description>Send a debug value. The index is used to discriminate between values. These values show up in the plot of QGroundControl as DEBUG N.</description>
<field type="uint32_t" name="time_boot_ms">Timestamp (milliseconds since system boot)</field>
<field type="uint8_t" name="ind">index of debug variable</field>
<field type="float" name="value">DEBUG value</field>
</message>
and added usage code near raw_sensors stream to test original debug and “mydebug” ones.
mavlink_msg_mydebug_send(chan,AP_HAL::micros(), 1, 100);
mavlink_msg_debug_send(chan,AP_HAL::micros(), 1, 100);
then compile code and upload it to ErleBrain2.
I have python code to test this:
master = mavutil.mavlink_connection('10.0.0.2:6000')
while True:
print 'Waiting for DEBUG'
print master.recv_match(type='DEBUG', blocking=True)
print 'Waiting for MYDEBUG'
print master.recv_match(type='MYDEBUG', blocking=True)Also i have C code to emulate copter (modified example from github.com/mavlink/mavlink/blob … link_udp.c):
mavlink_msg_debug_test_pack(1, 200, &msg, microsSinceEpoch(), 100, 200);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));
mavlink_msg_debug_pack(1, 200, &msg, microsSinceEpoch(), 100, 200);
len = mavlink_msg_to_send_buffer(buf, &msg);
bytes_sent = sendto(sock, buf, len, 0, (struct sockaddr*)&gcAddr, sizeof(struct sockaddr_in));With mavlink_udp.c - all works fine. But with ArduCopter.elf on my erlebrain2 i have problems.
For original debug - all works fine. But with custom message - not.
Seems that it does not sends from firmware at all.
Also, if i try to change id or name of original debug - it’s not working.
Can someone give me hint to solve this?