Sample for communication between quadcopter and ground

Demo_Crazyflie_Communication/DataView.asn

+TASTE-Dataview DEFINITIONS ::=
+BEGIN
+IMPORTS T-Int32, T-UInt32, T-Int8, T-UInt8, T-Boolean FROM TASTE-BasicTypes;
+
+-- A few simple types to start with ASN.1
+MyInteger   ::= T-UInt8
+
+MyReal      ::= REAL (0.0 .. 1000.0)
+
+MyBool      ::= BOOLEAN
+
+MyEnum      ::= ENUMERATED { hello, world, howareyou }
+
+MySeq       ::= SEQUENCE {
+    input-data  MyInteger,
+    output-data MyInteger,
+    validity    ENUMERATED { valid, invalid }
+}
+
+MyChoice    ::= CHOICE {
+    a BOOLEAN,
+    b MySeq
+}
+
+MySeqOf     ::= SEQUENCE (SIZE (2)) OF MyEnum
+
+MyOctStr    ::= OCTET STRING (SIZE (3))
+
+-- You can also declare constants
+myVar       MySeqOf     ::= { hello, world }
+
+-- Packet structure
+ANGLE         ::= REAL(-360.0 .. 360.0)
+RATE          ::= REAL(-100000.0 .. 100000.0)
+T-UInt16      ::= INTEGER (0 .. 65535)
+
+GYROSCOPE     ::= REAL(-100000.0 .. 100000.0)
+ACCELERATION  ::= REAL(-16.0 .. 16.0)
+HEIGHT        ::= INTEGER (0 .. 8191)
+
+GYRO-SEQ ::= SEQUENCE {
+       x GYROSCOPE,
+       y GYROSCOPE,
+       z GYROSCOPE
+}
+
+ACC-SEQ ::= SEQUENCE {
+       x ACCELERATION,
+       y ACCELERATION,
+       z ACCELERATION
+}
+
+TM-T ::= SEQUENCE {
+    gyro  GYRO-SEQ,
+    acc   ACC-SEQ,
+    z     HEIGHT
+}
+
+TC-T ::= SEQUENCE {
+    roll   ANGLE,
+    pitch  ANGLE,
+    yaw    RATE,
+    thrust T-UInt16
+}
+
+sampleMeasurement TM-T ::= { gyro { x 0.0, y 0.0, z 0.0 }, acc { x 0.0, y 0.0, z 1.0 }, z 0 }
+
+sampleCommand TC-T ::= { roll 0.0, pitch 0.0, yaw 0.0, thrust 0 }
+
+END
+

Demo_Crazyflie_Communication/DeploymentView.aadl

+---------------------------------------------------
+-- AADL2.0
+-- TASTE
+-- 
+-- 
+---------------------------------------------------
+
+PACKAGE deploymentview::DV::Node1
+PUBLIC
+
+WITH Taste;
+WITH Deployment;
+WITH TASTE_DV_Properties;
+PROCESS x86_partition
+END x86_partition;
+
+PROCESS IMPLEMENTATION x86_partition.others
+END x86_partition.others;
+
+END deploymentview::DV::Node1;
+
+PACKAGE deploymentview::DV
+PUBLIC
+
+WITH ocarina_processors_x86;
+WITH deploymentview::DV::Node1;
+WITH Taste;
+WITH Deployment;
+WITH interfaceview::IV;
+WITH TASTE_DV_Properties;
+SYSTEM Node1
+END Node1;
+
+SYSTEM IMPLEMENTATION Node1.others
+SUBCOMPONENTS
+  IV_Function1 : SYSTEM interfaceview::IV::Function1.others {
+    Taste::coordinates => "826 959 1298 1421";
+    Taste::FunctionName => "Function1";
+  };
+  IV_Function2 : SYSTEM interfaceview::IV::Function2.others {
+    Taste::coordinates => "1441 969 1949 1395";
+    Taste::FunctionName => "Function2";
+  };
+  x86_partition : PROCESS deploymentview::DV::Node1::x86_partition.others {
+    Taste::coordinates => "676 764 2186 1556";
+    Deployment::Port_Number => 0;
+  };
+  x86_linux32 : PROCESSOR ocarina_processors_x86::x86.linux32 {
+    Taste::coordinates => "487 538 2375 1669";
+  };
+PROPERTIES
+  Taste::APLC_Binding => (reference (x86_partition)) APPLIES TO IV_Function1;
+  Taste::APLC_Binding => (reference (x86_partition)) APPLIES TO IV_Function2;
+  Actual_Processor_Binding => (reference (x86_linux32)) APPLIES TO x86_partition;
+END Node1.others;
+
+SYSTEM deploymentview
+END deploymentview;
+
+SYSTEM IMPLEMENTATION deploymentview.others
+SUBCOMPONENTS
+  Node1 : SYSTEM Node1.others {
+    Taste::coordinates => "251 215 2611 1831";
+  };
+  interfaceview : SYSTEM interfaceview::IV::interfaceview.others;
+END deploymentview.others;
+
+PROPERTIES
+  Taste::coordinates => "0 0 2970 2100";
+  Taste::version => "1.3.19";
+  Taste::interfaceView => "InterfaceView.aadl";
+  Taste::HWLibraries => ("../../tool-inst/share/ocarina/AADLv2/ocarina_components.aadl");
+END deploymentview::DV;
+

Demo_Crazyflie_Communication/InterfaceView.aadl

+---------------------------------------------------
+-- AADL2.0
+-- TASTE
+-- 
+-- 
+---------------------------------------------------
+
+PACKAGE interfaceview::FV::Function1
+PUBLIC
+
+WITH Taste;
+WITH DataView;
+WITH TASTE_IV_Properties;
+SUBPROGRAM PI_tick
+END PI_tick;
+
+SUBPROGRAM IMPLEMENTATION PI_tick.others
+END PI_tick.others;
+
+SUBPROGRAM PI_TM_from_CF
+FEATURES
+  tm_data : IN PARAMETER DataView::TM_T {
+    Taste::encoding => NATIVE;
+  };
+PROPERTIES
+  Taste::Associated_Queue_Size => 1;
+END PI_TM_from_CF;
+
+SUBPROGRAM IMPLEMENTATION PI_TM_from_CF.others
+END PI_TM_from_CF.others;
+
+SUBPROGRAM RI_TC_from_TASTE
+FEATURES
+  tc_data : IN PARAMETER DataView::TC_T {
+    Taste::encoding => NATIVE;
+  };
+END RI_TC_from_TASTE;
+
+SUBPROGRAM IMPLEMENTATION RI_TC_from_TASTE.others
+END RI_TC_from_TASTE.others;
+
+END interfaceview::FV::Function1;
+
+PACKAGE interfaceview::FV::Function2
+PUBLIC
+
+WITH Taste;
+WITH DataView;
+WITH TASTE_IV_Properties;
+SUBPROGRAM PI_TC_from_TASTE
+FEATURES
+  tc_data : IN PARAMETER DataView::TC_T {
+    Taste::encoding => NATIVE;
+  };
+PROPERTIES
+  Taste::Associated_Queue_Size => 1;
+END PI_TC_from_TASTE;
+
+SUBPROGRAM IMPLEMENTATION PI_TC_from_TASTE.others
+END PI_TC_from_TASTE.others;
+
+SUBPROGRAM RI_TM_from_CF
+FEATURES
+  tm_data : IN PARAMETER DataView::TM_T {
+    Taste::encoding => NATIVE;
+  };
+END RI_TM_from_CF;
+
+SUBPROGRAM IMPLEMENTATION RI_TM_from_CF.others
+END RI_TM_from_CF.others;
+
+END interfaceview::FV::Function2;
+
+PACKAGE interfaceview::IV
+PUBLIC
+
+WITH interfaceview::FV::Function1;
+WITH interfaceview::FV::Function2;
+WITH interfaceview::FV;
+WITH Taste;
+WITH DataView;
+WITH TASTE_IV_Properties;
+SYSTEM Function1
+FEATURES
+  PI_tick : PROVIDES SUBPROGRAM ACCESS interfaceview::FV::Function1::PI_tick.others {
+    Taste::coordinates => "939 344";
+    Taste::RCMoperationKind => cyclic;
+    Taste::RCMperiod => 100 ms;
+    Taste::Deadline => 0 ms;
+    Taste::InterfaceName => "tick";
+  };
+  PI_TM_from_CF : PROVIDES SUBPROGRAM ACCESS interfaceview::FV::Function1::PI_TM_from_CF.others {
+    Taste::coordinates => "1390 1185";
+    Taste::RCMoperationKind => sporadic;
+    Taste::RCMperiod => 0 ms;
+    Taste::Deadline => 999 ms;
+    Taste::InterfaceName => "TM_from_CF";
+  };
+  RI_TC_from_TASTE : REQUIRES SUBPROGRAM ACCESS interfaceview::FV::Function2::PI_TC_from_TASTE.others {
+    Taste::coordinates => "1390 657";
+    Taste::RCMoperationKind => any;
+    Taste::InterfaceName => "TC_from_TASTE";
+    Taste::labelInheritance => "true";
+  };
+PROPERTIES
+  Source_Language => (C);
+END Function1;
+
+SYSTEM IMPLEMENTATION Function1.others
+SUBCOMPONENTS
+  tick_impl : SUBPROGRAM interfaceview::FV::Function1::PI_tick.others {
+    Compute_Execution_Time => 0 ms .. 0 ms;
+  };
+  TM_from_CF_impl : SUBPROGRAM interfaceview::FV::Function1::PI_TM_from_CF.others {
+    Compute_Execution_Time => 0 ms .. 999 ms;
+  };
+CONNECTIONS
+  OpToPICnx_tick_impl : SUBPROGRAM ACCESS tick_impl -> PI_tick;
+  OpToPICnx_TM_from_CF_impl : SUBPROGRAM ACCESS TM_from_CF_impl -> PI_TM_from_CF;
+END Function1.others;
+
+SYSTEM Function2
+FEATURES
+  PI_TC_from_TASTE : PROVIDES SUBPROGRAM ACCESS interfaceview::FV::Function2::PI_TC_from_TASTE.others {
+    Taste::coordinates => "1888 687";
+    Taste::RCMoperationKind => sporadic;
+    Taste::RCMperiod => 0 ms;
+    Taste::Deadline => 0 ms;
+    Taste::InterfaceName => "TC_from_TASTE";
+  };
+  RI_TM_from_CF : REQUIRES SUBPROGRAM ACCESS interfaceview::FV::Function1::PI_TM_from_CF.others {
+    Taste::coordinates => "1888 1149";
+    Taste::RCMoperationKind => any;
+    Taste::InterfaceName => "TM_from_CF";
+    Taste::labelInheritance => "true";
+  };
+PROPERTIES
+  Source_Language => (GUI);
+END Function2;
+
+SYSTEM IMPLEMENTATION Function2.others
+SUBCOMPONENTS
+  TC_from_TASTE_impl : SUBPROGRAM interfaceview::FV::Function2::PI_TC_from_TASTE.others {
+    Compute_Execution_Time => 0 ms .. 0 ms;
+  };
+CONNECTIONS
+  OpToPICnx_TC_from_TASTE_impl : SUBPROGRAM ACCESS TC_from_TASTE_impl -> PI_TC_from_TASTE;
+END Function2.others;
+
+SYSTEM interfaceview
+END interfaceview;
+
+SYSTEM IMPLEMENTATION interfaceview.others
+SUBCOMPONENTS
+  Function1 : SYSTEM interfaceview::IV::Function1.others {
+    Taste::coordinates => "441 344 1390 1580";
+  };
+  Function2 : SYSTEM interfaceview::IV::Function2.others {
+    Taste::coordinates => "1888 364 2698 1585";
+  };
+CONNECTIONS
+  Function1_TM_from_CF_Function2_TM_from_CF : SUBPROGRAM ACCESS Function1.PI_TM_from_CF -> Function2.RI_TM_from_CF {
+    Taste::coordinates => "1888 1149 1636 1149 1636 1185 1390 1185";
+  };
+  Function2_TC_from_TASTE_Function1_TC_from_TASTE : SUBPROGRAM ACCESS Function2.PI_TC_from_TASTE -> Function1.RI_TC_from_TASTE {
+    Taste::coordinates => "1390 657 1636 657 1636 687 1888 687";
+  };
+END interfaceview.others;
+
+PROPERTIES
+  Taste::dataView => ("DataView");
+  Taste::dataViewPath => ("DataView.aadl");
+  Taste::coordinates => "0 0 2970 2100";
+  Taste::version => "1.3.19";
+END interfaceview::IV;
+
+PACKAGE interfaceview::FV
+PUBLIC
+
+WITH Taste;
+WITH DataView;
+WITH TASTE_IV_Properties;
+END interfaceview::FV;
+

Demo_Crazyflie_Communication/InterfaceView.md5

+4ee6d9fccbe5005c3133226797bdf1a3  InterfaceView.aadl

Demo_Crazyflie_Communication/bridge.py

+# -*- 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))