RC piDrone connectivity

The network connection / interaction between the RC and piDrone is complete:

#!/usr/bin/python

import socket
import struct
import sys
import select
import time

def client():

    poll = select.poll()

    go_go_go = False

    pack_format = "=ffffb?"
    pack_size = struct.calcsize(pack_format)

    unpack_format = "=?"
    unpack_size = struct.calcsize(unpack_format)

    client = socket.socket()
    host = socket.gethostname()
    port = 31415

    while True:
        try:
            client.connect((host, port))
        except:
            time.sleep(0.1)
        else:
            break
        continue
        
    client_fd = client.fileno()
    poll.register(client_fd, select.POLLIN | select.POLLPRI)

    UD = 0.0
    state = 27
    beep = True

    try:
        running = True
        while running:
            results = poll.poll(500) # milliseconds

            #----------------------------------------------------------------------------------
            # Check whether there's I/O from RC
            #----------------------------------------------------------------------------------
            for fd, event in results:
                if fd == client_fd:
                    #--------------------------------------------------------------------------
                    # Has piDrone told piRC to start?
                    #--------------------------------------------------------------------------
                    raw = client.recv(unpack_size)
                    assert (len(raw) == unpack_size), "Invalid data"

                    #--------------------------------------------------------------------------
                    # React on the action
                    #--------------------------------------------------------------------------
                    formatted = struct.unpack(unpack_format, raw)

                    if formatted[0]: # is True
                        go_go_go = True
                    else:
                        go_go_go = False
                        running = False

            if not go_go_go:
                continue

            YR = UD + 0.1
            FB = YR + 0.1
            LR = FB + 0.1

            output = struct.pack(pack_format, UD, YR, FB, LR, state, beep)
            client.send(output)
            print "SENT:  UD = %f | YR = %f | FB = %f | LR = %f | status = %d | beep = %d" % (UD, YR, FB, LR, state, beep)

            UD = LR + 1


    except Exception, err:
        print err
    finally:
        client.close()

def server():

    poll = select.poll()

    unpack_format = "=ffffb?"
    unpack_size = struct.calcsize(unpack_format)

    pack_format = "=?"

    server = socket.socket()
    host = socket.gethostname()
    port = 31415
    server.bind((host, port))

    server.listen(5)

    try:
        connection, addr = server.accept()
        connection_fd = connection.fileno()
        poll.register(connection_fd, select.POLLIN | select.POLLPRI)

        #--------------------------------------------------------------------------------------
        # Tell the client to go-go-go!
        #--------------------------------------------------------------------------------------
        output = struct.pack(pack_format, True)
        connection.send(output)

        #--------------------------------------------------------------------------------------
        # Listen to the client and do what it says.
        #--------------------------------------------------------------------------------------
        while True:
            results = poll.poll(500)

            #----------------------------------------------------------------------------------
            # Check whether there's I/O from RC
            #----------------------------------------------------------------------------------
            for fd, event in results:
                assert fd == connection_fd, "WHOSE FD IS THIS?"

                #--------------------------------------------------------------------------
                # Unpack the data received
                #--------------------------------------------------------------------------
                raw = connection.recv(unpack_size)
                assert (len(raw) == unpack_size), "Invalid data"

                #--------------------------------------------------------------------------
                # React on the action
                #--------------------------------------------------------------------------
                formatted = struct.unpack(unpack_format, raw)
                assert (len(formatted) == 6), "Bad formatted size"

                UD = formatted[0]
                YR = formatted[1]
                FB = formatted[2]
                LR = formatted[3]
                state = formatted[4]
                beep = formatted[5]

                print "RECEIVED: UD = %f | YR = %f | FB = %f | LR = %f | status = %d | beep = %d" % (UD, YR, FB, LR, state, beep)

    except KeyboardInterrupt:
        #--------------------------------------------------------------------------------------
        # Tell the client to stop-stop-stop!
        #--------------------------------------------------------------------------------------
        output = struct.pack(pack_format, False)
        connection.send(output)

    except Exception, err:
        print err

    finally:
       connection.close()


if len(sys.argv) != 2:
    print "Select DRONE or RC"
elif sys.argv[1] == "RC":
    client()
elif sys.argv[1] == "DRONE":
    server()
else:
    print "Select RC or DRONE"

Next step is to merge this with this.

Leave a Reply

Your email address will not be published. Required fields are marked *