bridge.py 11.5 KB
Newer Older
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300 301 302 303 304 305 306 307 308 309 310 311 312 313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332 333 334 335
# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2014 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  This program is free software; you can redistribute it and/or
#  modify it under the terms of the GNU General Public License
#  as published by the Free Software Foundation; either version 2
#  of the License, or (at your option) any later version.
#
#  This program is distributed in the hope that it will be useful,
#  but WITHOUT ANY WARRANTY; without even the implied warranty of
#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#  GNU General Public License for more details.
#  You should have received a copy of the GNU General Public License
#  along with this program; if not, write to the Free Software
#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
#  MA  02110-1301, USA.
"""
Simple example that connects to the first Crazyflie found, logs the Stabilizer
and prints it to the console. After 10s the application disconnects and exits.
"""
import errno
import logging
import select
import socket
import struct
import sys
import time

import cflib.crtp  # noqa
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
import ctypes

from threading import Thread

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)


class LoggingExample:
    """
    Simple logging example class that logs the Stabilizer from a supplied
    link uri and disconnects after 5s.
    """

    def __init__(self, link_uri, socket):
        """ Initialize and run the example with the specified link_uri """

        # Create a Crazyflie object without specifying any cache dirs
        self._cf = Crazyflie()

        # Connect some callbacks from the Crazyflie API
        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

        print('Connecting to %s' % link_uri)

        # Try to connect to the Crazyflie
        self._cf.open_link(link_uri)
        self.is_connected = False
        self._socket = socket
        
        # Data that we want to send via socket
        self.gyroData = []
        self.accData = []

    def _connected(self, link_uri):
        """ This callback is called form the Crazyflie API when a Crazyflie
        has been connected and the TOCs have been downloaded."""
        print('Connected to %s' % link_uri)

        self.is_connected = True
        # Unlock startup thrust protection
        self.unlock_thrust_protection()

        print("socket", self._socket)
        # The definition of the logconfig can be made before connecting
        self._lg_gyro = LogConfig(name='Gyro', period_in_ms=10)
        self._lg_gyro.add_variable('gyro.x', 'float')
        self._lg_gyro.add_variable('gyro.y', 'float')
        self._lg_gyro.add_variable('gyro.z', 'float')
        
        # The definition of the logconfig can be made before connecting
        self._lg_acc = LogConfig(name='Acc', period_in_ms=10)
        self._lg_acc.add_variable('acc.x', 'float')
        self._lg_acc.add_variable('acc.y', 'float')
        self._lg_acc.add_variable('acc.z', 'float')
        self._lg_acc.add_variable('range.zrange', 'float')

        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_gyro)
            self._cf.log.add_config(self._lg_acc)
            # This callback will receive the data
            self._lg_gyro.data_received_cb.add_callback(self._gyro_log_data)
            self._lg_acc.data_received_cb.add_callback(self._acc_log_data)
            # This callback will be called on errors
            self._lg_gyro.error_cb.add_callback(self._sensorData_log_error)
            self._lg_acc.error_cb.add_callback(self._sensorData_log_error)
            # Start the logging
            self._lg_gyro.start()
            self._lg_acc.start()

        except KeyError as e:
            print('Could not start log configuration,'
                  '{} not found in TOC'.format(str(e)))
        except AttributeError:
            print('Could not add Stabilizer log config, bad configuration.')
            
        
    def unlock_thrust_protection(self):
        self._cf.commander.send_setpoint(0, 0, 0, 0)

    def send_zrange_setpoint(self, roll=0, pitch=0, yawrate=0, zdistance=0):
        self._cf.commander.send_zdistance_setpoint(0, 0, 0, 1)

    def send_setpoint(self, roll=0, pitch=0, yawrate=0, thrust=0):
        self._cf.commander.send_setpoint(roll, pitch, yawrate, thrust)

    def _sensorData_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print('Error when logging %s: %s' % (logconf.name, msg))

    def _gyro_log_data(self, timestamp, data, logconf):
        """Callback from the log API when data arrives"""
        # print('Data : %s' % (data))
        self.gyroData.append(data)
        
    def _acc_log_data(self, timestamp, data, logconf):
        """Callback from the log API when data arrives"""
        # print('Data : %s' % (data))
        self.accData.append(data)

    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the speficied address)"""
        print('Connection to %s failed: %s' % (link_uri, msg))
        self.is_connected = False
        self._socket.close()
        print('Connexion closed')
        #sys.exit(10)

    def _connection_lost(self, link_uri, msg):
        """Callback when disconnected after a connection has been made (i.e
        Crazyflie moves out of range)"""
        print('Connection to %s lost: %s' % (link_uri, msg))
        # Unlock startup thrust protection
        self.unlock_thrust_protection()
        # Close socket
        self._socket.close()
        print('Connexion closed')
        #sys.exit(2)

    def _disconnected(self, link_uri):
        """Callback when the Crazyflie is disconnected (called in all cases)"""
        print('Disconnected from %s' % link_uri)
        self.is_connected = False
        # Unlock startup thrust protection
        self.unlock_thrust_protection()
        # Close socket
        self._socket.close()
        print('Connexion closed')
        #sys.exit(3)

    def altHold(self):
        self._cf.param.set_value("flightmode.althold","1")


# Packet receiving
def recv_packet(sock):
    
    try:
        # Read message length (unsigned int = 4 bytes)
        raw_msglen = recv_length(sock, 4)

        if not raw_msglen:
            return None

    except socket.error as msg:
        if msg.args[0] == errno.EWOULDBLOCK: 
            time.sleep(0.01)
            return None           # short delay, no tight loops
        else:
            print(msg)
            sys.exit(1)

    # And unpack it into an integer
    msglen = struct.unpack('>I', raw_msglen)[0]

    # Then read the message data
    return recv_data(sock, msglen)
  
# Get length
def recv_length(sock, n):
    # Helper function to receive n bytes or return None if EOF is hit
    data = b""
    while len(data) < n:
        
        packet = sock.recv(n - len(data))
            
        if not packet:
            return None
            
        data += packet
            
    # Return bytes
    return data

# Get bytes
def recv_data(sock, n):
    # Helper function to receive n bytes or return None if EOF is hit
    data = b""
    while len(data) < n:
        
        try:
            # Read message length (unsigned int = 4 bytes)
            packet = sock.recv(n - len(data))
            
            if not packet:
                return None
            
            data += packet
            
        except socket.error as msg:
            if msg.args[0] == errno.EWOULDBLOCK: 
                time.sleep(0.01) # short delay, no tight loops
            else:
                print(msg)
                sys.exit(1)
    
    # Return bytes
    return data

if __name__ == '__main__':
    """
    Create socket server, example https://docs.python.org/3.4/library/socket.html#socket-objects 
    """
    HOST = ''  # Symbolic name meaning all available interfaces
    PORT = 50007  # Arbitrary non-privileged port
    
    # Create socket object
    mySocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 
    print('Socket created')
    
    # Re-use same port
    mySocket.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1) 
    
    # Bind socket to local host and port
    try:
        mySocket.bind((HOST, PORT))
    except socket.error as msg:
        print('Bind failed. Error Code : ' + str(msg[0]) + ' Message ' + msg[1])
        sys.exit()
        
    print('Socket bind complete')
    
    # Initialize the low-level drivers (don't list the debug drivers)
    cflib.crtp.init_drivers(enable_debug_driver=False)

    # Initialize main class
    le = LoggingExample("radio://0/80/250K",socket= mySocket)
    
    # Start listening on socket 
    mySocket.listen(5)
    print('Socket now listening')
    
    # Wait to accept a connection - blocking call
    connexion, addr = mySocket.accept()
    print('Connected with ' + addr[0] + ':' + str(addr[1]))

    while not mySocket._closed:
        # Wait for 1 client connection
        
        if connexion:
            
            if len(le.gyroData) > 0 and len(le.accData) > 0:
                # Pop first packet and send it
                packetGyro = le.gyroData.pop(0)
                packetAcc = le.accData.pop(0)

                print('Data gyro : %s' % (packetGyro))
                print('Data acc  : %s' % (packetAcc))

                # Serialize packet data
                serialiazed_packet = struct.pack('>7f', 
                                                 packetGyro['gyro.x'],
                                                 packetGyro['gyro.y'],
                                                 packetGyro['gyro.z'],
                                                 packetAcc['acc.x'],
                                                 packetAcc['acc.y'],
                                                 packetAcc['acc.z'],
                                                 packetAcc['range.zrange'])
                
                # Simple protocol : packet length followed by the data bytes
                connexion.sendall(struct.pack('>I', len(serialiazed_packet)))

                # Then send data
                connexion.sendall(serialiazed_packet)
                #print('Data sent : %s' % (str(serialiazed_packet)))


            # Receiving packets
            packet = recv_packet(connexion)
            if packet:
                deserialized_packet = struct.unpack('<fffH', packet)
                
                # Extract commands to send
                roll = deserialized_packet[0]
                pitch = deserialized_packet[1]
                yawrate = deserialized_packet[2]
                thrust = deserialized_packet[3]
                
                print('Data received : %s' % (str(deserialized_packet)))

                # Send commands to quadcopter
                #if thrust > 0 and le.is_connected:
                #    le.send_setpoint(roll, pitch, yawrate, thrust)

    connexion.close()
    print('Connexion closed')

    sys.exit(2)