diff --git a/Tools/Matlab/motors.m b/Tools/Matlab/motors.m deleted file mode 100644 index 6d688a3077..0000000000 --- a/Tools/Matlab/motors.m +++ /dev/null @@ -1,58 +0,0 @@ -clear all; -close all; - -% Measurement data -% 1045 propeller -% Robbe Roxxy Motor (1100 kV, data collected in 2010) -data = [ 45, 7.4;... - 38, 5.6;... - 33, 4.3;... - 26, 3.0;... - 18, 2.0;... - 10, 1.0 ]; - -% Normalize the data, as we're operating later -% anyways in normalized units -data(:,1) = data(:,1) ./ max(data(:,1)); -data(:,2) = data(:,2) ./ max(data(:,2)); - -% Fit a 2nd degree polygon to the data and -% print the x2, x1, x0 coefficients -p = polyfit(data(:,2), data(:,1),2) - -% Override the first coffefficient for testing -% purposes -pf = 0.62; - -% Generate plotting data -px1 = linspace(0, max(data(:,2))); -py1 = polyval(p, px1); - -pyt = zeros(size(data, 1), 1); -corr = zeros(size(data, 1), 1); - -% Actual code test -% the two lines below are the ones needed to be ported to C: -% pf: Power factor parameter. -% px1(i): The current normalized motor command (-1..1) -% corr(i): The required correction. The motor speed is: -% px1(i) -for i=1:size(px1, 2) - - % The actual output throttle - pyt(i) = -pf * (px1(i) * px1(i)) + (1 + pf) * px1(i); - - % Solve for input throttle - % y = -p * x^2 + (1+p) * x; - % -end - -plot(data(:,2), data(:,1), '*r'); -hold on; -plot(px1, py1, '*b'); -hold on; -plot([0 px1(end)], [0 py1(end)], '-k'); -hold on; -plot(px1, pyt, '-b'); -hold on; -plot(px1, corr, '-m'); diff --git a/Tools/Matlab/plot_mag.m b/Tools/Matlab/plot_mag.m deleted file mode 100644 index c9f0c29925..0000000000 --- a/Tools/Matlab/plot_mag.m +++ /dev/null @@ -1,77 +0,0 @@ -% -% Tool for plotting mag data -% -% Reference values: -% telem> [cal] mag #0 off: x:0.15 y:0.07 z:0.14 Ga -% MATLAB: x:0.1581 y: 0.0701 z: 0.1439 Ga -% telem> [cal] mag #0 scale: x:1.10 y:0.97 z:1.02 -% MATLAB: 0.5499, 0.5190, 0.4907 -% -% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga -% MATLAB: x:-0.1827 y:0.1147 z:-0.0848 Ga -% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 -% MATLAB: 0.5122, 0.5065, 0.4915 -% -% -% User-guided values: -% -% telem> [cal] mag #0 off: x:0.12 y:0.09 z:0.14 Ga -% telem> [cal] mag #0 scale: x:0.88 y:0.99 z:0.95 -% telem> [cal] mag #1 off: x:-0.18 y:0.11 z:-0.09 Ga -% telem> [cal] mag #1 scale: x:1.00 y:1.00 z:1.00 - -close all; -clear all; - -plot_scale = 0.8; - -xmax = plot_scale; -xmin = -xmax; -ymax = plot_scale; -ymin = -ymax; -zmax = plot_scale; -zmin = -zmax; - -mag0_raw = load('../../mag0_raw3.csv'); -mag1_raw = load('../../mag1_raw3.csv'); - -mag0_cal = load('../../mag0_cal3.csv'); -mag1_cal = load('../../mag1_cal3.csv'); - -fm0r = figure(); - -mag0_x_scale = 0.88; -mag0_y_scale = 0.99; -mag0_z_scale = 0.95; - -plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); -[mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); -mag0_raw_center -mag0_raw_radii -axis([xmin xmax ymin ymax zmin zmax]) -viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]); - -fm1r = figure(); -plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); -[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); -center -radii -axis([xmin xmax ymin ymax zmin zmax]) - -fm0c = figure(); -plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); -[mag0_cal_center, mag0_cal_radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) .* mag0_x_scale mag1_raw(:,2) .* mag0_y_scale mag1_raw(:,3) .* mag0_z_scale] ); -mag0_cal_center -mag0_cal_radii -axis([xmin xmax ymin ymax zmin zmax]) -viscircles([0, 0], [mag0_cal_radii(3)]); - -fm1c = figure(); -plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); -axis([xmin xmax ymin ymax zmin zmax]) -[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); -viscircles([0, 0], [radii(3)]); - -mag0_x_scale_matlab = 1 / (mag0_cal_radii(1) / mag0_raw_radii(1)) -mag0_y_scale_matlab = 1 / (mag0_cal_radii(2) / mag0_raw_radii(2)) -mag0_z_scale_matlab = 1 / (mag0_cal_radii(3) / mag0_raw_radii(3)) diff --git a/Tools/dist/vehicle_configs.xml b/Tools/dist/vehicle_configs.xml deleted file mode 100644 index 76d7ba11eb..0000000000 --- a/Tools/dist/vehicle_configs.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - Standard 8" Prop Quadrotor (x) - Standard quadrotor configuration in x configuration for 8-" propellers - /etc/mixers/quad_x.main.mix - - - Standard 8" Prop Quadrotor (+) - Standard quadrotor configuration in + configuration for 8-" propellers - /etc/mixers/quad_+.main.mix - - - Standard 8" Prop Quadrotor (x) - Standard quadrotor configuration in x configuration for 8-" propellers - /etc/mixers/quad_x.main.mix - - - Zeta Science Wing Wing Z-84 - Configuration for a small flying wing. - /etc/mixers/wingwing.main.mix - - diff --git a/Tools/models/sdp3x_pitot_model.py b/Tools/models/sdp3x_pitot_model.py deleted file mode 100644 index 864823d663..0000000000 --- a/Tools/models/sdp3x_pitot_model.py +++ /dev/null @@ -1,110 +0,0 @@ -""" -Copyright (c) 2017, Sensirion AG -All rights reserved. - -Redistribution and use in source and binary forms, with or without -modification, are permitted provided that the following conditions are met: - -* Redistributions of source code must retain the above copyright notice, this - list of conditions and the following disclaimer. - -* Redistributions in binary form must reproduce the above copyright notice, - this list of conditions and the following disclaimer in the documentation - and/or other materials provided with the distribution. - -* Neither the name of Sensirion AG nor the names of its - contributors may be used to endorse or promote products derived from - this software without specific prior written permission. - -THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE -DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE -FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL -DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR -SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER -CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, -OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE -OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. -""" - - - -# formula for metal pitot tube with round tip as here: https://drotek.com/shop/2986-large_default/sdp3x-airspeed-sensor-kit-sdp31.jpg -# and tubing as provided by px4/drotek (1.5 mm diameter) - - - -import numpy as np -import matplotlib.pyplot as plt - -P_cal=96600. #Pa -P_amb=96600. #dummy-value, use absolute pressure sensor!! - - -## differential pressure, sensor values in Pascal -dp_SDP33_raw=np.linspace(0,80,100) - - - - -dp_SDP33=dp_SDP33_raw*P_cal/P_amb - - -## total length tube in mm = length dynamic port+ length static port; compensation only valid for inner diameter of 1.5mm -l_tube=450 - -## densitiy air in kg/m3 -rho_air=1.29 - - -## flow through sensor -flow_SDP33=(300.805 - 300.878/(0.00344205*dp_SDP33**0.68698 + 1))*1.29/rho_air - - - -## additional dp through pitot tube -dp_Pitot=(0.0032*flow_SDP33**2 + 0.0123*flow_SDP33+1.)*1.29/rho_air - -## pressure drop through tube -dp_Tube=(flow_SDP33*0.674)/450*l_tube*rho_air/1.29 - -## speed at pitot-tube tip due to flow through sensor -dv=0.125*flow_SDP33 - -## sum of all pressure drops -dp_tot=dp_SDP33+dp_Tube+dp_Pitot - - -## computed airspeed without correction for inflow-speed at tip of pitot-tube -airspeed_uncorrected=np.sqrt(2*dp_tot/rho_air) - -## corrected airspeed -airspeed_corrected=airspeed_uncorrected+dv - - -## just to compare to value without compensation -airspeed_raw=np.sqrt(2*dp_SDP33/rho_air) - - -plt.figure() -plt.plot(dp_SDP33,airspeed_corrected) -plt.xlabel('differential pressure raw value [Pa]') -plt.ylabel('airspeed_corrected [m/s]') -plt.show() - - - -##plt.figure() -##plt.plot(dp_SDP33,airspeed_corrected/airspeed_raw) -##plt.xlabel('differential pressure raw value [Pa]') -##plt.ylabel('correction factor [-]') -##plt.show() -## -## -## -##plt.figure() -##plt.plot(airspeed_corrected,(airspeed_corrected-airspeed_raw)/airspeed_corrected) -##plt.xlabel('airspeed [m/s]') -##plt.ylabel('relative error [-]') -##plt.show() diff --git a/Tools/stack_usage/avstack.pl b/Tools/stack_usage/avstack.pl deleted file mode 100755 index 5af499b145..0000000000 --- a/Tools/stack_usage/avstack.pl +++ /dev/null @@ -1,251 +0,0 @@ -#!/usr/bin/perl -w -# avstack.pl: AVR stack checker -# Copyright (C) 2013 Daniel Beer -# -# Permission to use, copy, modify, and/or distribute this software for -# any purpose with or without fee is hereby granted, provided that the -# above copyright notice and this permission notice appear in all -# copies. -# -# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL -# WARRANTIES WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED -# WARRANTIES OF MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE -# AUTHOR BE LIABLE FOR ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL -# DAMAGES OR ANY DAMAGES WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR -# PROFITS, WHETHER IN AN ACTION OF CONTRACT, NEGLIGENCE OR OTHER -# TORTIOUS ACTION, ARISING OUT OF OR IN CONNECTION WITH THE USE OR -# PERFORMANCE OF THIS SOFTWARE. -# -# Usage -# ----- -# -# This script requires that you compile your code with -fstack-usage. -# This results in GCC generating a .su file for each .o file. Once you -# have these, do: -# -# ./avstack.pl -# -# This will disassemble .o files to construct a call graph, and read -# frame size information from .su. The call graph is traced to find, for -# each function: -# -# - Call height: the maximum call height of any callee, plus 1 -# (defined to be 1 for any function which has no callees). -# -# - Inherited frame: the maximum *inherited* frame of any callee, plus -# the GCC-calculated frame size of the function in question. -# -# Using these two pieces of information, we calculate a cost (estimated -# peak stack usage) for calling the function. Functions are then listed -# on stdout in decreasing order of cost. -# -# Functions which are recursive are marked with an 'R' to the left of -# them. Their cost is calculated for a single level of recursion. -# -# The peak stack usage of your entire program can usually be estimated -# as the stack cost of "main", plus the maximum stack cost of any -# interrupt handler which might execute. - -use strict; - -# Configuration: set these as appropriate for your architecture/project. - -my $objdump = "arm-none-eabi-objdump"; -my $call_cost = 4; - -# First, we need to read all object and corresponding .su files. We're -# gathering a mapping of functions to callees and functions to frame -# sizes. We're just parsing at this stage -- callee name resolution -# comes later. - -my %frame_size; # "func@file" -> size -my %call_graph; # "func@file" -> {callees} -my %addresses; # "addr@file" -> "func@file" - -my %global_name; # "func" -> "func@file" -my %ambiguous; # "func" -> 1 - -foreach (@ARGV) { - # Disassemble this object file to obtain a callees. Sources in the - # call graph are named "func@file". Targets in the call graph are - # named either "offset@file" or "funcname". We also keep a list of - # the addresses and names of each function we encounter. - my $objfile = $_; - my $source; - - open(DISASSEMBLY, "$objdump -dr $objfile|") || - die "Can't disassemble $objfile"; - while () { - chomp; - - if (/^([0-9a-fA-F]+) <(.*)>:/) { - my $a = $1; - my $name = $2; - - $source = "$name\@$objfile"; - $call_graph{$source} = {}; - $ambiguous{$name} = 1 if defined($global_name{$name}); - $global_name{$name} = "$name\@$objfile"; - - $a =~ s/^0*//; - $addresses{"$a\@$objfile"} = "$name\@$objfile"; - } - - if (/: R_[A-Za-z0-9_]+_CALL[ \t]+(.*)/) { - my $t = $1; - - if ($t eq ".text") { - $t = "\@$objfile"; - } elsif ($t =~ /^\.text\+0x(.*)$/) { - $t = "$1\@$objfile"; - } - - $call_graph{$source}->{$t} = 1; - } - } - close(DISASSEMBLY); - - # Extract frame sizes from the corresponding .su file. - if ($objfile =~ /^(.*).obj$/) { - my $sufile = "$1.su"; - - open(SUFILE, "<$sufile") || die "Can't open $sufile"; - while () { - $frame_size{"$1\@$objfile"} = $2 + $call_cost - if /^.*:([^\t ]+)[ \t]+([0-9]+)/; - } - close(SUFILE); - } -} - -# In this step, we enumerate each list of callees in the call graph and -# try to resolve the symbols. We omit ones we can't resolve, but keep a -# set of them anyway. - -my %unresolved; - -foreach (keys %call_graph) { - my $from = $_; - my $callees = $call_graph{$from}; - my %resolved; - - foreach (keys %$callees) { - my $t = $_; - - if (defined($addresses{$t})) { - $resolved{$addresses{$t}} = 1; - } elsif (defined($global_name{$t})) { - $resolved{$global_name{$t}} = 1; - warn "Ambiguous resolution: $t" if defined ($ambiguous{$t}); - } elsif (defined($call_graph{$t})) { - $resolved{$t} = 1; - } else { - $unresolved{$t} = 1; - } - } - - $call_graph{$from} = \%resolved; -} - -# Create fake edges and nodes to account for dynamic behaviour. -$call_graph{"INTERRUPT"} = {}; - -foreach (keys %call_graph) { - $call_graph{"INTERRUPT"}->{$_} = 1 if /^__vector_/; -} - -# Trace the call graph and calculate, for each function: -# -# - inherited frames: maximum inherited frame of callees, plus own -# frame size. -# - height: maximum height of callees, plus one. -# - recursion: is the function called recursively (including indirect -# recursion)? - -my %has_caller; -my %visited; -my %total_cost; -my %call_depth; - -sub trace { - my $f = shift; - - if ($visited{$f}) { - $visited{$f} = "R" if $visited{$f} eq "?"; - return; - } - - $visited{$f} = "?"; - - my $max_depth = 0; - my $max_frame = 0; - - my $targets = $call_graph{$f} || die "Unknown function: $f"; - if (defined($targets)) { - foreach (keys %$targets) { - my $t = $_; - - $has_caller{$t} = 1; - trace($t); - - my $is = $total_cost{$t}; - my $d = $call_depth{$t}; - - $max_frame = $is if $is > $max_frame; - $max_depth = $d if $d > $max_depth; - } - } - - $call_depth{$f} = $max_depth + 1; - $total_cost{$f} = $max_frame + ($frame_size{$f} || 0); - $visited{$f} = " " if $visited{$f} eq "?"; -} - -foreach (keys %call_graph) { trace $_; } - -# Now, print results in a nice table. -printf " %-30s %8s %8s %8s\n", - "Func", "Cost", "Frame", "Height"; -print "------------------------------------"; -print "------------------------------------\n"; - -my $max_iv = 0; -my $main = 0; - -foreach (sort { $total_cost{$b} <=> $total_cost{$a} } keys %visited) { - my $name = $_; - - if (/^(.*)@(.*)$/) { - $name = $1 unless $ambiguous{$name}; - } - - my $tag = $visited{$_}; - my $cost = $total_cost{$_}; - - $name = $_ if $ambiguous{$name}; - $tag = ">" unless $has_caller{$_}; - - if (/^__vector_/) { - $max_iv = $cost if $cost > $max_iv; - } elsif (/^main@/) { - $main = $cost; - } - - if ($ambiguous{$name}) { $name = $_; } - - printf "%s %-30s %8d %8d %8d\n", $tag, $name, $cost, - $frame_size{$_} || 0, $call_depth{$_}; -} - -print "\n"; - -print "Peak execution estimate (main + worst-case IV):\n"; -printf " main = %d, worst IV = %d, total = %d\n", - $total_cost{$global_name{"main"}}, - $total_cost{"INTERRUPT"}, - $total_cost{$global_name{"main"}} + $total_cost{"INTERRUPT"}; - -print "\n"; - -print "The following functions were not resolved:\n"; -foreach (keys %unresolved) { print " $_\n"; } diff --git a/Tools/usb_serialload.py b/Tools/usb_serialload.py deleted file mode 100755 index 5c864532f7..0000000000 --- a/Tools/usb_serialload.py +++ /dev/null @@ -1,55 +0,0 @@ -import serial, time - - -port = serial.Serial('/dev/ttyACM0', baudrate=57600, timeout=2) - -data = '01234567890123456789012345678901234567890123456789' -#data = 'hellohello' -outLine = 'echo %s\n' % data - -port.write('\n\n\n') -port.write('free\n') -line = port.readline(80) -while line != '': - print(line) - line = port.readline(80) - - -i = 0 -bytesOut = 0 -bytesIn = 0 - -startTime = time.time() -lastPrint = startTime -while True: - bytesOut += port.write(outLine) - line = port.readline(80) - bytesIn += len(line) - # check command line echo - if (data not in line): - print('command error %d: %s' % (i,line)) - #break - # read echo output - line = port.readline(80) - if (data not in line): - print('echo output error %d: %s' % (i,line)) - #break - bytesIn += len(line) - #print('%d: %s' % (i,line)) - #print('%d: bytesOut: %d, bytesIn: %d' % (i, bytesOut, bytesIn)) - - elapsedT = time.time() - lastPrint - if (time.time() - lastPrint >= 5): - outRate = bytesOut / elapsedT - inRate = bytesIn / elapsedT - usbRate = (bytesOut + bytesIn) / elapsedT - lastPrint = time.time() - print('elapsed time: %f' % (time.time() - startTime)) - print('data rates (bytes/sec): out: %f, in: %f, total: %f' % (outRate, inRate, usbRate)) - - bytesOut = 0 - bytesIn = 0 - - i += 1 - #if (i > 2): break - diff --git a/integrationtests/python_src/px4_it/dronekit/MissionCheck.py b/integrationtests/python_src/px4_it/dronekit/MissionCheck.py deleted file mode 100755 index cf01f097ea..0000000000 --- a/integrationtests/python_src/px4_it/dronekit/MissionCheck.py +++ /dev/null @@ -1,229 +0,0 @@ -#!/usr/bin/env python - -################################################################################################ -# @File MissionCheck.py -# Automated mission loading, execution and monitoring -# for Continuous Integration -# -# @author Sander Smeets -# -# Code partly based on DroneKit (c) Copyright 2015-2016, 3D Robotics. -################################################################################################ - - -################################################################################################ -# Settings -################################################################################################ - -from __future__ import print_function -connection_string = '127.0.0.1:14540' - -import_mission_filename = 'VTOL_TAKEOFF.mission' -max_execution_time = 200 -alt_acceptance_radius = 5 - -################################################################################################ -# Init -################################################################################################ - -# Import DroneKit-Python -from dronekit import connect, Command, VehicleMode -from pymavlink import mavutil -import time, sys, argparse, json - -parser = argparse.ArgumentParser() -parser.add_argument("-c", "--connect", help="connection string") -parser.add_argument("-f", "--filename", help="mission filename") -parser.add_argument("-t", "--timeout", help="execution timeout", type=float) -parser.add_argument("-a", "--altrad", help="altitude acceptance radius", type=float) -args = parser.parse_args() - -if args.connect: - connection_string = args.connect -if args.filename: - import_mission_filename = args.filename -if args.timeout: - max_execution_time = args.timeout -if args.altrad: - alt_acceptance_radius = args.altrad - - - -mission_failed = False -MAV_MODE_AUTO = 4 - -# start time counter -start_time = time.time() -elapsed_time = time.time() - start_time - - - -# Connect to the Vehicle -print("Connecting") -vehicle = connect(connection_string, wait_ready=True) - -while not vehicle.system_status.state == "STANDBY" or vehicle.gps_0.fix_type < 3: - if time.time() - start_time > 20: - print("FAILED: SITL did not reach standby with GPS fix within 20 seconds") - sys.exit(98) - print("Waiting for vehicle to initialise... %s " % vehicle.system_status.state) - time.sleep(1) - -# Display basic vehicle state -print(" Type: %s" % vehicle._vehicle_type) -print(" Armed?: %s" % vehicle.armed) -print(" System status: %s" % vehicle.system_status.state) -print(" GPS: %s" % vehicle.gps_0) -print(" Alt: %s" % vehicle.location.global_relative_frame.alt) - - -################################################################################################ -# Functions -################################################################################################ - -def read_mission_json(f): - d = json.load(f) - current = True - missionlist=[] - for wp in d['items']: - cmd = Command( 0, 0, 0, int(wp['frame']), int(wp['command']), current, int(wp['autoContinue']), float(wp['param1']), float(wp['param2']), float(wp['param3']), float(wp['param4']), float(wp['coordinate'][0]), float(wp['coordinate'][1]), float(wp['coordinate'][2])) - missionlist.append(cmd) - if current: - current = False - return missionlist - - -def upload_mission(aFileName): - """ - Upload a mission from a file. - """ - #Read mission from file - with open(aFileName) as f: - missionlist = read_mission_json(f) - - #Clear existing mission from vehicle - cmds = vehicle.commands - cmds.clear() - #Add new mission to vehicle - for command in missionlist: - cmds.add(command) - print(' Uploaded mission with %s items' % len(missionlist)) - vehicle.commands.upload() - return missionlist - - - -################################################################################################ -# Listeners -################################################################################################ - -current_sequence = -1 -current_sequence_changed = False -current_landed_state = -1 -home_position_set = False - -#Create a message listener for mission sequence number -@vehicle.on_message('MISSION_CURRENT') -def listener(self, name, mission_current): - global current_sequence, current_sequence_changed - if (current_sequence != mission_current.seq): - current_sequence = mission_current.seq; - current_sequence_changed = True - print('current mission sequence: %s' % mission_current.seq) - -#Create a message listener for mission sequence number -@vehicle.on_message('EXTENDED_SYS_STATE') -def listener(self, name, extended_sys_state): - global current_landed_state - if (current_landed_state != extended_sys_state.landed_state): - current_landed_state = extended_sys_state.landed_state; - -#Create a message listener for home position fix -@vehicle.on_message('HOME_POSITION') -def listener(self, name, home_position): - global home_position_set - home_position_set = True - - - -################################################################################################ -# Start mission test -################################################################################################ - - -while not home_position_set: - if time.time() - start_time > 30: - print("FAILED: getting home position 30 seconds") - sys.exit(98) - print("Waiting for home position...") - time.sleep(1) - - -#Upload mission from file -missionlist = upload_mission(import_mission_filename) -time.sleep(2) - -# set mission mode -vehicle.mode = VehicleMode("MISSION") -time.sleep(1) - - -# Arm vehicle -vehicle.armed = True - -while not vehicle.system_status.state == "ACTIVE": - if time.time() - start_time > 30: - print("FAILED: vehicle did not arm within 30 seconds") - sys.exit(98) - print("Waiting for vehicle to arm...") - time.sleep(1) - - - -# Wait for completion of mission items -while (current_sequence < len(missionlist)-1 and elapsed_time < max_execution_time): - time.sleep(.2) - if current_sequence > 0 and current_sequence_changed: - - if missionlist[current_sequence-1].z - alt_acceptance_radius > vehicle.location.global_relative_frame.alt or missionlist[current_sequence-1].z + alt_acceptance_radius < vehicle.location.global_relative_frame.alt: - print("waypoint %s out of bounds altitude %s gps altitude: %s" % (current_sequence, missionlist[current_sequence-1].z, vehicle.location.global_relative_frame.alt)) - mission_failed = True - current_sequence_changed = False - elapsed_time = time.time() - start_time - -if elapsed_time < max_execution_time: - print("Mission items have been executed") - -# wait for the vehicle to have landed -while (current_landed_state != 1 and elapsed_time < max_execution_time): - time.sleep(1) - elapsed_time = time.time() - start_time - -if elapsed_time < max_execution_time: - print("Vehicle has landed") - -# Disarm vehicle -vehicle.armed = False - -# count elapsed time -elapsed_time = time.time() - start_time - -# Close vehicle object before exiting script -vehicle.close() -time.sleep(2) - -# Validate time constraint -if elapsed_time <= max_execution_time and not mission_failed: - print("Mission succesful time elapsed %s" % elapsed_time) - sys.exit(0) - -if elapsed_time > max_execution_time: - print("Mission FAILED to execute within %s seconds" % max_execution_time) - sys.exit(99) - -if mission_failed: - print("Mission FAILED out of bounds") - sys.exit(100) - -print("Mission FAILED something strange happened") -sys.exit(101) diff --git a/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission b/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission deleted file mode 100644 index 5094f11c5d..0000000000 --- a/integrationtests/python_src/px4_it/dronekit/VTOL_TAKEOFF.mission +++ /dev/null @@ -1,89 +0,0 @@ -{ - "MAV_AUTOPILOT": 12, - "complexItems": [ - ], - "groundStation": "QGroundControl", - "items": [ - { - "autoContinue": true, - "command": 84, - "coordinate": [ - 47.397510528564453, - 8.5502662658691406, - 10 - ], - "frame": 3, - "id": 1, - "param1": 0, - "param2": 0, - "param3": 0, - "param4": 0, - "type": "missionItem" - }, - { - "autoContinue": true, - "command": 16, - "coordinate": [ - 47.395450592041016, - 8.5501842498779297, - 10 - ], - "frame": 3, - "id": 2, - "param1": 0, - "param2": 0, - "param3": 0, - "param4": 0, - "type": "missionItem" - }, - { - "autoContinue": true, - "command": 16, - "coordinate": [ - 47.395420809467687, - 8.5456138849258423, - 10 - ], - "frame": 3, - "id": 3, - "param1": 0, - "param2": 0, - "param3": 0, - "param4": 0, - "type": "missionItem" - }, - { - "autoContinue": true, - "command": 85, - "coordinate": [ - 47.397735595703125, - 8.5456113815307617, - 10 - ], - "frame": 3, - "id": 4, - "param1": 0, - "param2": 0, - "param3": 0, - "param4": 0, - "type": "missionItem" - } - ], - "plannedHomePosition": { - "autoContinue": true, - "command": 16, - "coordinate": [ - 47.39659309387207, - 8.5479388236999512, - 0 - ], - "frame": 0, - "id": 0, - "param1": 0, - "param2": 0, - "param3": 0, - "param4": 0, - "type": "missionItem" - }, - "version": "1.0" -} diff --git a/integrationtests/python_src/px4_it/util/TODO.md b/integrationtests/python_src/px4_it/util/TODO.md deleted file mode 100644 index b57e40a46e..0000000000 --- a/integrationtests/python_src/px4_it/util/TODO.md +++ /dev/null @@ -1 +0,0 @@ -TODO: Adopt to new SITL diff --git a/integrationtests/python_src/px4_it/util/flight_path_assertion.py b/integrationtests/python_src/px4_it/util/flight_path_assertion.py deleted file mode 100644 index 7de82cc84d..0000000000 --- a/integrationtests/python_src/px4_it/util/flight_path_assertion.py +++ /dev/null @@ -1,197 +0,0 @@ -#!/usr/bin/env python -#*************************************************************************** -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -#***************************************************************************/ - -# -# @author Andreas Antener -# -import rospy -import threading - -from px4.msg import vehicle_local_position -from gazebo_msgs.srv import SpawnModel -from gazebo_msgs.srv import SetModelState -from gazebo_msgs.srv import DeleteModel -from geometry_msgs.msg import Pose -from geometry_msgs.msg import Twist - -from numpy import linalg -import numpy as np - -# -# Helper to test if vehicle stays on expected flight path. -# -class FlightPathAssertion(threading.Thread): - - # - # Arguments - # - positions: tuple of tuples in the form (x, y, z, heading) - # - # TODO: yaw validation - # TODO: fail main test thread - # - def __init__(self, positions, tunnelRadius=1, yaw_offset=0.2): - threading.Thread.__init__(self) - rospy.Subscriber("vehicle_local_position", vehicle_local_position, self.position_callback) - self.spawn_model = rospy.ServiceProxy('/gazebo/spawn_sdf_model', SpawnModel) - self.set_model_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState) - self.delete_model = rospy.ServiceProxy('/gazebo/delete_model', DeleteModel) - - self.positions = positions - self.tunnel_radius = tunnelRadius - self.yaw_offset = yaw_offset - self.has_pos = False - self.should_stop = False - self.center = positions[0] - self.end_of_segment = False - self.failed = False - self.local_position = vehicle_local_position - - def position_callback(self, data): - self.has_pos = True - self.local_position = data - - def spawn_indicator(self): - self.delete_model("indicator") - xml = ( - "" + - "" + - "" + - "true" + - "" + - "" + - "0.7" + - "" + - "" + - "%f" + - "" + - "" + - "" + - "1 0 0 0.5" + - "1 0 0 0.5" + - "" + - "" + - "" + - "" + - "") % self.tunnel_radius - - self.spawn_model("indicator", xml, "", Pose(), "") - - def position_indicator(self): - state = SetModelState() - state.model_name = "indicator" - pose = Pose() - pose.position.x = self.center[0] - pose.position.y = (-1) * self.center[1] - pose.position.z = (-1) * self.center[2] - state.pose = pose - state.twist = Twist() - state.reference_frame = "" - self.set_model_state(state) - - def distance_to_line(self, a, b, pos): - v = b - a - w = pos - a - - c1 = np.dot(w, v) - if c1 <= 0: # before a - self.center = a - return linalg.norm(pos - a) - - c2 = np.dot(v, v) - if c2 <= c1: # after b - self.center = b - self.end_of_segment = True - return linalg.norm(pos - b) - - x = c1 / c2 - l = a + x * v - self.center = l - return linalg.norm(pos - l) - - def stop(self): - self.should_stop = True - - def run(self): - rate = rospy.Rate(10) # 10hz - self.spawn_indicator() - - current = 0 - count = 0 - while not self.should_stop: - if self.has_pos: - # calculate distance to line segment between first two points - # if distances > tunnel_radius - # exit with error - # advance current pos if not on the line anymore or distance to next point < tunnel_radius - # exit if current pos is now the last position - - self.position_indicator() - - pos = np.array((self.local_position.x, - self.local_position.y, - self.local_position.z)) - a_pos = np.array((self.positions[current][0], - self.positions[current][1], - self.positions[current][2])) - b_pos = np.array((self.positions[current + 1][0], - self.positions[current + 1][1], - self.positions[current + 1][2])) - - dist = self.distance_to_line(a_pos, b_pos, pos) - b_dist = linalg.norm(pos - b_pos) - - rospy.logdebug("distance to line: %f, distance to end: %f" % (dist, b_dist)) - - if dist > self.tunnel_radius: - msg = "left tunnel at position (%f, %f, %f)" % (self.local_position.x, self.local_position.y, self.local_position.z) - rospy.logerr(msg) - self.failed = True - break - - if self.end_of_segment or b_dist < self.tunnel_radius: - rospy.loginfo("next segment") - self.end_of_segment = False - current = current + 1 - - if current == len(self.positions) - 1: - rospy.loginfo("no more positions") - break - - rate.sleep() - count = count + 1 - - if count > 10 and not self.has_pos: # no position after 1 sec - rospy.logerr("no position") - self.failed = True - break diff --git a/platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh b/platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh deleted file mode 100755 index dd92e0a764..0000000000 --- a/platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/usr/bin/env bash - -set -e - -rm -rf build - -(mkdir -p build && cd build && CXX=g++ cmake .. && make -j8 && time $@ ./test/lockstep_scheduler_test) -rm -rf build - -(mkdir -p build && cd build && CXX=clang++ cmake .. && make -j8 && time $@ ./test/lockstep_scheduler_test) -rm -rf build diff --git a/src/modules/local_position_estimator/fault_table.py b/src/modules/local_position_estimator/fault_table.py deleted file mode 100644 index 9c84074836..0000000000 --- a/src/modules/local_position_estimator/fault_table.py +++ /dev/null @@ -1,13 +0,0 @@ -#!/usr/bin/env python -from __future__ import print_function -import pylab as pl -import scipy.optimize -from scipy.stats import chi2 - -for fa_rate in 1.0/pl.array([1e1, 1e2, 1e4, 1e6, 1e9]): - print(fa_rate) - for df in range(1,7): - f_eq = lambda x: ((1- fa_rate) - chi2.cdf(x, df))**2 - res = scipy.optimize.minimize(f_eq, df) - assert res['success'] - print('\t', res.x[0])