Mavproxy Server Implementation

Overview

MAVProxy can be used to control autopilots by using mavlink commands. As a first step into allowing an Android phone to control the autopilot using the famous applications such as 3DR's Tower wirelessly without the need for any new hardware, I allowed mavproxy to be controller through a Tcp/Udp client.

If you haven't installed Mavproxy yet, here is how to do it.

Through this post, I'll show you how you can modify Mavproxy to run a Tcp/Udp server. If you are not interested in how it is done, and just want the final code, you can find it here on github.

Mavproxy-server usage

To run a TCP server:

        mavproxy.py --tcp=0.0.0.0:5234

To run a UDP server:

        mavproxy.py --udp=0.0.0.0:5234

You can run both TCP and UDP servers simultaneously, but only the UDP server will be able to write mavlink commands. This is not advised.

Steps into getting there

Learn Tcp/Udp programming in Python

Since mavproxy is written in python, then we do have to modify it in python. For starters, to run a UDP server, this snippet of code would open port 4123 on the machine and print whatever is sent to it.

>>> import sys, socket
>>> s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
>>> s.bind(('0.0.0.0', 4123))
>>> while 1:
...     data,addr = s.recvfrom(1024)
...     print data

To run a UDP client, that can be done easily through a linux terminal in bash.

nc 127.0.0.1 4123 -u -v  

And you'd be able to send to the server.

If this worked locally for you, but remotely it didn't, check your firewall settings.

Modify mavproxy's input

Looking through mavproxy's code in mavproxy.py, there is a function called input_loop(). This function waits for user input. We would want to redirect the input from keyboard to the Tcp/Udp server set.

The previous code was:

def input_loop():  
    '''wait for user input'''
    while mpstate.status.exit != True:
        try:
            if mpstate.status.exit != True:
                line = raw_input(mpstate.rl.prompt)
        except EOFError:
            mpstate.status.exit = True
            sys.exit(1)
        mpstate.input_queue.put(line)

If we want a UDP server to listen to input here, recklessly we can modify it as such:

def input_loop():  
    s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
    s.bind(('0.0.0.0', 4123))
    '''wait for user input'''
    while mpstate.status.exit != True:
        try:
            if mpstate.status.exit != True:
                data,addr = s.recvfrom(1024)
                line = data
                # line = raw_input(mpstate.rl.prompt)
        except EOFError:
            mpstate.status.exit = True
            sys.exit(1)
        mpstate.input_queue.put(line)

Remember to add the socket import.

And that is it. Whatever now is sent through UDP on this port will be run as a command.

Modify mavproxy's output

To read the log, we would also have to send the log back to the UDP client. If we look around, you'd find that writing is done by mpstate.console.writeln(text). console is a TextConsole class that is available in modules/lib/textconsole.py. Looking at its output code:

    def write(self, text, fg='black', bg='white'):
        '''write to the console'''
        if isinstance(text, str):
            sys.stdout.write(text)
        else:
            sys.stdout.write(str(text))
        sys.stdout.flush()

We can add s.sendto(data, address), to be able to send to the client.

You would have to be able to read the socket s there somehow.

Cleaning things up

To clean things up:

1. Create a class for UdpServer and TcpServer. In my project, you can find them under modules/lib.

2. Add parser options in mavproxy.py to read --udp and --tcp options to allow you to run the servers upon will.

3. Put everything where it should be.

Next steps

Now that I can connect and send commands remotely to the autopilot, it is time to be able to use the Android applications to control the autopilot. Maybe someone can add a functionality for mavproxy to understand mavlink packets directly.

More: mavproxy, tcp/ip, python