Issue with custom message

Hi!

I have an issue with sending a custom mavlink message and receiving it in QGroundControl.

I have been following this and this.

Here is what I did so far:

  1. Declare ca_trajectory.msg:
uint64 timestamp
uint64 time_start_usec
uint64 time_stop_usec
uint32 coefficients
uint16 seq_id

#TOPICS ca_trajectory
  1. Add the message to common.xml
    <message id="401" name="CA_TRAJECTORY">
      <description>This message encodes all of the raw rudder sensor data from the USV.</description>
      <field type="uint64_t" name="timestamp">Timestamp in milliseconds since system boot</field>
      <field type="uint64_t" name="time_start_usec">start time, unit usec.</field>
      <field type="uint64_t" name="time_stop_usec">stop time, unit usec.</field>
      <field type="uint32_t" name="coefficients">as it says.</field>
      <field type="uint16_t" name="seq_id">can not cheat any more.</field>
    </message>
  1. Add a class etc in mavlink_messages.cpp:
#include <uORB/topics/ca_trajectory.h>
...
class MavlinkStreamCaTrajectory : public MavlinkStream
{
public:
    const char *get_name() const
    {
        return MavlinkStreamCaTrajectory::get_name_static();
    }

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

    static uint16_t get_id_static()
    {
        return MAVLINK_MSG_ID_CA_TRAJECTORY;
    }

    uint16_t get_id()
    {
        return get_id_static();
    }

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

    unsigned get_size()
    {
        return MAVLINK_MSG_ID_CA_TRAJECTORY_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
    }

private:
    MavlinkOrbSubscription *_sub;
    uint64_t _ca_traj_time;

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

protected:
    explicit MavlinkStreamCaTrajectory(Mavlink *mavlink) : MavlinkStream(mavlink),
        _sub(_mavlink->add_orb_subscription(ORB_ID(ca_trajectory))),  // make sure you enter the name of your uorb topic here
        _ca_traj_time(0)
    {}

    bool send(const hrt_abstime t)
    {
        struct ca_trajectory_s _ca_trajectory;    //make sure ca_trajectory_s is the definition of your uorb topic

        if (_sub->update(&_ca_traj_time, &_ca_trajectory)) {

            mavlink_ca_trajectory_t msg;//make sure mavlink_ca_trajectory_t is the definition of your custom mavlink message 

            msg.timestamp = _ca_trajectory.timestamp;
            msg.time_start_usec = _ca_trajectory.time_start_usec;
            msg.time_stop_usec  = _ca_trajectory.time_stop_usec;
            msg.coefficients =_ca_trajectory.coefficients;
            msg.seq_id = _ca_trajectory.seq_id;

            mavlink_msg_ca_trajectory_send_struct(_mavlink->get_channel(), &msg);
        }
				return true;
    }
};

// Added A StreamListItem too:
...	
StreamListItem(&MavlinkStreamCaTrajectory::new_instance, &MavlinkStreamCaTrajectory::get_name_static, &MavlinkStreamCaTrajectory::get_id_static),

  1. Enabled stream in mavlink_main.cpp:
configure_stream_local("CA_TRAJECTORY", 5.0f);
  1. Created a module based on module template with the following run function:
void CaTrajectory::run()
{
	while (!should_exit()) {
			msg.timestamp = hrt_absolute_time();
			msg.seq_id =  123;
			msg.time_start_usec = 12;
			msg.time_stop_usec = 2311;

			orb_publish(ORB_ID(ca_trajectory), _ca_pub, &msg);
	}
}

I made sure that the application is starting correctly. Here are some further observations:

  • in nsh shell I can see the message definitions by doing ls /obj and also uorb top shows that uorb message is published.
  • I placed some debug variables in the send function in my MavlinkStreamCaTrajectory:send method and send them over on the debug topic (I could successfully see them in QGroundControl mavlink inspector.
  • I used the same headers in QGroundControl as in mavlink (created by running python -m pymavlink.tools.mavgen --lang=C --wire-protocol=2.0 --output=. message_definitions/standard.xml in Firmware/mavlink/include/mavlink/v2.0 (used the same generated headers in QGroundControl.
  • My new messages are not visible in QGroundControl (checked mavlink inspector and analyze widget).
  • I placed a breakpoint in Vehicle.CC in QGC in void Vehicle::_mavlinkMessageReceived(LinkInterface* link, mavlink_message_t message) with my new message ID (MAVLINK_MSG_ID_CA_TRAJECTORY), however it is never hit

Anyone has any idea what else I might be missing?

Stupid question: in which mavlink mode have you put configure_stream_local("CA_TRAJECTORY", 5.0f); ?

Throughout the testing I set it to all of the below:

  • MAVLINK_MODE_NORMAL
  • MAVLINK_MODE_ONBOARD
  • MAVLINK_MODE_EXTVISION
  • MAVLINK_MODE_OSD
  • MAVLINK_MODE_CONFIG
  • MAVLINK_MODE_MINIMAL

I assume having them configured at the same time shouldn’t be an issue. Is it possible that I missed anything here?

MAVLINK_MODE_NORMAL is the stream to QGC, so it seems correct.
I would check that the generated headers are used both in the Firmware and QGC, that you’re using mavlink v2 in both, and try with clean builds.

I’m quite sure that the generated headers are imported correctly. I basically forked the “c_library_v2” repostiory where I added the message and then I updated the submodules in both qgroundcontrol and PX4 Firmware to point to that.

The reason I think they are imported correctly is that when I use the generated id “MAVLINK_MSG_ID_CA_TRAJECTORY” in both QGroundControl and Px4 and everything compiles successfully. I did a full clean and nothing changed.

Would you have some advice how else I can test it?

Mavlink performs CRC checking and messages are discarded if they don’t match. This is usually the most common problem and it’s also usually hard to debug. So usually the single components build fine but do not work together.
You could try to check if the mavlink message is actually received on the qgc side but discarded during parsing.

OMG that was it! Thanks for pointing it out!

So the issue was that I changed the common message definitions and only exported the standard.xml message definitions. Apparently QGroundControl uses ardupilotmega.xml definitions and therefeore the CRCs were bound not to match…

Now everything seems to work correctly.

Glad to hear it’s working now