Telemetry Forwarding with Mavproxy

Stephen,

Do I need to create a custom mavlink message in order for the sensor data to be received?

I’m currently testing the forwarding with an ethernet cable between the pi and my computer. When I try to receive the output directly from the sensor, the pymavlink listen script doesn’t seem to be receiving any traffic, while it is able to receive when it is only flight controller as master. Is the sensor data packaged into the mavlink payload automatically when I set the sensor as master and out as my pymavlink receiver script?

Thank you for being so patient with me and I appreciate all the info you’ve given me this far!

Most likely not. It is easiest to use the existing message set. You’ll need to look through the MAVLink spec to find a message type that suits the data you’re sending.

Is the listen script waiting for a heartbeat? If so, you’ll need to implement a heartbeat send in your sensor send script.

If you’re able to post your sensor send script, I’m happy to give it a quick look-over.

I was using the guide in ardupilot and was trying to find a similar message. The page in the guide mentions the SCALED_PRESSURE message and although my message is similar to that or the HYGROMETER_SENSOR message, my sensor outputs all the data in 255 bytes of ASCII, so I wasn’t sure what type of premade message I should look for or if I should create a char[255] type message.

The sensor I am using is connected to the raspberry pi companion computer via USB, baudrate 115200, and is transmitting data at 40Hz when a switch is flipped. The data is all ASCII and is formatted with a tag followed by the value as shown below. For instance the first value S 01.042 is the wind speed(Im in a room with no wind).

In the raspberry pi companion computer connected via serial to the flight controller. In the pi I am running the script mavproxy.py --master=/dev/serial0,912600 --aircraft --MyCopter --default-modules "link" --master=/dev/ttyUSB0,115200 --nowait --non-interactive --out=udpbcast:0.0.0.0:14550

On the GCS I’ve tried reading and listening with pymavlink and mission planner. mission planner was able to receive the connection via SIK telem COM#. My listen script was also able to connect to the flight controller using mavutil.mavlink_connection(udp:IP:PORT).

#from dronekit-python docs
from pymavlink import mavutil

# Start a connection listening to a UDP port
the_connection = mavutil.mavlink_connection('udp:0.0.0.0:14550')

# Wait for the first heartbeat
#   This sets the system and component ID of remote system for the link
the_connection.wait_heartbeat()
print("Heartbeat from system (system %u component %u)" % (the_connection.target_system, the_connection.target_component))

# Once connected, use 'the_connection' to get and send messages
while 1:
    #this displays all of the data
    msg = the_connection.recv_match(blocking=True)

    #this requests ALL data streams
    #the_connection.mav.request_data_stream_send(the_connection.target_system,the_connection.target_component,mavutil.mavlink.MAV_DATA_STREAM_ALL,args.rate,1)
    print(msg)

    # This sends a heartbeat back to the drone
    mavlink.heartbeat_send()

I’ve also removed tried it with wait_heartbeat()

There’s not a single message that can hold a string that large. Typically, MAVLink messages are designed to be small - to make it easier to transmit packets over lossy/slow RF links.

Looking at the example data you posted, do you need all of that data? Or just some parts of it? You’d have more options for using pre-existing MAVLink messages if you could keep the message <100 bytes.

MAVProxy only accepts MAVLink data on all links. If the /dev/ttyUSB0 sensor is not outputting MAVLink data, MAVProxy will likely not work. You’ll need a script that takes in data from the /dev/ttyUSB0 sensor and outputs it in MAVLink format to MAVProxy.

Ah I see! That answers a lot of my problems with the way I was approaching this.

I definitely do not need all the data and I can have it output less.

So, to my understanding, the tasks I would need to complete at this point would be:

  1. Configuring the sensor to output or pass less data. Preferably under 100 bytes.

  2. Create a script that takes the data and formats it into a MAVLink Serialization Frame. For the MSG_ID packet, I can either:

  • Use something like COMPONENT_INFORMATION(#395) or COMPONENT_METADATA(#397), which are the only messages with char[100] and use mavlink2.
  • Create a custom message using mavgen and add it to the common or ardupilot_mega in the mavlink directories for both the computer companion and GCS.
  1. Create a listen script that listens for that creates the link and receives the matching connection. Something like:
from pymavlink import mavutil
conn = mavutil.mavlink_connection('udp:IP:PORT')
while 1:
   msg = conn.recv_match(type=MSG_ID_TYPE,blocking=True)
   print(msg)

Is there a known specific tool for the packet construction? Otherwise, I am looking at creating a script using SCAPY in python.

Am I on the right track or am I missing anything important here?

Yes, that’s correct. The only gotcha is that if you create a custom message, you’ll need to re-build the MAVLink library (i.e. pymavlink if you’re using Python) on the companion computer (and any other system that consumes the custom message).

I’d also suggest looking at the NAMED_VALUE_FLOAT (Messages (common) · MAVLink Developer Guide) message. It’s commonly used for transporting custom sensor messages.

1 Like

This is what I have so far for my script for my step 2. I was planning on running this script as a background process. Although, I wasn’t sure how to handle the flags and new fields in mavlink2 frame.

#This is a generic script for logging sensor data from a sensor connected to a raspberry pi.
#To run this as a background process on the CLI, type 'python3 file_write_TSM.py &'
#type 'kill -9' followed by PID number to terminate process

#The script does the following.
#1)Creates a file for the sensor data onboard the computer companion
#2)Reads the incoming Serial data and stores it in a variable
#3)Creates a mavlink packet to forward to the ardupilot flight controller.

#!/usr/bin/env python
import time
import serial
import datetime
from os.path import exists

###########Variables for converting to mavlink###################
stx='0xFD' #protocol specific start of text
length=100 #payload length 0-255
incF= #incompatibility flag
cmpF= #compatibility flag
sq= 0 #sequence - detects apcket loss
sysID= 1#ID of vehicle sending the message 1-255
cmpID= 0#ID of component sending the message. zero broadcast to ALL
msgID=251#ID of message type used in payload used to decode data back in msg object.251 is a NAMED_VALUE_FLOAT
payload=' '#Stores the sensor data
chksm=
sig=0#13 byte signature. THIS IS OPTIONAL

########Naming and creating file for Sensor Log################
dt =  str(datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_%S"))
filename = 'SENSOR_SAMPLE' + dt +'.txt'
f = open('PATH/TO/FILE'+filename , 'w')
########Naming and creating file for mavlink converted packets####
filename = 'mavlink_SENSOR_SAMPLE' + dt +'.txt'
f2 = open('PATH/TO/FILE'+filename , 'w')


#Set Up Serial Port for the sensor
#Ensure you are using the correct header name in '/dev' which the component is plugged in to. If the ETHUSB Hub Hat is attached, the ttyUSB number sometimes changes between zero and one.
ser = serial.Serial(
        '/dev/ttyUSB0',
        baudrate = 115200,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE,
        bytesize=serial.EIGHTBITS,
        timeout=1
)

#The program will continue to write into the file until it is terminated.
while 1:
    data = str(ser.read(100))
    #debugs printer
    print(data)
    #writes into the log
    f.write(data)
    f.write('/n')
    #convert the data into mavlink
    sq++ #increment sequence
    if sq = 256:
        sq = 0
    payload = data
    packet= stx + len + seq + sysID + comID + msgID + payload + checksum
    f2.write(packet)
    f2.write('/n')

f.close()
f2.close()

You’re on the right track, but using the pymavlink library will make things easier for you, as all the message encoding and assembly is done for you.

I’ve modified your code to use pymavlink (note I’ve not tested it … there might be a typo or two)

#This is a generic script for logging sensor data from a sensor connected to a raspberry pi.
#To run this as a background process on the CLI, type 'python3 file_write_TSM.py &'
#type 'kill -9' followed by PID number to terminate process

#The script does the following.
#1)Creates a file for the sensor data onboard the computer companion
#2)Reads the incoming Serial data and stores it in a variable
#3)Creates a mavlink packet to forward to the ardupilot flight controller.

#!/usr/bin/env python
import time
import serial
import datetime
from os.path import exists
from pymavlink import mavutil

########Naming and creating file for Sensor Log################
dt =  str(datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_%S"))
filename = 'SENSOR_SAMPLE' + dt +'.txt'
f = open('PATH/TO/FILE'+filename , 'w')

#Set Up Serial Port for the sensor
#Ensure you are using the correct header name in '/dev' which the component is plugged in to. If the ETHUSB Hub Hat is attached, the ttyUSB number sometimes changes between zero and one.
ser = serial.Serial(
        '/dev/ttyUSB0',
        baudrate = 115200,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE,
        bytesize=serial.EIGHTBITS,
        timeout=1
)

# Setup MAVLink to connect on udp 127.0.0.1:14550
conn = mavutil.mavlink_connection("udp:127.0.0.1:14550", autoreconnect=True, source_system=self.source_system, force_connected=False, source_component=mavutil.mavlink.MAV_COMP_ID_PERIPHERAL)

# wait for the heartbeat msg to find the system ID
while True:
    if conn.wait_heartbeat(timeout=0.5) != None:
        # Got a heartbeat from remote MAVLink device, good to continue
        break

#The program will continue to write into the file until it is terminated.
while 1:
    data = str(ser.read(100))
    #debugs printer
    print(data)
    #writes to MAVLink as a STATUSTEXT message, encoded as ASCII
    conn.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_INFO, data.encode())


f.close()
1 Like

Stephen,

Sorry for the late response. I appreciate the feedback.

Ahh, I see… So pymavlink does all the heavy lifting for encoding the data. Running this in the background, whilst running mavproxy with the flight controller and the sensor as masters should allow me to see the data in QGroundControl’s COMMCONSOLE, according to the statustext_send() function’s notes.

This is the reiteration of my test setup.
I have an mrox2.1 with ardupilot quad frame firmware 4.2 The sensor is connected to the pi via serial and is ttyUSB0.
I’m running the following commands in a bash script:

python3 /home/pi/metoc_stuff/telem2.py & 
mavproxy.py --master=/dev/serial0,912600 --aircraft --MyCopter --default-modules "link" --non-interactive --master=/dev/ttyUSB0,115200 --nowait --non-interactive --out=udpbcast:127.0.0.1:14550 &

telem2.py is just that script you helped me with earlier.

I get the following result:

  1. The upper right is the pi executing my script which runs telem2.py and the mavproxy command given above.
  2. The bottom right is showing the sik telem radio known as ttyUSB0 on my ubuntu desktop.
  3. The top left is QGroundControl. It is connected to the flight controller via sik telemetry radio. I have the console open.
  4. The bottom right is the QGC CLI and it doesn’t seem to display any responses from mavproxy

I didn’t set the flight controller master to “non-interactive” initially but when I did, the second link is down

I’m trying to adjust my script in different ways such as using one line for mavproxy and setting multiple masters, or two lines, but im still having difficulties seeing it on the other end.

  1. Am I going to be able to see the data in MAVLink console on QGC? The QGC documentation says it would be seen in the “COMM CONSOLE” I’m not too sure where that is, but I assumed I would be able to see the messages in the CLI.
  2. Do you have any tips for setting up the mavproxy command so that the second master’s(sensor) data doesn’t go directly into the mavproxy interactive shell? I’m trying to use daemon and non-interactive but they don’t seem to be working.

That’s not quite right. telem2.py takes in data on /dev/ttyUSB0, converts to mavlink and outputs that on udp:127.0.0.1:14550. So MAVProxy needs to listen on the udp port, not the serial port.

So, what you need is:

python3 /home/pi/metoc_stuff/telem2.py & 
mavproxy.py --master=/dev/serial0,912600 --aircraft --MyCopter --default-modules "link" --non-interactive --master=udp:127.0.0.1:14550 --nowait --out=udp:127.0.0.1:14551 &

So MAVProxy is now outputting on 127.0.0.1:14551, so you’ll need to connection QGroundControl to that port.

You should see the messages in the “MAVLink Inspector” tab of QGroundControl

That makes sense. Once I added the UDP listening port 14551 and ran your edited telem_script.sh I am able to see the heartbeats on MAVLink Inspector coming in on
System 255. And I see it stop once I kill the processes. Although, the system_status value is staying at a constant zero. Where do you think I need to fix this? I was thinking it had something to do with the encoded data not being transmitted properly. There are no error messages given.

This is what I’m using to verify that the data from the weather sensor is still coming in

I also combined the while loops into one for the telem2.py. If I uncomment the print(data) line the sensor data prints.

System 255 is typically the GCS, so you might be seeing the GCS sending out heartbeat packets. Are there other systems available in the dropdown menu? If you, the GCS likely does not have a connection to MAVProxy.

The GCS as in mavproxy acting as a GCS right?

The drop down had three connections. System 1 which I assumed to be the flight controller; it showed everything from the battery, imu, vibration, and etc; the second system I think may have been may have been System ID 55? I wasn’t able to get a snapshot of it. 255 only showed up when I ran the bash script. And I observed the values for system_status for System 1 & 255 and didn’t see any updates. I’m not sure if I saw any updates on the third system but I will check again.

Ok, I’ve had another look at this. MAVProxy won’t quite do what you want, as there’s a number of assumptions about the links containing vehicle or GCS data.

I’d suggest using mavlink-router instead. Install details are at GitHub - mavlink-router/mavlink-router: Route mavlink packets between endpoints

Using the following mavlink-router command:

mavlink-routerd -e 127.0.0.1:14550 -e 127.0.0.1:14551 /dev/serial0,912600

And the following script:

#!/usr/bin/env python3
#This is a generic script for logging sensor data from a sensor connected to a raspberry pi.
#To run this as a background process on the CLI, type 'python3 file_write_TSM.py &'
#type 'kill -9' followed by PID number to terminate process

#The script does the following.
#1)Creates a file for the sensor data onboard the computer companion
#2)Reads the incoming Serial data and stores it in a variable
#3)Creates a mavlink packet to forward to the ardupilot flight controller.

#!/usr/bin/env python
import time
import serial
import datetime
from os.path import exists
from pymavlink import mavutil

########Naming and creating file for Sensor Log################
dt =  str(datetime.datetime.now().strftime("%Y_%m_%d_%H_%M_%S"))
filename = 'SENSOR_SAMPLE' + dt +'.txt'
f = open(filename , 'w')

#Set Up Serial Port for the sensor
#Ensure you are using the correct header name in '/dev' which the component is plugged in to. If the ETHUSB Hub Hat is attached, the ttyUSB number sometimes changes between zero and one.
ser = serial.Serial(
        '/dev/ttyACM0',
        baudrate = 115200,
        parity=serial.PARITY_NONE,
        stopbits=serial.STOPBITS_ONE,
        bytesize=serial.EIGHTBITS,
        timeout=1
)

# Setup MAVLink to connect on udp 127.0.0.1:14550
conn = mavutil.mavlink_connection("udp:127.0.0.1:14550", autoreconnect=True, source_system=1, force_connected=False, source_component=mavutil.mavlink.MAV_COMP_ID_PERIPHERAL)

# wait for the heartbeat msg to find the system ID
while True:
    if conn.wait_heartbeat(timeout=0.5) != None:
        # Got a heartbeat from remote MAVLink device, good to continue
        break

#The program will continue to write into the file until it is terminated.
while 1:
    # convert from bytes to string then strip any whitespace
    data = ser.read(100).decode("utf-8").strip()
    if data != '':
        #debugs printer
        print(data)
        #writes to MAVLink as a STATUSTEXT message, encoded as ASCII
        conn.mav.statustext_send(mavutil.mavlink.MAV_SEVERITY_INFO, data.encode())


f.close()

I was able to get this showing up in QgroundControl’s MAVLink Inspector window (component 158).

The data went through!

I can see most of it under Comp 158 → text.

Now in order to receive the values in a separate script I just have to add another UDP endpoint on the pi side (lets say 14552) and listen on that port using pymavlink.mavutil.mavlink_connection() right? Using the examples from mavlink.io, I set up a listen.py script to test it. what functions should I use to access the incoming data? I saw that recv_math allows us to capture the message. Do I need to decode the message in order to print?

This screenshot shows my listen.py script running on my desktop along with QGroundControl receiving data in the text; the pi is sending using mavlink router where I added an extra endpoint 14552.

I used mavgen to create the plugin for Wireshark to detect the messages, but I’m not seeing them despite the live data coming through on QGC. I used common for the messages and mavlink 2.

I do see a bunch of MDNS protocol data coming in when QGC is running, but nothing else. How is QGC able to get the data, but nothing else? My listen script or wireshark? Does it have to do with the way it is sent in mavlink router?

1 Like

Yes, that’s correct.

A script like this would work:

#!/usr/bin/env python3
#This is a generic script for accessing MAVLink data over UDP port 14552

from pymavlink import mavutil

# Setup MAVLink to connect on udp 127.0.0.1:14552
conn = mavutil.mavlink_connection("udp:127.0.0.1:14552", autoreconnect=True, source_system=1, force_connected=False, source_component=mavutil.mavlink.MAV_COMP_ID_LOG)

# wait for the heartbeat msg to find the system ID
while True:
    if conn.wait_heartbeat(timeout=0.5) != None:
        # Got a heartbeat from remote MAVLink device, good to continue
        break

print("Got Heartbeat from ArduPilot (system {0} component {1})".format(conn.target_system,
                                                                 conn.target_system))

while True:
    msg = conn.recv_match(blocking=True, timeout=0.5)
    if msg:
        if msg.get_type() == 'STATUSTEXT':
            #print STATUSTEXT packet text
            print(msg.text)

I believe it gets stuck on the wait_heartbeat() when I run that script you shared with me. I’ve been playing around with other ways and have been stuck with this state:

In this image you can see it receiving continuously on the STATUSTEXT, but not on the listen script on the bottom left. Does it have to do with the connection type? I have the receiving sik telemetry radio in ttyUSB0 on the desktop.

This is how it is sending on the pi via ssh from a laptop.
image

I’ve taken a few steps and got some different results:

  1. Re-flashed a firmware Ardupilot 4.0.7
  2. Application Settings → General → Autoconnect to the following devices: Disabled all autoconnect except sik radio.
  3. Application Settings → Comm Links → Autconnect, Type: UDP, Port:14552.
  4. Application Settings → MAVLink → Ground Station: Enabled Emit Heartbeat, Enabled MAVLink Forwarding: to localhost:14552


I am now seeing some standard data for the firmware type and etc but I can’t see my sensors data going. I am also not seeing COMPONENT 158 anymore.

I haven’t touched my pi side script.

I’ve verified my serial data is still being sent to the flight controller using a TTL USB adapter, but I just can’t seem to see the data on component 158 now. Any ideas on why this is occurring???

Good morning Sir, I am doing a thesis on real-time characterisation of atmosphere using a drone swarm.
For that I look for receiving sensor data via mavlink messages. How can I proceed? Thank you.