I am making some custom mavlink messages for some sensors that I have, I have successfully created the mavlink message and I can see it in QGC working fine, however I can not seem to get it working for multiple instances (I only get 1 sensor showing values, the others do not have messages.)
Below is the streams hpp file code, let me know if there is any other code that is relevant. I’ve followed the example in the guides and PX4 does build so I’m sceptical that this specific code is the issue and think it might be coming from somewhere else, but i’m not sure what else is involved.
Also just note, I am new to this so if something looks a bit odd or you think another way would be better please let me know and explain too
Hi @owen487 with multiple instances did you mean MAV_0 MAV_1 etc. ?
So as I understand you can see your custom mavlink message in MAV_0 instance but it is not exist in others is this true?
Hi @Alp_Uca , thanks for the reply, I think I might have used a poor choice of words here to describe what I mean. I don’t think what I’m talking about is to do with MAV_0 MAV_1 etc.
I have multiple sensors and am trying to make it so that all of them provide me with a mavlink message with their relevant data so that I can look at them all in mavlink inspector. At the moment, I can get 1 sensor to send me data and that is it, the other sensors do not have any mavlink messages associated with them.
Let me know if you’re still unsure about what I mean and I can try explain again:)
Thanks
You defined seperate mavlink messages in xml dialect for each sensor you have. And you can getting one of them but others not streaming although you did same things for each right ?
Sort of… when I say I have multiple sensors I mean multiple of the exact same sensor, so I have only created 1 xml message which i’ll attach below. Its a custom dialect that I have included in common.xml
<?xml version="1.0"?>
<mavlink>
<dialect>3</dialect>
<messages>
<message id="11517" name="IMU_SENSOR_STREAM">
<description>IMU live data.</description>
<field type="uint32_t" name="time_boot_ms" units="ms">Timestamp from boot</field>
<field type="int16_t" name="Xaxis"> X axis data </field>
<field type="int16_t" name="Yaxis"> Y axis data </field>
<field type="int16_t" name="Zaxis"> Z axis data </field>
</message>
</messages>
</mavlink>
It works for 1 sensor yes, but if I want multiple do I need to keep creating more and more xml definitions and hpp files? And how would I be able to distinguish between the different sensors as there is nothing unique between them other than being listed in different instances in the uORB topics.
My understanding is that you could have a mavlink message that is subscribed to a “uORB::SubscriptionMultiArray” which would give multiple instances of the same uORB topic, which I have in my case, and you could send all of them in mavlink messages. - but I could be wrong, what do you think?
I understand. Yes you are right. Actually you can examine some of mavlink streams which are using this method. For example DISTANCE_SENSOR stream like here.
But you should also publish your uORB topic with uORB::PublicationMulti which is contain instances of your sensors. So when you try “listener sensor_attitude” in QGround console you should see your instances under seperate titles.
For example when I use two lidar sensor same time it’s looking like this
instance 0
…
… first lidar measurements
instance 1
…
… second lidar measurements
To sum up, first be sure that you can publishing your uORB topic with instances like this. After that you can subscribe to this multi instance uORB topic from your mavlink stream code and use instances. I did not try to do but I guess it should be like this.
Hi @Alp_Ucan , I had a check and and I do get separate instances for each sensor from my uORB topic, its setup with uORB::PublicationMulti .
I also tried re-formatting my code to the same style as the distance sensor for the hpp file and it still isnt quite working. My mavlink message still comes through, however it squashes the values together rather than giving separate values. I’ll attach a picture of the data plot to show what I mean.
The first half has 2 sensors plugged in, and you can see the step changes between the 2 sensors values, the second half I unplugged one of the sensors and it resumes to the single stream of data as expected.
Also here is my remodelled code
#ifndef IMU_SENSOR_STREAM_HPP
#define IMU_SENSOR_STREAM_HPP
#include <uORB/topics/sensor_attitude.h>
class MavlinkStreamImuSensorStream : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamImuSensorStream(mavlink);
}
const char *get_name() const
{
return MavlinkStreamImuSensorStream::get_name_static();
}
static const char *get_name_static()
{
return "IMU_SENSOR_STREAM";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_IMU_SENSOR_STREAM;
}
uint16_t get_id()
{
return get_id_static();
}
unsigned get_size()
{
return _sensor_att_subs.advertised_count() * (MAVLINK_MSG_ID_IMU_SENSOR_STREAM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
}
private:
explicit MavlinkStreamImuSensorStream(Mavlink *mavlink) : MavlinkStream(mavlink) {}
//Subscription to the sensor data
uORB::SubscriptionMultiArray<sensor_attitude_s> _sensor_att_subs{ORB_ID::sensor_attitude};
/* do not allow top copying this class - not sure if needed, come back and check*/
MavlinkStreamImuSensorStream(MavlinkStreamImuSensorStream &);
MavlinkStreamImuSensorStream& operator = (const MavlinkStreamImuSensorStream &);
bool send() override
{
bool updated = false;
for (int i = 0; i < _sensor_att_subs.size(); i++) {
sensor_attitude_s _sensor_att;
if (_sensor_att_subs[i].update(&_sensor_att)) {
mavlink_imu_sensor_stream_t msg{};
msg.time_boot_ms = _sensor_att.timestamp / 1000;
// Looking at the cpp file for BNO055 gives these names
msg.Xaxis = _sensor_att.pitch_deg;
msg.Yaxis = _sensor_att.roll_deg;
msg.Zaxis = _sensor_att.heading_deg;
msg.ID = _sensor_att.device_id;
mavlink_msg_imu_sensor_stream_send_struct(_mavlink->get_channel(), &msg);
updated = true;
}
}
return updated;
}
};
#endif /* IMU_SENSOR_STREAM */
Actually when I check streamer it is looking like the expected result is this. I looked other streams which like this there is no visualization of different instances in mavlink inspector tab. When you try “listener sensor_attitude” in console tab can you see instances like this ?
Yes when I type “listener sensor_attitude” I get multiple instances.
I think for now I will just have to create a new message for each one. The trouble is I need to have 20+ sensors in total on my project so its a lot of extra bits to add, and adding lots of things to the definition will make quite an unclean list (plus I don’t know how I would differentiate between all the instances in the message anyway).
@owen487 The way you do this is to have an instance field in your message, as in the BATTERY_STATUS message. This gets iterated for each IMU in your case, and should start from 1 if at all possible
<message id="147" name="BATTERY_STATUS">
<description>Battery information. Updates GCS with flight controller battery status. Smart batteries also use this message, but may additionally send BATTERY_INFO.</description>
<field type="uint8_t" name="id" instance="true">Battery ID</field>
<field type="uint8_t" name="battery_function" enum="MAV_BATTERY_FUNCTION">Function of the battery</field>
...
Your streaming code will need to emit the message for each sensor, updating the instance number appropriately depending on what uorb topic is updated.
Hi @hamishwillee , thanks for your reply, I noticed this the other day and wasn’t sure if it was needed , you’ve confirmed it was important! However it hasn’t seemed to solve my issue, I think I must be misunderstanding how the code works for streaming and how it updates through the different instances.
Are you able to explain a little for me on how/ what I need to do or what I’m doing wrong, I’ve changed the code slightly so I’ll put it below.
#ifndef IMU_SENSOR_STREAM_HPP
#define IMU_SENSOR_STREAM_HPP
#include <uORB/topics/sensor_attitude.h>
class MavlinkStreamImuSensorStream : public MavlinkStream
{
public:
static MavlinkStream *new_instance(Mavlink *mavlink)
{
return new MavlinkStreamImuSensorStream(mavlink);
}
const char *get_name() const
{
return MavlinkStreamImuSensorStream::get_name_static();
}
static const char *get_name_static()
{
return "IMU_SENSOR_STREAM";
}
static uint16_t get_id_static()
{
return MAVLINK_MSG_ID_IMU_SENSOR_STREAM;
}
uint16_t get_id()
{
return get_id_static();
}
unsigned get_size()
{
return _sensor_att_subs.advertised_count() * (MAVLINK_MSG_ID_IMU_SENSOR_STREAM_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES);
}
private:
explicit MavlinkStreamImuSensorStream(Mavlink *mavlink) : MavlinkStream(mavlink) {}
//Subscription to the sensor data
uORB::SubscriptionMultiArray<sensor_attitude_s, sensor_attitude_s::MAX_INSTANCES> _sensor_att_subs{ORB_ID::sensor_attitude};
/* do not allow top copying this class - not sure if needed, come back and check
MavlinkStreamImuSensorStream(MavlinkStreamImuSensorStream &);
MavlinkStreamImuSensorStream& operator = (const MavlinkStreamImuSensorStream &); */
bool send() override
{
bool updated = false;
sensor_attitude_s _sensor_att;
for (auto &_sensor_att_sub : _sensor_att_subs) {
if (_sensor_att_sub.update(&_sensor_att)) {
mavlink_imu_sensor_stream_t msg{};
msg.ID = _sensor_att.device_id;
msg.time_boot_ms = _sensor_att.timestamp / 1000;
// Looking at the cpp file for BNO055 gives these names
msg.Xaxis = _sensor_att.pitch_deg;
msg.Yaxis = _sensor_att.roll_deg;
msg.Zaxis = _sensor_att.heading_deg;
mavlink_msg_imu_sensor_stream_send_struct(_mavlink->get_channel(), &msg);
updated = true;
}
}
return updated;
}
};
#endif /* IMU_SENSOR_STREAM */
And in case you want to see
<?xml version="1.0"?>
<mavlink>
<dialect>3</dialect>
<messages>
<message id="11517" name="IMU_SENSOR_STREAM">
<description>IMU live data.</description>
<field type="uint32_t" name="time_boot_ms" units="ms">Timestamp from boot</field>
<field type="int16_t" name="Xaxis"> X axis data </field>
<field type="int16_t" name="Yaxis"> Y axis data </field>
<field type="int16_t" name="Zaxis"> Z axis data </field>
<field type="uint8_t" name="ID" instance="true"> go through instances </field>
</message>
</messages>
</mavlink>
I have read the docs quite extensively and tried to understand it as best I can but it does leave a little to be desired particularly for a beginner like me so any further explaining would be amazing
The code looks good to me, but I am not an expert on code review. You have certainly got the right “shape” - a multi-subscription, which you iterate through and update only on change, then writing the change topic to the mavlink message.
This would not be the problem, but remember I suggested that you iterate from 1 - this iterates from 0.
Is it compiling? How do you know it isn’t working? If it is compiling you’re at where I would be madly logging/printfing to see what is going on.
I will make the iteration change, simply didn’t have time to add before hand.
It does compile, and I can see its not working from the values being displayed and the graphs that get plotted. See graph I showed in an earlier post
I also tried re-formatting my code to the same style as the distance sensor for the hpp file and it still isnt quite working. My mavlink message still comes through, however it squashes the values together rather than giving separate values. I’ll attach a picture of the data plot to show what I mean.
The graph shows having 2 IMUs plugged in for the first half, then 1 for the second half, to start the values are jumping between the different values of the 2 IMUs, then smooth when only 1 is used. I can also confirm its not working just by the fact I only have one “displayed” set of values.
When you say logging, what would you actually be logging to help diagnose? As I said I’m a bit of a noob so I’m not entirely sure where to start for that.
When I eventually do figure this out, if its appropriate, I will definitely try and contribute something to the guides to potentially “clear it up a bit”
Yeah, so clearly not working in your plot at least - is this QGroundControl output?
When you look at MAVLink inspector and plot the instance/id, do you get just one instance value or both? Because if you are getting messages with the two different instance values then your code is most likely correct, and the problem is that QGC is not differentiating them. Does that make sense?
@DonLakeFlyer QGC is aware of MAVLink instances right? I.e. it will separately plot a trace for each instance by default? - say for a battery with an id of 0 and 1?
Yep this is QGC output. And yes plotting the instance/ID has the same effect, it is constantly bouncing between the 2 ID values. The inspector also flickers between the sensor data in the values it displays so I suppose that does reinforce that it may be a QGC issue. Makes sense to me.
Is this something that I might be able to confirm by using some other “mavlink inspection” software, wireshark for example? I haven’t had a huge amount of success with them in the past but I could give it another go whilst we wait for the QGC devs response if it helps point to things?
@owen487 If it keeps bouncing between the two id values that you have for your imu instances then your PX4 code IS correct.
What that means is that QGC is not instance-aware in MAVLink inspector, at least by default. It is very likely it just hard codes demultiplexing specific messages it knows about when needed - such as batteries.
@owen487 Mission Planner supports plotting instances, but QGC does not.
You can’t use Mission Planner generally for flying PX4, but you should be able to connect to it and use it to verify this particular behaviour - though of course you may need to rebuild Mission Planner with your messages, and I have no idea how to do that.
Note that I don’t think you need to do this to know that your PX4-side code is OK, but it will make inspection easier.
