Adding a sensor to mavlink stream

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' # MUST be 9 characters or fewer
    
    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.

Hi, I followed @EliotBR 's guide for my sensor and have it working nicely, so thank you! However, we are transitioning our systems to a Navigator/Blue OS setup, and I was wondering if it is possible to reuse the script and settings I used on my Companion computer on my BlueOS computer. If not, do you have any suggestions to how I can achieve similar functionality on BlueOS?

Thanks,

Kristian

Hi @krisaue,

I’m glad this thread has been helpful for you :slight_smile:

BlueOS (1.1 beta) has an extensions system, which is the recommended approach for making device integrations. The creation process is a bit more involved than just putting a script in a folder but as a result makes extensions much easier to share across vehicles, and to integrate a visual interface where that’s desirable.

Hello my friend. Sorry for the translation. First of all, thank you for the above topic. I wrote a script similar to this and was able to send the sensor yield. Now, I want to send the battery data I want to do to pixhawk as a mavlink message. So instead of reading the voltage with a ready module, I want to send the voltage I read to Pixhawk. Any ideas on this?

Hey, how can I use this approach to use Bar02 pressure sensor for depth hold and related purpose?

Currently, I am trying to configure ( run & build ) QGroundControl Source Code ( from github ), to costumize it but failed to rebuild it , as it showing " missing kit error .

I am using :-
Windows 11
Qt creator 5.15

how to configure MS Visual Studio 2019 with Qt Creator

2 posts were merged into an existing topic: Cygnus Ultrasonic thickness gage

A post was merged into an existing topic: Cygnus Ultrasonic thickness gage

Hi @ysfkg, welcome to the forum :slight_smile:

You may have already solved this, but in case you haven’t and are still interested in doing so, I believe it should work to use the BATTERY_STATUS MAVLink message, although that’s not something I’ve tried to do before.

ArduSub does not currently support receiving depth/pressure estimates via MAVLink.

There is some further discussion here on how the Bar02 could potentially be integrated directly into ArduSub, which is likely a more appropriate thread for further discussion on this particular topic :slight_smile:

Hi @Programming_Yug, welcome to the forum :slight_smile:

I haven’t built QGC in quite a while, so the main suggestion I can give is to very closely follow the steps specified in the QGroundControl build docs - if they are not followed exactly then building may fail to work as expected.


As something of a side note, if you’re just wanting to display an additional sensor value you may wish to try using Cockpit instead of QGroundControl, because Cockpit can automatically display any custom NAMED_VALUE_FLOAT messages in its VeryGenericIndicator mini-widget, without needing to be rebuilt.

Cockpit currently requires BlueOS to be installed on your vehicle’s onboard computer (Raspberry Pi), so if your sensor code is intended to run on the onboard computer then you’ll likely want to create a BlueOS extension instead of the script creation and running process described in this thread.