Adding a sensor to mavlink stream

Blue Robotics Software Companion (Retired)
22

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