Home        Store        Learn        Blog

Adding a sensor to mavlink stream

BlueROV2 and ArduSub ArduSub
#1

Hello,

I’m currently trying to send an ultrasonic thickness gauge sensor reading as a Mavlink message through the companion computer up to the ground station (running QGC). The sensor outputs serial data which I’m parsing with Python into a float value with optional metadata, but I’m struggling to find where the companion computer sends its Mavlink messages from.

I’m imagining there’s some kind of event loop or similar that checks the existing sensors and reports their status to the ground station, but I haven’t been able to find that, and am thinking perhaps the companion computer just relays mavlink messages from the pixhawk without actually processing them or the sensor readings.

Is it possible for me to have a script sending mavlink messages without interfering with the existing setup, or is there an event loop I can add a sensor-reading thread to?

Also, I’m hoping that I’ll be able to just send a float message with the name of my sensor, and that QGC will pick that up and display it automatically. If that’s not the case, is there some convenient way to register an extra sensor in QGC?

#2

Looks like the companion computer just runs mavproxy.py with some input parameters. Likely no no parsing of mavlink messages, just straight handover from pixhawk to ground station.

Hopefully looking into how the ping sonar is added will yield some useful insights, in I dive.

#3

Well that was quick. Ping360 imaging sonar sets up its own port instead of transmitting via mavlink, which I suppose makes sense since it’s more like a camera than a single-value sensor.

Not sure where to look now, time for some more googling and forum searching.

#4

Looking at the ardusub pymavlink reference page there aren’t any instructions for sending a message directly from the companion computer to the top computer, but maybe I can send a sensor measurement to the autopilot (Pixhawk), which I believe should get automatically related back to the top computer as part of the normal logging.

Would be super useful as then I’m able to keep my sensor read+transmit script separate from the normal companion computer code.

#5

Also, I’m hoping that I’ll be able to just send a float message with the name of my sensor, and that QGC will pick that up and display it automatically. If that’s not the case, is there some convenient way to register an extra sensor in QGC?

Unfortunately, no. You will need to recompile QGC to add a sensor that isn’t already supported. If you want to go this route I recommend looking at the NAMED_VALUE_FLOAT message. https://github.com/mavlink/qgroundcontrol/blob/master/src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc#L217

Is it a hard requirement for the measurement to be displayed in QGC? It will be easier to print your value out in a terminal in another window.

#6

Thanks for the reply!

Yeah, I’d settled on named_value_float yesterday and put it in my script. Haven’t had a chance to test yet as I’m working from home at the moment.

Alright, if I’m recompiling do I just have to update _handleNamedValueFloat to include my custom name, and add it as a ‘Fact’ in APMSubmarineFactGroup (including in the header file), or is there specific UI stuff that would need changing too?

Mostly important for it to end up in the telemetry log so it’s with the rest of the data, but given we want to see the reading while we’re taking it (it’s a thickness gauge, so we need to see when it comes into contact with what we’re measuring and starts sending valid readings), it’d be preferable to have as part of the normal sensor value overlay as opposed to having a separate window just for that.

If we were to decide to have a terminal output instead, can I read the same port as QGC, or would it be necessary to get the companion computer to send every message to the QGC port plus a separate port for the extra sensor parsing script?

Lastly, I’m assuming I need to put in a meaningful timestamp for it to register at the correct time in the dive. My current idea is to recv_match on an arbitrary message just before it and then add the time delta from then to when submitting, but before I do that it’d be good to know if there’s an easier recommended way.

#7

Just saw in the “Run pyMavlink on the companion computer” example on the pymavlink page that they use int(time.time()), commented as # Unix time for the time specified in their ping. I’m confused about that since the mavlink common message set requires microseconds, but time.time() is in seconds.

If the ‘time since system start’ that’s supposed to be used in these messages is indeed for the companion computer, I’m assuming I should use int(time.time() * 1e9) in wait_conn, and int(time.time()) * 1e6 in my autopilot.mav.named_value_float_send (which specifies ms instead of us). Please correct me if I’m wrong, or otherwise fix the incorrect documentation (or let me know where I can raise an issue for it) :slight_smile:

#8

Hi @EliotInsight,

Thank you for pointing this!
We indeed used the wrong unit, we should be using microseconds over seconds for ping message.
Be free to send pull requests if you catch any problem in our documentation. I have already created one to fix this issue here: https://github.com/bluerobotics/ardusub-gitbook/pull/155, it should be merged soon.

Just a small correction, the scale for (nano)seconds is 1e9, (micro)seconds is 1e6 and (milli)seconds is 1e3.

For the QGC related question, maybe @williangalvani can help you.

#9

Hi @EliotInsight,

Check this commit in QGC. It adds support for the named_value_float “RollPitch”, which tells QGC if the user is currently controlling rotation or translation.

#10

Thanks for this. I’ve seen that it’s merged now - should that auto-update the website display of the ardusub-gitbook, or does that require some form of release/rebuild to occur? The docs page (html file) I was referring to seemingly hasn’t been updated.

Haha, I must have been tired, thanks.

Great, thanks @williangalvani - looks like I had everything except the json file update. Is there a reason rollPitchToggle wasn’t added as a Q_PROPERTY or getter method near the top of ArduSubFirmwarePlugin.h? All the other properties are there, and it was added in all the other places where the other properties are used/specified.

#11

Just noting here that the pymavlink complained when I used int(time.time() * 1e3) within my call to named_value_float_send claiming the number was too large. The error message specified that the variable the time is assigned to within pymavlink is time_boot_ms, so presumably it should be time since boot rather than since the epoch. Currently changed to int((time.time() - boot_time) * 1e3), with boot_time set as time.time() at the start of my script. Will confirm if this works once I’ve managed to get my qgc build working.

Apparently Qt needs ~50GB of space, so I’ll need to make the build on a computer with some more memory…

#12

Have built custom QGC on my laptop for now, and a spot appears correctly for the sensor I’m trying to connect, which is great (thanks @patrickelectric and @williangalvani for the help!). Unfortunately no reading is displaying yet (I’ve confirmed that the reading is being processed correctly by my script and can display it printing at regular intervals in a screen session), so I’ll have to try to see if the mavlink message is sending correctly.

The current process is

  1. run script on startup
  2. set time as boot_time
  3. connect to mavlink port 9000 (to the autopilot)
  4. wait_conn (ping and wait for response) with microseconds since boot_time
  5. parse next reading
  6. named_value_float_send with reading value and milliseconds since boot_time

I’m hoping that’s a valid process, but I’m unsure if the boot time should be determined in some other way, and I’m not entirely certain if the reading should be going via the autopilot or directly to the top computer instead.

#13

I don’t think mavproxy is forwarding it to the topside computer, experiment sending that straight to the topside computer (192.168.2.1:14550).

#14

Hi @williangalvani, thanks for the advice - I’ll give it a try on Monday when I’m back with the hardware.

I’m assuming that’s using mavutil.mavlink_connection(udpin:192.168.2.1:14550), since I want that address and port to accept messages, but if I’ve misunderstood that directionality I’ll try with udpout (ie if udpin doesn’t work) :slight_smile:

#15

Couldn’t make a connection (raised an exception) when I tried using udpin:192.168.2.1:14550. udpout:192.168.2.1:14550 connected but still didn’t come up with any value on QGC.

Having looked over the pymavlink examples page again, udpout seems like it’s the correct way to send data from the calling computer (the companion in this case) to the receiving computer (the top computer here), given

udpout : Outputs data to a specified address:port (client).

and the ‘Run pyMavlink on the companion computer’ example shows that it should be followed by wait_conn, so reasonably sure that that part of the process is set up as it’s supposed to be now. wait_conn completes successfully, but the measurement values (via named_value_float_send) still don’t seem to be getting through to QGC unfortunately. Not sure where else issues could be coming in - might have to try running pymavlink on the top computer and seeing if the messages are getting that far.

#16

Finally got back to working on this a bit more. Got pymavlink today and found that my sensor readings are coming through correctly as named value floats, but their ‘time since boot’ was much larger than the normal named value float messages (6.1x10^6 vs 6.5x10^5 ms). Will have to check next week if it’s a scaling issue, or an offset issue from my sensor reading script starting earlier than the normal telemetry, or the normal telemetry script restarting (or at least restarting its boot count) at some point while the sensor script continues.

Also not sure if that’d be causing no displaying, so it’s still possible I’m checking the wrong value in my custom QGC build or something. At least now I know the messages are getting to the top, and can debug a bit more with pymavlink :slight_smile:

#17

Hi @EliotInsight,

Thank you for reporting your progress!

You could use mavlogdump to check where your messages don’t match the ones sent by the ROV. I’d take a look at the sysID and CompIP fields as QGC could be using those.

#18

No worries - forums work best when later viewers of posts can refer to useful work and solutions that have already been gone through, including the mistakes that were made along the way. I’m hoping this will be a useful reference for others looking to do something similar in future, and am planning to make a summary once it’s successfully working :slight_smile:

Thanks! I wasn’t aware there were extra fields that don’t come through to pymavlink’s recv_match. I’ll make sure to check those if it’s still not working once I’ve sorted out the timing issue and double-checked the parameters I’ve put in QGC.

2 Likes
#19

Timing difference is now just the small difference between when the two scripts start, which may be problematic for accurate timing later but should at least still allow the readings to display because they’re quite close together. That didn’t solve the issue unfortunately, but one less major thing to be concerned about for now.

@williangalvani I’m not sure how I’d be able to use mavlogdump in this case. I got one of the .bin log files off the pixhawk, but when analysing it I couldn’t find any named_value_float messages at all (there were a bunch of supposedly invalid headers, and then a bunch of parameters with 3 or 4 letter names) - maybe I need to use verbose, although I think I tried that and it didn’t help. I also realised that the bin files won’t help me to tell the difference between pixhawk and companion mavlink messages anyway because my sensor script sends directly to the top computer, so my custom messages shouldn’t even appear in the log file.

The tlogs have both my custom and the normal named_value_float messages, but they seem to just have the same information as a live pymavlink connection (no extra sysID or compIP fields, or anything else, just the usual name, time_boot_ms, and value). I can’t remember if I tried reading the tlogs with verbose, so will do that tomorrow, but it seems like that’s unlikely to be the missing piece given the other things I’ve tried.

#20

Hi @EliotInsight

That is actually a Dataflash Log. It doesn’t have mavlink communication, so it is not very relevant to your use case.

This seems to work here:

from pymavlink import mavutil
master = mavutil.mavlink_connection('udpout:localhost:14550', source_system=1)
master.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_NOTICE, "Qgc will read this".encode())

change localhost to the topside IP, if your qgc read this message, the communication is working and you can try the named_value_float message next.