chore(tools): remove dead scripts and files with zero references

Remove files that have zero references in the codebase and haven't been
meaningfully touched in years:

- Tools/stack_usage/avstack.pl: AVR stack checker (2017), wrong arch, 0 refs
- Tools/usb_serialload.py: USB serial load tester (2022), Python 2, 0 refs
- Tools/dist/vehicle_configs.xml: old vehicle configs (2015), deprecated mixer format, 0 refs
- Tools/models/sdp3x_pitot_model.py: SDP3x pitot model (2017), 0 refs
- Tools/Matlab/motors.m: MATLAB motor script (2015), 0 refs
- Tools/Matlab/plot_mag.m: MATLAB mag plotter (2015), 0 refs
- src/modules/local_position_estimator/fault_table.py: fault table gen (2018), LPE disabled, 0 refs
- integrationtests/python_src/px4_it/util/flight_path_assertion.py: old ROS1 util (2016), 0 refs
- integrationtests/python_src/px4_it/util/TODO.md: stale TODO (2016), 0 refs
- integrationtests/python_src/px4_it/dronekit/: DroneKit mission checker (2019), deprecated lib, 0 refs
- platforms/posix/src/px4/common/lockstep_scheduler/build-and-test.sh: standalone build (2019), replaced by CMake, 0 refs

Co-authored-by: dakejahl <37091262+dakejahl@users.noreply.github.com>
This commit is contained in:
Jacob Dahl
2026-03-18 01:10:42 -08:00
co-authored by dakejahl
parent 7c1dee0b41
commit e05eca20a4
12 changed files with 0 additions and 1114 deletions
-58
View File
@@ -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');
-77
View File
@@ -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))
-23
View File
@@ -1,23 +0,0 @@
<?xml version='1.0' encoding='UTF-8'?>
<vehicle_configs>
<vehicle class="multirotor" rotors="4" geometry="x" propeller_diameter="8" weight="1.0" id="4010">
<title>Standard 8" Prop Quadrotor (x)</title>
<short_desc>Standard quadrotor configuration in x configuration for 8-" propellers</short_desc>
<mixer>/etc/mixers/quad_x.main.mix</mixer>
</vehicle>
<vehicle class="multirotor" rotors="4" geometry="+" propeller_diameter="8" weight="1.0" id="5001">
<title>Standard 8" Prop Quadrotor (+)</title>
<short_desc>Standard quadrotor configuration in + configuration for 8-" propellers</short_desc>
<mixer>/etc/mixers/quad_+.main.mix</mixer>
</vehicle>
<vehicle class="multirotor" rotors="4" geometry="x" propeller_diameter="10" weight="1.5" id="4011">
<title>Standard 8" Prop Quadrotor (x)</title>
<short_desc>Standard quadrotor configuration in x configuration for 8-" propellers</short_desc>
<mixer>/etc/mixers/quad_x.main.mix</mixer>
</vehicle>
<vehicle class="fixed_wing" wingspan="0.85" elevons="yes" ailerons="no" rudder="no" weight="1.5" id="3033">
<title>Zeta Science Wing Wing Z-84</title>
<short_desc>Configuration for a small flying wing.</short_desc>
<mixer>/etc/mixers/wingwing.main.mix</mixer>
</vehicle>
</vehicle_configs>
-110
View File
@@ -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()
-251
View File
@@ -1,251 +0,0 @@
#!/usr/bin/perl -w
# avstack.pl: AVR stack checker
# Copyright (C) 2013 Daniel Beer <dlbeer@gmail.com>
#
# 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 <object files>
#
# 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 (<DISASSEMBLY>) {
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 (<SUFILE>) {
$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"; }
-55
View File
@@ -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
@@ -1,229 +0,0 @@
#!/usr/bin/env python
################################################################################################
# @File MissionCheck.py
# Automated mission loading, execution and monitoring
# for Continuous Integration
#
# @author Sander Smeets <sander@droneslab.com>
#
# 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)
@@ -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"
}
@@ -1 +0,0 @@
TODO: Adopt to new SITL
@@ -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 <andreas@uaventure.com>
#
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 = (
"<?xml version='1.0'?>" +
"<sdf version='1.4'>" +
"<model name='indicator'>" +
"<static>true</static>" +
"<link name='link'>" +
"<visual name='visual'>" +
"<transparency>0.7</transparency>" +
"<geometry>" +
"<sphere>" +
"<radius>%f</radius>" +
"</sphere>" +
"</geometry>" +
"<material>" +
"<ambient>1 0 0 0.5</ambient>" +
"<diffuse>1 0 0 0.5</diffuse>" +
"</material>" +
"</visual>" +
"</link>" +
"</model>" +
"</sdf>") % 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
@@ -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
@@ -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])