Guides I used
How to recieve custom mavlink messages from PX4 module using pymavlink
I followed the above guide and changed a lot of code. I used the mavlink-debug module as a guide on how everything is connected but for some reason I cannot recieve the custom message. This is my pymavlink code
import time
import sys
import argparse
parser = argparse.ArgumentParser()
parser.add_argument('--name', type=str, required=False)
args = parser.parse_args()
# Import mavutil
from pymavlink import mavutil
mavutil.set_dialect("common")
# Create the connection
master = mavutil.mavlink_connection('udpin:localhost:14445')
# Wait a heartbeat before sending commands
master.wait_heartbeat()
# Request all parameters
master.mav.param_request_list_send(
master.target_system, master.target_component
)
while True:
time.sleep(0.01)
try:
if args.name:
message = master.recv_match(type=args.name, blocking=True)
else:
message = master.recv_match( blocking=True)
print(f'name: {message}')
except Exception as error:
print(error)
From Px4 this is what I did
- I created a new module with a new
.msg
file - I updated the common.xml with the new message which was similar to the
.msg
file like so
<message id="254" name="J50_MSG">
<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="timestamp" units="ms">Timestamp (time since system boot).</field>
<field type="uint8_t" name="test_bool">index of debug variable</field>
<field type="float" name="test_float">DEBUG value</field>
</message>
- I used mavgenerate, targetting the updated file and replaced the older files with the new header
- I updated the
mavlink_messages.cpp
file with the following
#ifndef J50_MSG_HPP
#define J50_MSG_HPP
#include <uORB/topics/j50_msg.h>
class MavlinkStreamJ50Msg : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink) { return new MavlinkStreamJ50Msg(mavlink); }
static constexpr const char *get_name_static() { return "J50_MSG"; }
static constexpr uint16_t get_id_static() { return MAVLINK_MSG_ID_J50_MSG; }
const char *get_name() const override { return get_name_static(); }
uint16_t get_id() override { return get_id_static(); }
unsigned get_size() override
{
return _j50_msg_sub.advertised() ? MAVLINK_MSG_ID_J50_MSG_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES : 0;
}
private:
explicit MavlinkStreamJ50Msg(Mavlink *mavlink) : MavlinkStream(mavlink) {}
uORB::Subscription _j50_msg_sub{ORB_ID(j50_msg)};
bool send() override
{
j50_msg_s debug;
if (_j50_msg_sub.update(&debug)) {
mavlink_j50_msg_t msg{};
msg.timestamp = debug.timestamp;
msg.test_float = debug.test_float;
msg.test_bool = debug.test_bool;
mavlink_msg_j50_msg_send_struct(_mavlink->get_channel(), &msg);
PX4_WARN("uorb => mavlink - message was sent !!!!");
return true;
}
return false;
}
};
- I compiled it and ran
make px4_sitl_default jmavsim
on a windows pc - I called the module which updated the uORB data which in return printed
uorb => mavlink - message was sent !!!!
to the console - I used the new xml files for pymavlink and targetted them using
mavutil.set_dialect("common")
output
I can see data printed like
name: ATTITUDE_QUATERNION {time_boot_ms : 297264, q1 : 0.9999986886978149, q2 : -0.000639534555375576, q3 : -0.00021637501777149737, q4 : 0.0014910579193383455, rollspeed : -0.004011197946965694, pitchspeed : -0.001532914349809289, yawspeed : 0.005574052222073078, repr_offset_q : [0.0, 0.0, 0.0, 0.0]}
but none of them are the new custom data I have in the common.xml for both PX4 and pymavlink. Have I missed anything?