Compare commits

...

4 Commits

Author SHA1 Message Date
Jacob Dahl 1af6f70d57 chore(tests): remove dead integration test utilities with zero references
- integrationtests/python_src/px4_it/util/manual_input.py: last meaningful change 2019, 0 refs
- integrationtests/python_src/px4_it/util/px4_test_helper.py: last meaningful change 2016, 0 refs
2026-03-18 01:11:10 -08:00
Jacob Dahl 47c8c53745 fix(lpe): remove stale comment referencing deleted fault_table.py 2026-03-18 01:11:03 -08:00
Jacob Dahl a3681de493 chore(ci): remove Jenkinsfile superseded by GitHub Actions
All Jenkinsfile functions are now covered by GitHub Actions:
- Metadata generation → docs-orchestrator.yml + metadata_sync.sh
- PX4-user_guide deploy → in-repo docs/ + docs-orchestrator.yml
- S3 upload → build_all_targets.yml artifacts job
- QGC metadata push → already broken (no metadata commits)
- uORB graphs → supported by metadata_sync.sh

Co-authored-by: mrpollo <317648+mrpollo@users.noreply.github.com>
2026-03-18 01:10:49 -08:00
Jacob Dahl e05eca20a4 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>
2026-03-18 01:10:42 -08:00
16 changed files with 0 additions and 1661 deletions
Vendored
-268
View File
@@ -1,268 +0,0 @@
#!/usr/bin/env groovy
pipeline {
agent none
stages {
stage('Analysis') {
when {
anyOf {
branch 'main'
branch 'master' // should be removed, but in case there is something going on...
branch 'pr-jenkins' // for testing
}
}
parallel {
stage('Airframe') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh 'make distclean; git clean -ff -x -d .'
sh 'git fetch --all --tags'
sh 'make airframe_metadata'
dir('build/px4_sitl_default/docs') {
archiveArtifacts(artifacts: 'airframes.md, airframes.xml')
stash includes: 'airframes.md, airframes.xml', name: 'metadata_airframes'
}
}
post {
always {
sh 'make distclean; git clean -ff -x -d .'
}
}
}
stage('Parameter') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh 'make distclean; git clean -ff -x -d .'
sh 'git fetch --all --tags'
sh 'make parameters_metadata'
dir('build/px4_sitl_default/docs') {
archiveArtifacts(artifacts: 'parameters.md, parameters.xml, parameters.json.xz')
stash includes: 'parameters.md, parameters.xml, parameters.json.xz', name: 'metadata_parameters'
}
}
post {
always {
sh 'make distclean; git clean -ff -x -d .'
}
}
}
stage('Module') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh 'make distclean; git clean -ff -x -d .'
sh 'git fetch --all --tags'
sh 'make module_documentation'
dir('build/px4_sitl_default/docs') {
archiveArtifacts(artifacts: 'modules/*.md')
stash includes: 'modules/*.md', name: 'metadata_module_documentation'
}
}
post {
always {
sh 'make distclean; git clean -ff -x -d .'
}
}
}
stage('msg file docs') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh 'mkdir -p build/msg_docs; ./Tools/msg/generate_msg_docs.py -d build/msg_docs'
dir('build') {
archiveArtifacts(artifacts: 'msg_docs/*.md')
stash includes: 'msg_docs/*.md', name: 'msg_documentation'
}
}
post {
always {
sh 'make distclean; git clean -ff -x -d .'
}
}
}
stage('failsafe docs') {
agent {
docker { image 'px4io/px4-dev-nuttx-focal:2022-08-12' }
}
steps {
sh '''#!/bin/bash -l
echo $0;
git clone https://github.com/emscripten-core/emsdk.git _emscripten_sdk;
cd _emscripten_sdk;
git checkout 4.0.15;
./emsdk install latest;
./emsdk activate latest;
cd ..;
. ./_emscripten_sdk/emsdk_env.sh;
git fetch --all --tags;
make failsafe_web;
cd build/px4_sitl_default_failsafe_web;
mkdir -p failsafe_sim;
cp index.* parameters.json failsafe_sim;
'''
dir('build/px4_sitl_default_failsafe_web') {
archiveArtifacts(artifacts: 'failsafe_sim/*')
stash includes: 'failsafe_sim/*', name: 'failsafe_sim'
}
}
post {
always {
sh 'make distclean; git clean -ff -x -d .'
}
}
}
stage('uORB graphs') {
agent {
docker {
image 'px4io/px4-dev-nuttx-focal:2022-08-12'
args '-e CCACHE_BASEDIR=$WORKSPACE -v ${CCACHE_DIR}:${CCACHE_DIR}:rw'
}
}
steps {
sh 'export'
sh 'make distclean; git clean -ff -x -d .'
sh 'git fetch --all --tags'
sh 'make uorb_graphs'
dir('Tools/uorb_graph') {
archiveArtifacts(artifacts: 'graph_*.json')
stash includes: 'graph_*.json', name: 'uorb_graph'
}
}
post {
always {
sh 'make distclean; git clean -ff -x -d .'
}
}
}
} // parallel
} // stage: Generate Metadata
stage('Deploy') {
parallel {
stage('Userguide') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh('export')
unstash 'metadata_airframes'
unstash 'metadata_parameters'
unstash 'metadata_module_documentation'
unstash 'msg_documentation'
unstash 'failsafe_sim'
unstash 'uorb_graph'
withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/PX4/PX4-user_guide.git')
sh('cp airframes.md PX4-user_guide/en/airframes/airframe_reference.md')
sh('cp parameters.md PX4-user_guide/en/advanced_config/parameter_reference.md')
sh('cp -R modules/*.md PX4-user_guide/en/modules/')
sh('cp -R graph_*.json PX4-user_guide/public/middleware/') // vitepress
sh('cp -R msg_docs/*.md PX4-user_guide/en/msg_docs/')
sh('cp -R failsafe_sim/* PX4-user_guide/public/config/failsafe') // vitepress
sh('cd PX4-user_guide; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
sh('cd PX4-user_guide; git push origin main || true')
sh('rm -rf PX4-user_guide')
}
}
when {
anyOf {
branch 'main'
branch 'master' // should be removed, but in case there is something going on...
branch 'pr-jenkins' // for testing
}
}
options {
skipDefaultCheckout()
}
}
stage('QGroundControl') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh('export')
unstash 'metadata_airframes'
unstash 'metadata_parameters'
withCredentials([usernamePassword(credentialsId: 'px4buildbot_github_personal_token', passwordVariable: 'GIT_PASS', usernameVariable: 'GIT_USER')]) {
sh('git clone https://${GIT_USER}:${GIT_PASS}@github.com/mavlink/qgroundcontrol.git')
sh('cp airframes.xml qgroundcontrol/src/AutoPilotPlugins/PX4/AirframeFactMetaData.xml')
sh('cp parameters.xml qgroundcontrol/src/FirmwarePlugin/PX4/PX4ParameterFactMetaData.xml')
sh('cd qgroundcontrol; git status; git add .; git commit -a -m "Update PX4 Firmware metadata `date`" || true')
sh('cd qgroundcontrol; git push origin master || true')
sh('rm -rf qgroundcontrol')
}
}
when {
anyOf {
branch 'main'
branch 'master' // should be removed, but in case there is something going on...
branch 'pr-jenkins' // for testing
}
}
options {
skipDefaultCheckout()
}
}
stage('S3') {
agent {
docker { image 'px4io/px4-dev-base-focal:2021-08-18' }
}
steps {
sh('export')
unstash 'metadata_airframes'
unstash 'metadata_parameters'
sh('ls')
withAWS(credentials: 'px4_aws_s3_key', region: 'us-east-1') {
s3Upload(acl: 'PublicRead', bucket: 'px4-travis', file: 'airframes.xml', path: 'Firmware/master/')
s3Upload(acl: 'PublicRead', bucket: 'px4-travis', file: 'parameters.xml', path: 'Firmware/master/')
s3Upload(acl: 'PublicRead', bucket: 'px4-travis', file: 'parameters.json.xz', path: 'Firmware/master/')
}
}
when {
anyOf {
branch 'main'
branch 'master' // should be removed, but in case there is something going on...
branch 'pr-jenkins' // for testing
}
}
options {
skipDefaultCheckout()
}
}
} // parallel
} // stage: Generate Metadata
} // stages
environment {
CCACHE_DIR = '/tmp/ccache'
CI = true
GIT_AUTHOR_EMAIL = "bot@px4.io"
GIT_AUTHOR_NAME = "PX4BuildBot"
GIT_COMMITTER_EMAIL = "bot@px4.io"
GIT_COMMITTER_NAME = "PX4BuildBot"
}
options {
buildDiscarder(logRotator(numToKeepStr: '20', artifactDaysToKeepStr: '30'))
timeout(time: 90, unit: 'MINUTES')
}
}
-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,168 +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
from px4.msg import manual_control_setpoint
from px4.msg import offboard_control_mode
from mav_msgs.msg import CommandAttitudeThrust
from std_msgs.msg import Header
#
# Manual input control helper
#
class ManualInput(object):
def __init__(self):
rospy.init_node('test_node', anonymous=True)
self.pub_mcsp = rospy.Publisher('manual_control_setpoint', manual_control_setpoint, queue_size=10)
self.pub_off = rospy.Publisher('offboard_control_mode', offboard_control_mode, queue_size=10)
def arm(self):
rate = rospy.Rate(10) # 10hz
att = CommandAttitudeThrust()
att.header = Header()
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("zeroing")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
pos.r = 1
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("arming")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
def posctl(self):
rate = rospy.Rate(10) # 10hz
# triggers posctl
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 2
pos.return_switch = 3
pos.posctl_switch = 1
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 3
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("triggering posctl")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
def offboard_attctl(self):
self.offboard(False, False, True, True, True, True)
def offboard_posctl(self):
self.offboard(False, False, True, False, True, True)
# Trigger offboard and set offboard control mode before
def offboard(self, ignore_thrust=False, ignore_attitude=False, ignore_bodyrate=True,
ignore_position=False, ignore_velocity=True, ignore_acceleration_force=True):
rate = rospy.Rate(10) # 10hz
mode = offboard_control_mode()
mode.ignore_thrust = ignore_thrust
mode.ignore_attitude = ignore_attitude
mode.ignore_bodyrate_x = ignore_bodyrate
mode.ignore_bodyrate_y = ignore_bodyrate
mode.ignore_bodyrate_z = ignore_bodyrate
mode.ignore_position = ignore_position
mode.ignore_velocity = ignore_velocity
mode.ignore_acceleration_force = ignore_acceleration_force
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("setting offboard mode")
time = rospy.get_rostime().now()
mode.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_off.publish(mode)
rate.sleep()
count = count + 1
# triggers offboard
pos = manual_control_setpoint()
pos.x = 0
pos.z = 0
pos.y = 0
pos.r = 0
pos.mode_switch = 3
pos.return_switch = 3
pos.posctl_switch = 3
pos.loiter_switch = 3
pos.acro_switch = 0
pos.offboard_switch = 1
count = 0
while not rospy.is_shutdown() and count < 5:
rospy.loginfo("triggering offboard")
time = rospy.get_rostime().now()
pos.timestamp = time.secs * 1e6 + time.nsecs / 1000
self.pub_mcsp.publish(pos)
rate.sleep()
count = count + 1
@@ -1,110 +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>
#
PKG = 'px4'
import unittest
import rospy
import rosbag
from px4.msg import vehicle_local_position
from px4.msg import vehicle_attitude_setpoint
from px4.msg import vehicle_attitude
from px4.msg import vehicle_local_position_setpoint
from threading import Condition
#
# Test helper
#
class PX4TestHelper(object):
def __init__(self, test_name):
self.test_name = test_name
def setUp(self):
self.condition = Condition()
self.closed = False
rospy.init_node('test_node', anonymous=True)
self.bag = rosbag.Bag(self.test_name + '.bag', 'w', compression="lz4")
self.sub_vlp = rospy.Subscriber("iris/vehicle_local_position",
vehicle_local_position, self.vehicle_position_callback)
self.sub_vasp = rospy.Subscriber("iris/vehicle_attitude_setpoint",
vehicle_attitude_setpoint, self.vehicle_attitude_setpoint_callback)
self.sub_va = rospy.Subscriber("iris/vehicle_attitude",
vehicle_attitude, self.vehicle_attitude_callback)
self.sub_vlps = rospy.Subscriber("iris/vehicle_local_position_setpoint",
vehicle_local_position_setpoint, self.vehicle_local_position_setpoint_callback)
def tearDown(self):
try:
self.condition.acquire()
self.closed = True
self.sub_vlp.unregister()
self.sub_vasp.unregister()
self.sub_va.unregister()
self.sub_vlps.unregister()
self.bag.close()
finally:
self.condition.release()
def vehicle_position_callback(self, data):
self.bag_write('px4/vehicle_local_position', data)
def vehicle_attitude_setpoint_callback(self, data):
self.bag_write('px4/vehicle_attitude_setpoint', data)
def vehicle_attitude_callback(self, data):
self.bag_write('px4/vehicle_attitude', data)
def vehicle_local_position_setpoint_callback(self, data):
self.bag_write('px4/vehicle_local_position_setpoint', data)
def bag_write(self, topic, data):
try:
self.condition.acquire()
if not self.closed:
self.bag.write(topic, data)
else:
rospy.logwarn("Trying to write to bag but it's already closed")
finally:
self.condition.release()
@@ -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
@@ -49,7 +49,6 @@ static const size_t N_DIST_SUBS = 4;
// for fault detection
// chi squared distribution, false alarm probability 0.0001
// see fault_table.py
// note skip 0 index so we can use degree of freedom as index
static const float BETA_TABLE[7] = {0,
8.82050518214,
@@ -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])