Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Sign in
Toggle navigation
Menu
Open sidebar
TASTE
regression-suites
Commits
19833c24
Commit
19833c24
authored
Jun 16, 2017
by
TASTE User
Browse files
Sample for communication between quadcopter and ground
parent
f670bab1
Changes
36
Expand all
Hide whitespace changes
Inline
Side-by-side
Demo_Crazyflie_Communication/DataView.aadl
0 → 100644
View file @
19833c24
This diff is collapsed.
Click to expand it.
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.
Click to expand it.
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'
)