Read custom MAVLink topic

1

Hi,

Is it possible to look up if something is published to a certain MAVLink topic? I am publishing a message via mavros to a custom MAVLink topic and try to read it with PX4. I am not sure if mavros does not publish the topic or PX4 does not read it.

Best Nils

2

Did you write the code to handle it in PX4? I would start on that side and add some basic debugging.

3

Hi @dagar,

I just followed this guide: https://dev.px4.io/en/middleware/mavlink.html

I created two messages in uORB, one to send data to my custom MAVLink topic and one to receive data from it. When I try to compilate it it works just fine. But I receive nothing from the MAVLink topic. Do I have to announce it somewhere that I use a custom MAVLink topic too?

Best
Nils

1 Like
4

Are you talking about within PX4, ROS, or QGC? Where are you filling the mavlink message and sending?

5

I am also having some trouble with a custom MAVLink message.
I’m trying to send a second message using TELEM2 on a PixRacer (so px4fmu-v4). As practice I just decided to use the ca_trajectory help there.
I can compile and upload just fine using
make px4fmu_default upload

What I’ve been trying to figure out for days is that I added the following to my rcS file:
if param compare SYS_COMPANION 1499
then
mavlink start -d ${MAVLINK_COMPANION_DEVICE} -b 57600 -s CA_TRAJECTORY
fi

What do all of these options mean? I’m guessing -d is device, -b is baudrate, what is -s? I can’t seem to find where the code that parses that command is.

I realized after writing this that the SYS_COMPANION code was under an
if ver hwcmp PX4FMU_V1
statement. Probably a part of why my Arduino is seeing nothing when connected to the TELEM2 port. Nonetheless I’m curious what all of the options are for starting mavlink. Is there a problem with having two “mavlink start” in the rcS code?

6

@dagar

I tried it within PX4. I added two different topics (gazebo_pos.msg and gazebo_pos_test.msg). In an example app I publish data to the gazebo_pos_test topic

send_pos.x = 1.0;
send_pos.y = 2.0;
send_pos.z = 3.0;
orb_publish(ORB_ID(gazebo_pos_test), gazebo_pos_pub, &send_pos);

In MAVlink messages I added therefore the following class in mavlink_messages.cpp

class MavlinkStreamGazeboPos : public MavlinkStream
{
public:
  const char *get_name() const
  {
	return MavlinkStreamGazeboPos::get_name_static();
  }

  static const char *get_name_static()
  {
	return "GAZEBO_POS";
  }

  static uint16_t get_id_static()
  {
	return MAVLINK_MSG_ID_GAZEBO_POS;
  }

  uint16_t get_id()
  {
	return get_id_static();
  }

  static MavlinkStream *new_instance(Mavlink *mavlink)
  {
	return new MavlinkStreamGazeboPos(mavlink);
  }

  unsigned get_size()
  {
	return MAVLINK_MSG_ID_GAZEBO_POS_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
  }

private:
MavlinkOrbSubscription *_gazebo_sub;
uint64_t _gazebo_time;

// do not allow top copying this class
MavlinkStreamGazeboPos(MavlinkStreamGazeboPos &);
MavlinkStreamGazeboPos &operator = (const MavlinkStreamGazeboPos &);

protected:
explicit MavlinkStreamGazeboPos(Mavlink *mavlink) : MavlinkStream(mavlink),
	_gazebo_sub(_mavlink->add_orb_subscription(ORB_ID(gazebo_pos_test))),
	_gazebo_time(0)
{}

void send(const hrt_abstime t)
{
	struct gazebo_pos_test_s gazebo;

	if (_gazebo_sub->update(&_gazebo_time, &gazebo)) {
		mavlink_gazebo_pos_t msg = {};

		msg.time_usec = gazebo.timestamp;
		msg.qw = gazebo.qw;
		msg.qx = gazebo.qx;
		msg.qy = gazebo.qy;
		msg.qz = gazebo.qz;
		msg.x = gazebo.x;
		msg.y = gazebo.y;
		msg.z = gazebo.z;

		mavlink_msg_gazebo_pos_send_struct(_mavlink->get_channel(), &msg);
	}
}
};

In addition I added in mavlink_receiver.cpp the follwoing function in order to receive the data from the MAVLink topic and put it into the gazebo_pos uORB topic

void
MavlinkReceiver::handle_message_gazebo_pos(mavlink_message_t *msg)
{
mavlink_gazebo_pos_t gazebo;
mavlink_msg_gazebo_pos_decode(msg, &gazebo);

struct gazebo_pos_s gazebo_pos = {};

//att_pos_mocap.timestamp = sync_stamp(mocap.time_usec);
//att_pos_mocap.timestamp_received = hrt_absolute_time();

gazebo_pos.qw = gazebo.qw;
gazebo_pos.qx = gazebo.qx;
gazebo_pos.qy = gazebo.qy;
gazebo_pos.qz = gazebo.qz;

gazebo_pos.x = gazebo.x;
gazebo_pos.y = gazebo.y;
gazebo_pos.z = gazebo.z;

if (_gazebo_pos_pub == nullptr) {
	_gazebo_pos_pub = orb_advertise(ORB_ID(gazebo_pos), &gazebo_pos);

} else {
	orb_publish(ORB_ID(gazebo_pos), _gazebo_pos_pub, &gazebo_pos);
}
}

In my example app I subscribe to this uORB topic (gazebo_pos). But when I print the receiving data I dont get the right values for for x,y,z (1.0,2.0,3.0). Instead I get for y = 0 and fpr x and z really high numbers such as 5468798798.
Do you know what I am doing wrong?

Best
Nils

7

Hi all & Nils,

I’m trying to do the same. I am able to read all messages coming from the px4 -> mavlink -> mavros -> ros. But i am not able to send my custom message to px4. Almost the same problem like you, but with the only difference, that I don’t need a separate uORB msg. I tried to send some set_actuator_control messages to px4 ( from ros->mavros->mavlink->px4) and they worked fine for me, after offboard configuration was done. The code for my message is almost the same.

Are you now able to read mavlink messages?

Cheers,
Danu

8

HI danusanth,

Have you sovlved your problem? We are able to read messages comming from ros -> mavros ->mavlink ->px4, but not for the inverse way. Could you please list all steps that you have done for reading messages coming from the px4 -> mavlink -> mavros -> ros.

Thanks in advanced!

Lam Hung