Home        Store        Learn        Blog

Adding a sensor to mavlink stream

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:

Hi @EliotBR,

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.

Hi @EliotBR,

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.

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.

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…

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.

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).

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:

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.

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:

Hi @EliotBR,

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.

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

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.

Hi @EliotBR

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.

That’d make sense, thanks!

Absolute legend!
I needed source_system=1 it seems. Thanks for sticking through with me until this got sorted out! :smiley:
I was pretty excited when I heard QGC talking to me :stuck_out_tongue:

1 Like

Here’s the summary

Companion computer

Read & Send Script

Make a script in the ~/companion/tools/ directory that:

  1. Connects to the top computer (requires QGC to be open)
  2. Gets sensor data (float)
  3. Sends the data in a named_value_float message

e.g.

import time
from pymavlink import mavutil

def wait_conn(device):
    """ Sends a ping to develop UDP communication and waits for a response. """
    msg = None
    global boot_time
    while not msg:
        # boot_time = time.time()
        device.mav.ping_send(
            int((time.time()-boot_time) * 1e6), # Unix time since boot (microseconds)
            0, # Ping number
            0, # Request ping of all systems
            0  # Request ping of all components
        )
        msg = device.recv_match()
        time.sleep(0.5)

if __name__ == '__main__':
    boot_time = time.time()
    # establish connection to top-side computer
    direction = 'udpout'
    device_ip = '192.168.2.1'
    port      = '14550'
    params    = [direction, device_ip, port]
    print('connecting to {1} via {0} on port {2}'.format(*params))
    computer  = mavutil.mavlink_connection(':'.join(params), source_system=1)
    print('waiting for confirmation...')
    wait_conn(computer)
    print('connection success!')
    
    # connect to sensor and set up output
    # TODO: connect to sensor
    sensor_name = 'MySensor'
    
    while 'reading data':
        value = sensor.get_value() # TODO: implement something like this
        computer.mav.named_value_float_send(
            int((time.time() - boot_time) * 1e3), # Unix time since boot (milliseconds)
           sensor_name,
           value
        )

For my ultrasonic thickness gauge case, I was reading in data with a serial connection and wanted a command-line option to only read and print the sensor data, without connecting to mavlink, so I could use ssh to check if the sensor was working. Code for that can be found here.

Run on startup

Modify ~/companion/.companion.rc, adding a new screen session with an appropriate name, to run right after telem.py. If the read and send script doesn’t end (likely the case), have an & to run in its own thread
e.g.

	sudo -H -u pi screen -dm -S mavproxy $COMPANION_DIR/tools/telem.py
	sudo -H -u pi screen -dm -S sensor $COMPANION_DIR/tools/sensor.py &
	sudo -H -u pi screen -dm -S video $COMPANION_DIR/tools/streamer.py 

Restart on telemetry restart

Modify ~/companion/scripts/restart-mavproxy.sh to also quit and restart the custom screen session.
e.g.

#!/bin/bash

screen -X -S sensor quit
screen -X -S mavproxy quit

sudo -H -u pi screen -dm -S mavproxy $COMPANION_DIR/tools/telem.py
sudo -H -u pi screen -dm -S sensor $COMPANION_DIR/tools/sensor.py &

QGroundControl

Follow the instructions to get the code, but before building make the following modifications:

  1. In main ArduSub code file (src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.cc)
// in void ArduSubFirmwarePlugin::_handleNamedValueFloat(mavlink_message_t* message)
    } else if (name == "RollPitch") {
        _infoFactGroup.getFact("rollPitchToggle")->setRawValue(value.value);
    } else if (name == "MySensor") { // should be the same name as in your companion script
      _infoFactGroup.getFact("mySensor")->setRawValue(value.value); //name for finding in QGC
    }
}

// ...
const char* APMSubmarineFactGroup::_rollPitchToggleFactName     = "rollPitchToggle";
const char* APMSubmarineFactGroup::_mySensorFactName            = "mySensor";
const char* APMSubmarineFactGroup::_rangefinderDistanceFactName = "rangefinderDistance";

// ...
// in APMSubmarineFactGroup::APMSubmarineFactGroup(QObject* parent)
    , _rollPitchToggleFact     (0, _rollPitchToggleFactName,     FactMetaData::valueTypeDouble)
    , _mySensorFact            (0, _mySensorFactName,            FactMetaData::valueTypeDouble)
    , _rangefinderDistanceFact (0, _rangefinderDistanceFactName, FactMetaData::valueTypeDouble)
{
// ...
    _addFact(&_rollPitchToggleFact    , _rollPitchToggleFactName);
    _addFact(&_mySensorFact           , _mySensorFactName);
    _addFact(&_rangefinderDistanceFact, _rangefinderDistanceFactName);

// ...
    _rollPitchToggleFact.setRawValue     (2); // 2 shows "Unavailable" in older firmwares
    _mySensorFact.setRawValue            (std::numeric_limits<float>::quiet_NaN()); // display as --.-- when not connected
    _rangefinderDistanceFact.setRawValue (std::numeric_limits<float>::quiet_NaN());
}
  1. In ArduSub header file (src/FirmwarePlugin/APM/ArduSubFirmwarePlugin.h)
// in class APMSubmarineFactGroup : public FactGroup
    Q_PROPERTY(Fact* inputHold           READ inputHold           CONSTANT)
    Q_PROPERTY(Fact* mySensor            READ mySensor            CONSTANT)
    Q_PROPERTY(Fact* rangefinderDistance READ rangefinderDistance CONSTANT)

// ...
    Fact* inputHold           (void) { return &_inputHoldFact; }
    Fact* mySensor            (void) { return &_mySensorFact; }
    Fact* rangefinderDistance (void) { return &_rangefinderDistanceFact; }

// ...
    static const char* _rollPitchToggleFactName;
    static const char* _mySensorFactName;
    static const char* _rangefinderDistanceFactName;

// ...
    Fact            _rollPitchToggleFact;
    Fact            _mySensorFact;
    Fact            _rangefinderDistanceFact;
};
  1. In QGC name descriptions file (src/Vehicle/SubmarineFact.json)
{
    "name":             "rollPitchToggle",
    "shortDesc": "Roll/Pitch Toggle",
    "type":             "int16",
    "enumStrings":      "Disabled,Enabled,Unavailable",
    "enumValues":       "0,1,2"
},
{
    "name":             "mySensor",
    "shortDesc": "DisplayName",
    "type":             "float",
    "decimalPlaces":    3,
    "units":            "mm"
}
]
}

then complete the QGC build through QtCreator and run.

Your new sensor should now be available through the QGC sensor display. If it appears but with --.-- as the value then it’s not currently sending any readings, so check that the screen session exists (through ssh, or the list of services at the top of 192.168.2.2:2770/system).

Note:

There will be some time offset between the messages sent by the custom script and those sent by the pixhawk. If doing time-based alignment it’s best to ignore the time for the custom script messages and instead calculate a new time based on the message received before and after in the top-side mavlink stream. Alternatively it may be possible to change the custom script to first make a mavlink connection to the pixhawk, and replace boot_time by the difference between time.time() and the time of the received message, although I haven’t yet tried this and can’t guarantee it would work correctly.

4 Likes

Very nice! Thank you for this!

Do you mind if we put this on our docs at ardusub.com (with proper credits)?

Not at all, I want it to be as useful as possible :slight_smile:
Thanks again for all the help getting it to work in the first place :slight_smile:

2 Likes

I have tried your @EliotBR code and got this error

connecting to localhost via udpin on port 14540
waiting for confirmation...
connection success!
Traceback (most recent call last):
  File "main.py", line 40, in <module>
    master.mav.named_value_float_send(
  File "/home/***/.local/lib/python3.8/site-packages/pymavlink/dialects/v10/ardupilotmega.py", line 20329, in named_value_float_send
    return self.send(self.named_value_float_encode(time_boot_ms, name, value), force_mavlink1=force_mavlink1)
  File "/home/***/.local/lib/python3.8/site-packages/pymavlink/dialects/v10/ardupilotmega.py", line 13784, in send
    buf = mavmsg.pack(self, force_mavlink1=force_mavlink1)
  File "/home/***/.local/lib/python3.8/site-packages/pymavlink/dialects/v10/ardupilotmega.py", line 13336, in pack
    return MAVLink_message.pack(self, mav, 170, struct.pack('<If10s', self.time_boot_ms, self.value, self.name), force_mavlink1=force_mavlink1)
struct.error: argument for 's' must be a bytes object

I couldnt find anything helpfull yet. I would be very happy if you can help me.

Hi @mhcekic, welcome to the forum :slight_smile:

I wrote and posted this code while working for a different company, but it did definitely work at the time.

What kind of setup are you using?

This specifies you’re connecting to something on the same device, on port 14540, so you’ve clearly set up your own endpoints/ports. The code was written for sending messages from the Companion computer to QGroundControl on the topside computer.

Here your traceback specifies you’re using Python 3.8, which isn’t available in the normal Companion software image. The code was written using Python 2.7, which has some differences from Python 3 with respect to bytes and strings. Given it’s complaining that it wants a bytes object you may need to change the sensor_name from the current string (e.g. try sensor_name = b'MySensor').

Note that since your setup is seemingly quite different to what the code was designed for, it’s very possible there will be other required changes for it to work properly.