Skip to content

GitLab

Commit 19833c24 authored by TASTE User's avatar TASTE User
Browse files

Sample for communication between quadcopter and ground

parent f670bab1
Expand all Hide whitespace changes
Inline Side-by-side
Demo_Crazyflie_Communication/DataView.aadl 0 → 100644
View file @ 19833c24
This diff is collapsed.
Demo_Crazyflie_Communication/DataView.asn 0 → 100644
View file @ 19833c24
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 0 → 100644
View file @ 19833c24
---------------------------------------------------
-- 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 0 → 100644
View file @ 19833c24
---------------------------------------------------
-- 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 0 → 100644
View file @ 19833c24
4ee6d9fccbe5005c3133226797bdf1a3 InterfaceView.aadl
Demo_Crazyflie_Communication/bench.vcd 0 → 100644
View file @ 19833c24
This diff is collapsed.
Demo_Crazyflie_Communication/bridge.py 0 → 100644
View file @ 19833c24
# -*- 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()