Telemetry Forwarding with Mavproxy

Hello friends,

I’m currently stuck trying to forward telemetry data from a sensor through a pi computer companion to a flight controller down to the GCS using mavproxy’s UDP broadcast option.

So:
Sensor → Pi → Flight Controller → Sik Telemetry Radio → GCS

I’ve verified that the pi can run a master command and control the flight controller via mavlink

I’ve verified that the sensor is in fact in working and sending data out, /dev/ttyUSB1

So then now when I am trying to forward the data from the sensor I run:
mavproxy.py --master=/dev/ttyUSB1 --baudrate 115200 --out=udpbcast<MY_IP>:14550 --nowait

I get a link 1 down, but I am able to see the UDP packets through wireshark when I set the IP to 0.0.0.0 now I need to capture the broadcast. What’s the best way to capture the packets? I’m thinking of writing a python script using pymavlink.

image

Is the sensor on /dev/ttyUSB0 and the flight controller on /dev/serial0 ?

In that case you want to run mavproxy.py --master=/dev/serial0 --baudrate 921600 --out=/dev/ttyUSB0:115200 --nowait

The flight controller must always be on the --master side

1 Like

Stephen,

Thanks for the response! I definitely need all the help I can get.

I have a USB hat on the pi so the sensor is on /dev/ttyUSB1 and yes, the flight controller is on /dev/serial0.

So in that case I would need to run two masters on the same CLI?

mavproxy.py --master=/dev/serial0 --baudrate 921600 --out=/dev/ttyUSB1:115200 --nowait --master=/dev/ttyUSB1 --baudrate 115200 --out=udpbcast<MY_IP>:14550 --nowait

MAVProxy assumes that any master links are a flight controller and goes through a query-response routine to configure MAVProxy for the relevant controllers. If there’s no controller on a master link, MAVProxy will assume there’s a link error.

The “output” links do support 2-way communication, so your messages will be routed.

Awesome. I really appreciate the help!

Now that it’s running it appears to be mixing the data into the other data being printed out, as seen on the image below.

I’m trying to run a simple receiver script using pymavlink below. Right now, I just want to see if it is receiving the messages by printing the message and work on printing the data later. Currently, I am not seeing anything on the receiver, but I can see the UDP packets going out on wireshark. Am I right in assuming I can use the pymavlink library for receiving even though it is transmitted through mavproxy, because the data is encapsulated using the mavlink protocol?

MAVProxy uses pymavlink for MAVLink message encoding/decoding. So yes, using pymavlink will work.

1 Like

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