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:
- Declare ca_trajectory.msg:
uint64 timestamp
uint64 time_start_usec
uint64 time_stop_usec
uint32 coefficients
uint16 seq_id
#TOPICS ca_trajectory
- 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>
- 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),
- Enabled stream in mavlink_main.cpp:
configure_stream_local("CA_TRAJECTORY", 5.0f);
- 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 alsouorb 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?