Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

Merged release_v1.0.0 branch into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-25 21:45:17 +02:00
parent 74bdec2cc6 5b93d92339
commit 454becdae5
73 changed files with 2726 additions and 887 deletions
Download Patch File Download Diff File Expand all files Collapse all files
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
+2 -3
View File
@@ -9,16 +9,15 @@ sh /etc/init.d/rc.mc_defaults
if [ $AUTOCNF == yes ]
then
# TODO tune roll/pitch separately
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.004
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_P 0.19
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.004
param set MC_YAW_P 2.8
param set MC_YAW_P 4.0
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
ROMFS/px4fmu_common/init.d/10019_sk450_deadcat
+4 -4
View File
@@ -10,13 +10,13 @@ sh /etc/init.d/rc.mc_defaults
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 6.0
param set MC_ROLLRATE_P 0.04
param set MC_ROLLRATE_I 0.1
param set MC_ROLLRATE_P 0.08
param set MC_ROLLRATE_I 0.03
param set MC_ROLLRATE_D 0.0015
param set MC_PITCH_P 6.0
param set MC_PITCHRATE_P 0.08
param set MC_PITCHRATE_I 0.2
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.03
param set MC_PITCHRATE_D 0.0015
param set MC_YAW_P 2.8
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
+2 -1
View File
@@ -1,7 +1,7 @@
#
# Generic configuration file for caipirinha VTOL version
#
# Roman Bapst <bapstr@ethz.ch>
# Roman Bapst <bapstr@gmail.com>
#
sh /etc/init.d/rc.vtol_defaults
@@ -13,3 +13,4 @@ set PWM_MAX 2000
set PWM_RATE 400
param set VT_MOT_COUNT 2
param set VT_IDLE_PWM_MC 1080
param set VT_TYPE 0
ROMFS/px4fmu_common/init.d/13002_firefly6
+2 -1
View File
@@ -2,7 +2,7 @@
#
# Generic configuration file for BirdsEyeView Aerobotics FireFly6
#
# Roman Bapst <romanbapst@yahoo.de>
# Roman Bapst <bapstroman@gmail.com>
#
sh /etc/init.d/rc.vtol_defaults
@@ -41,3 +41,4 @@ set MAV_TYPE 21
param set VT_MOT_COUNT 6
param set VT_IDLE_PWM_MC 1080
param set VT_TYPE 1
ROMFS/px4fmu_common/init.d/13003_quad_tailsitter
+18
View File
@@ -0,0 +1,18 @@
#
# Generic configuration file for a tailsitter with motors in X configuration.
#
# Roman Bapst <bapstroman@gmail.com>
#
sh /etc/init.d/rc.vtol_defaults
set MIXER quad_x_vtol
set PWM_OUT 1234
set PWM_MAX 2000
set PWM_RATE 400
set MAV_TYPE 20
param set VT_MOT_COUNT 4
param set VT_IDLE_PWM_MC 1080
param set VT_TYPE 0
param set VT_ELEV_MC_LOCK 1
ROMFS/px4fmu_common/init.d/3030_io_camflyer
+24
View File
@@ -2,6 +2,30 @@
sh /etc/init.d/rc.fw_defaults
if [ $AUTOCNF == yes ]
then
param set FW_AIRSPD_MAX 15
param set FW_AIRSPD_MIN 10
param set FW_AIRSPD_TRIM 13
param set FW_ATT_TC 0.3
param set FW_L1_DAMPING 0.74
param set FW_L1_PERIOD 16
param set FW_LND_ANG 15
param set FW_LND_FLALT 5
param set FW_LND_HHDIST 15
param set FW_LND_HVIRT 13
param set FW_LND_TLALT 5
param set FW_THR_LND_MAX 0
param set FW_PR_FF 0.35
param set FW_PR_I 0.005
param set FW_PR_IMAX 0.4
param set FW_PR_P 0.08
param set FW_RR_FF 0.6
param set FW_RR_I 0.005
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.04
fi
set MIXER Q
# Provide ESC a constant 1000 us pulse while disarmed
set PWM_OUT 4
ROMFS/px4fmu_common/init.d/3033_wingwing
-10
View File
@@ -30,16 +30,6 @@ then
param set FW_RR_I 0.005
param set FW_RR_IMAX 0.2
param set FW_RR_P 0.04
param set MT_TKF_PIT_MAX 30.0
param set MT_ACC_D 0.2
param set MT_ACC_P 0.6
param set MT_A_LP 0.5
param set MT_PIT_OFF 0.1
param set MT_PIT_I 0.1
param set MT_THR_OFF 0.65
param set MT_THR_I 0.35
param set MT_THR_P 0.2
param set MT_THR_FF 1.5
fi
set MIXER wingwing
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
+5 -3
View File
@@ -34,7 +34,9 @@ then
param set PWM_MAIN_REV1 1
fi
set PWM_DISARMED p:PWM_DISARMED
set PWM_MIN p:PWM_MIN
set PWM_MAX p:PWM_MAX
set MIXER caipi
# Provide ESC a constant 1000 us pulse
set PWM_OUT 4
set PWM_DISARMED 1000
set PWM_OUT 1234
ROMFS/px4fmu_common/init.d/4010_dji_f330
+2 -2
View File
@@ -10,11 +10,11 @@ sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_P 0.13
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_P 0.13
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
ROMFS/px4fmu_common/init.d/4012_quad_x_can
+6 -7
View File
@@ -9,18 +9,17 @@ sh /etc/init.d/4001_quad_x
if [ $AUTOCNF == yes ]
then
# TODO REVIEW
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_P 0.16
param set MC_ROLLRATE_I 0.05
param set MC_ROLLRATE_D 0.003
param set MC_ROLLRATE_D 0.01
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_P 0.16
param set MC_PITCHRATE_I 0.05
param set MC_PITCHRATE_D 0.003
param set MC_PITCHRATE_D 0.01
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.0
param set MC_YAWRATE_P 0.3
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
fi
ROMFS/px4fmu_common/init.d/rc.autostart
+8
View File
@@ -268,6 +268,14 @@ then
sh /etc/init.d/13002_firefly6
fi
#
# Tailsitter with 4 motors in x config
#
if param compare SYS_AUTOSTART 13003
then
sh /etc/init.d/13003_quad_tailsitter
fi
#
# TriCopter Y Yaw+
#
ROMFS/px4fmu_common/init.d/rc.mc_defaults
-32
View File
@@ -4,38 +4,6 @@ set VEHICLE_TYPE mc
if [ $AUTOCNF == yes ]
then
param set MC_ROLL_P 7.0
param set MC_ROLLRATE_P 0.1
param set MC_ROLLRATE_I 0.0
param set MC_ROLLRATE_D 0.003
param set MC_PITCH_P 7.0
param set MC_PITCHRATE_P 0.1
param set MC_PITCHRATE_I 0.0
param set MC_PITCHRATE_D 0.003
param set MC_YAW_P 2.8
param set MC_YAWRATE_P 0.2
param set MC_YAWRATE_I 0.1
param set MC_YAWRATE_D 0.0
param set MC_YAW_FF 0.5
param set MPC_THR_MAX 1.0
param set MPC_THR_MIN 0.1
param set MPC_XY_P 1.0
param set MPC_XY_VEL_P 0.1
param set MPC_XY_VEL_I 0.02
param set MPC_XY_VEL_D 0.01
param set MPC_XY_VEL_MAX 5
param set MPC_XY_FF 0.5
param set MPC_Z_P 1.0
param set MPC_Z_VEL_P 0.1
param set MPC_Z_VEL_I 0.02
param set MPC_Z_VEL_D 0.0
param set MPC_Z_VEL_MAX 3
param set MPC_Z_FF 0.5
param set MPC_TILTMAX_AIR 45.0
param set MPC_TILTMAX_LND 15.0
param set MPC_LAND_SPEED 1.0
param set PE_VELNE_NOISE 0.5
param set PE_VELD_NOISE 0.7
param set PE_POSNE_NOISE 0.5
ROMFS/px4fmu_common/init.d/rc.usb
+3 -3
View File
@@ -3,7 +3,7 @@
# USB MAVLink start
#
mavlink start -r 80000 -d /dev/ttyACM0 -x
mavlink start -r 800000 -d /dev/ttyACM0 -x
# Enable a number of interesting streams we want via USB
mavlink stream -d /dev/ttyACM0 -s PARAM_VALUE -r 300
mavlink stream -d /dev/ttyACM0 -s MISSION_ITEM -r 50
@@ -11,14 +11,14 @@ mavlink stream -d /dev/ttyACM0 -s NAMED_VALUE_FLOAT -r 10
mavlink stream -d /dev/ttyACM0 -s OPTICAL_FLOW_RAD -r 10
mavlink stream -d /dev/ttyACM0 -s DISTANCE_SENSOR -r 10
mavlink stream -d /dev/ttyACM0 -s VFR_HUD -r 20
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 20
mavlink stream -d /dev/ttyACM0 -s ATTITUDE -r 100
mavlink stream -d /dev/ttyACM0 -s ACTUATOR_CONTROL_TARGET0 -r 30
mavlink stream -d /dev/ttyACM0 -s RC_CHANNELS_RAW -r 5
mavlink stream -d /dev/ttyACM0 -s SERVO_OUTPUT_RAW_0 -r 20
mavlink stream -d /dev/ttyACM0 -s POSITION_TARGET_GLOBAL_INT -r 10
mavlink stream -d /dev/ttyACM0 -s LOCAL_POSITION_NED -r 30
mavlink stream -d /dev/ttyACM0 -s MANUAL_CONTROL -r 5
mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 20
mavlink stream -d /dev/ttyACM0 -s HIGHRES_IMU -r 100
mavlink stream -d /dev/ttyACM0 -s GPS_RAW_INT -r 20
# Exit shell to make it available to MAVLink
ROMFS/px4fmu_common/mixers/firefly6.aux.mix
+2 -2
View File
@@ -11,12 +11,12 @@ Elevon mixers
-------------
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 7500 7500 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
S: 1 1 8000 8000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 7500 7500 0 -10000 10000
S: 1 0 -7500 -7500 0 -10000 10000
S: 1 1 -8000 -8000 0 -10000 10000
Landing gear mixer
ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix
+18
View File
@@ -0,0 +1,18 @@
Mixer for Tailsitter with x motor configuration and elevons
===========================================================
This file defines a single mixer for tailsitter with motors in X configuration. All controls
are mixed 100%.
R: 4x 10000 10000 10000 0
#mixer for the elevons
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 10000 10000 0 -10000 10000
S: 1 1 10000 10000 0 -10000 10000
M: 2
O: 10000 10000 0 -10000 10000
S: 1 0 10000 10000 0 -10000 10000
S: 1 1 -10000 -10000 0 -10000 10000
Tools/Matlab/ellipsoid_fit.m
+174
View File
@@ -0,0 +1,174 @@
% Copyright (c) 2009, Yury Petrov
% 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
%
% 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.
%
function [ center, radii, evecs, v ] = ellipsoid_fit( X, flag, equals )
%
% Fit an ellispoid/sphere to a set of xyz data points:
%
% [center, radii, evecs, pars ] = ellipsoid_fit( X )
% [center, radii, evecs, pars ] = ellipsoid_fit( [x y z] );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 1 );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 2, 'xz' );
% [center, radii, evecs, pars ] = ellipsoid_fit( X, 3 );
%
% Parameters:
% * X, [x y z] - Cartesian data, n x 3 matrix or three n x 1 vectors
% * flag - 0 fits an arbitrary ellipsoid (default),
% - 1 fits an ellipsoid with its axes along [x y z] axes
% - 2 followed by, say, 'xy' fits as 1 but also x_rad = y_rad
% - 3 fits a sphere
%
% Output:
% * center - ellispoid center coordinates [xc; yc; zc]
% * ax - ellipsoid radii [a; b; c]
% * evecs - ellipsoid radii directions as columns of the 3x3 matrix
% * v - the 9 parameters describing the ellipsoid algebraically:
% Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
%
% Author:
% Yury Petrov, Northeastern University, Boston, MA
%
error( nargchk( 1, 3, nargin ) ); % check input arguments
if nargin == 1
flag = 0; % default to a free ellipsoid
end
if flag == 2 && nargin == 2
equals = 'xy';
end
if size( X, 2 ) ~= 3
error( 'Input data must have three columns!' );
else
x = X( :, 1 );
y = X( :, 2 );
z = X( :, 3 );
end
% need nine or more data points
if length( x ) < 9 && flag == 0
error( 'Must have at least 9 points to fit a unique ellipsoid' );
end
if length( x ) < 6 && flag == 1
error( 'Must have at least 6 points to fit a unique oriented ellipsoid' );
end
if length( x ) < 5 && flag == 2
error( 'Must have at least 5 points to fit a unique oriented ellipsoid with two axes equal' );
end
if length( x ) < 3 && flag == 3
error( 'Must have at least 4 points to fit a unique sphere' );
end
if flag == 0
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Dxy + 2Exz + 2Fyz + 2Gx + 2Hy + 2Iz = 1
D = [ x .* x, ...
y .* y, ...
z .* z, ...
2 * x .* y, ...
2 * x .* z, ...
2 * y .* z, ...
2 * x, ...
2 * y, ...
2 * z ]; % ndatapoints x 9 ellipsoid parameters
elseif flag == 1
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1
D = [ x .* x, ...
y .* y, ...
z .* z, ...
2 * x, ...
2 * y, ...
2 * z ]; % ndatapoints x 6 ellipsoid parameters
elseif flag == 2
% fit ellipsoid in the form Ax^2 + By^2 + Cz^2 + 2Gx + 2Hy + 2Iz = 1,
% where A = B or B = C or A = C
if strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
D = [ y .* y + z .* z, ...
x .* x, ...
2 * x, ...
2 * y, ...
2 * z ];
elseif strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
D = [ x .* x + z .* z, ...
y .* y, ...
2 * x, ...
2 * y, ...
2 * z ];
else
D = [ x .* x + y .* y, ...
z .* z, ...
2 * x, ...
2 * y, ...
2 * z ];
end
else
% fit sphere in the form A(x^2 + y^2 + z^2) + 2Gx + 2Hy + 2Iz = 1
D = [ x .* x + y .* y + z .* z, ...
2 * x, ...
2 * y, ...
2 * z ]; % ndatapoints x 4 sphere parameters
end
% solve the normal system of equations
v = ( D' * D ) \ ( D' * ones( size( x, 1 ), 1 ) );
% find the ellipsoid parameters
if flag == 0
% form the algebraic form of the ellipsoid
A = [ v(1) v(4) v(5) v(7); ...
v(4) v(2) v(6) v(8); ...
v(5) v(6) v(3) v(9); ...
v(7) v(8) v(9) -1 ];
% find the center of the ellipsoid
center = -A( 1:3, 1:3 ) \ [ v(7); v(8); v(9) ];
% form the corresponding translation matrix
T = eye( 4 );
T( 4, 1:3 ) = center';
% translate to the center
R = T * A * T';
% solve the eigenproblem
[ evecs evals ] = eig( R( 1:3, 1:3 ) / -R( 4, 4 ) );
radii = sqrt( 1 ./ diag( evals ) );
else
if flag == 1
v = [ v(1) v(2) v(3) 0 0 0 v(4) v(5) v(6) ];
elseif flag == 2
if strcmp( equals, 'xz' ) || strcmp( equals, 'zx' )
v = [ v(1) v(2) v(1) 0 0 0 v(3) v(4) v(5) ];
elseif strcmp( equals, 'yz' ) || strcmp( equals, 'zy' )
v = [ v(2) v(1) v(1) 0 0 0 v(3) v(4) v(5) ];
else % xy
v = [ v(1) v(1) v(2) 0 0 0 v(3) v(4) v(5) ];
end
else
v = [ v(1) v(1) v(1) 0 0 0 v(2) v(3) v(4) ];
end
center = ( -v( 7:9 ) ./ v( 1:3 ) )';
gam = 1 + ( v(7)^2 / v(1) + v(8)^2 / v(2) + v(9)^2 / v(3) );
radii = ( sqrt( gam ./ v( 1:3 ) ) )';
evecs = eye( 3 );
end
Tools/Matlab/plot_mag.m
+77
View File
@@ -0,0 +1,77 @@
%
% 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))
msg/airspeed.msg
+2 -1
View File
@@ -1,4 +1,5 @@
uint64 timestamp # microseconds since system boot, needed to integrate
float32 indicated_airspeed_m_s # indicated airspeed in meters per second, -1 if unknown
float32 true_airspeed_m_s # true airspeed in meters per second, -1 if unknown
float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if unknown
float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
msg/mission_result.msg
+12
View File
@@ -0,0 +1,12 @@
uint32 instance_count # Instance count of this mission. Increments monotonically whenever the mission is modified
uint32 seq_reached # Sequence of the mission item which has been reached
uint32 seq_current # Sequence of the current mission item
bool valid # true if mission is valid
bool warning # true if mission is valid, but has potentially problematic items leading to safety warnings
bool reached # true if mission has been reached
bool finished # true if mission has been completed
bool stay_in_failsafe # true if the commander should not switch out of the failsafe mode
bool flight_termination # true if the navigator demands a flight termination from the commander app
bool item_do_jump_changed # true if the number of do jumps remaining has changed
uint32 item_changed_index # indicate which item has changed
uint32 item_do_jump_remaining # set to the number of do jumps remaining for that item
nuttx-configs/px4fmu-v2/nsh/defconfig
+17 -16
View File
@@ -369,7 +369,8 @@ CONFIG_BOARD_LOOPSPERMSEC=16717
CONFIG_DRAM_START=0x20000000
CONFIG_DRAM_SIZE=262144
CONFIG_ARCH_HAVE_INTERRUPTSTACK=y
CONFIG_ARCH_INTERRUPTSTACK=1500
# The actual usage is 420 bytes
CONFIG_ARCH_INTERRUPTSTACK=750
#
# Boot options
@@ -550,8 +551,8 @@ CONFIG_UART7_SERIAL_CONSOLE=y
#
# USART1 Configuration
#
CONFIG_USART1_RXBUFSIZE=512
CONFIG_USART1_TXBUFSIZE=512
CONFIG_USART1_RXBUFSIZE=600
CONFIG_USART1_TXBUFSIZE=600
CONFIG_USART1_BAUD=115200
CONFIG_USART1_BITS=8
CONFIG_USART1_PARITY=0
@@ -563,7 +564,7 @@ CONFIG_USART1_2STOP=0
# USART2 Configuration
#
CONFIG_USART2_RXBUFSIZE=600
CONFIG_USART2_TXBUFSIZE=2200
CONFIG_USART2_TXBUFSIZE=1860
CONFIG_USART2_BAUD=57600
CONFIG_USART2_BITS=8
CONFIG_USART2_PARITY=0
@@ -574,8 +575,8 @@ CONFIG_USART2_OFLOWCONTROL=y
#
# USART3 Configuration
#
CONFIG_USART3_RXBUFSIZE=512
CONFIG_USART3_TXBUFSIZE=512
CONFIG_USART3_RXBUFSIZE=400
CONFIG_USART3_TXBUFSIZE=400
CONFIG_USART3_BAUD=57600
CONFIG_USART3_BITS=8
CONFIG_USART3_PARITY=0
@@ -586,8 +587,8 @@ CONFIG_USART3_OFLOWCONTROL=y
#
# UART4 Configuration
#
CONFIG_UART4_RXBUFSIZE=512
CONFIG_UART4_TXBUFSIZE=512
CONFIG_UART4_RXBUFSIZE=400
CONFIG_UART4_TXBUFSIZE=400
CONFIG_UART4_BAUD=57600
CONFIG_UART4_BITS=8
CONFIG_UART4_PARITY=0
@@ -598,8 +599,8 @@ CONFIG_UART4_2STOP=0
#
# USART6 Configuration
#
CONFIG_USART6_RXBUFSIZE=512
CONFIG_USART6_TXBUFSIZE=512
CONFIG_USART6_RXBUFSIZE=400
CONFIG_USART6_TXBUFSIZE=400
CONFIG_USART6_BAUD=57600
CONFIG_USART6_BITS=8
CONFIG_USART6_PARITY=0
@@ -610,8 +611,8 @@ CONFIG_USART6_2STOP=0
#
# UART7 Configuration
#
CONFIG_UART7_RXBUFSIZE=512
CONFIG_UART7_TXBUFSIZE=512
CONFIG_UART7_RXBUFSIZE=400
CONFIG_UART7_TXBUFSIZE=400
CONFIG_UART7_BAUD=57600
CONFIG_UART7_BITS=8
CONFIG_UART7_PARITY=0
@@ -622,8 +623,8 @@ CONFIG_UART7_2STOP=0
#
# UART8 Configuration
#
CONFIG_UART8_RXBUFSIZE=512
CONFIG_UART8_TXBUFSIZE=512
CONFIG_UART8_RXBUFSIZE=400
CONFIG_UART8_TXBUFSIZE=400
CONFIG_UART8_BAUD=57600
CONFIG_UART8_BITS=8
CONFIG_UART8_PARITY=0
@@ -665,8 +666,8 @@ CONFIG_CDCACM_EPBULKIN_HSSIZE=512
CONFIG_CDCACM_NWRREQS=4
CONFIG_CDCACM_NRDREQS=4
CONFIG_CDCACM_BULKIN_REQLEN=96
CONFIG_CDCACM_RXBUFSIZE=1000
CONFIG_CDCACM_TXBUFSIZE=8000
CONFIG_CDCACM_RXBUFSIZE=600
CONFIG_CDCACM_TXBUFSIZE=5000
CONFIG_CDCACM_VENDORID=0x26ac
CONFIG_CDCACM_PRODUCTID=0x0011
CONFIG_CDCACM_VENDORSTR="3D Robotics"
src/drivers/drv_pwm_output.h
+2 -2
View File
@@ -84,7 +84,7 @@ __BEGIN_DECLS
/**
* Highest maximum PWM in us
*/
#define PWM_HIGHEST_MAX 2100
#define PWM_HIGHEST_MAX 2150
/**
* Default maximum PWM in us
@@ -94,7 +94,7 @@ __BEGIN_DECLS
/**
* Lowest PWM allowed as the maximum PWM
*/
#define PWM_LOWEST_MAX 1400
#define PWM_LOWEST_MAX 950
/**
* Do not output a channel with this value
src/drivers/drv_tone_alarm.h
+1
View File
@@ -152,6 +152,7 @@ enum {
TONE_EKF_WARNING_TUNE,
TONE_BARO_WARNING_TUNE,
TONE_SINGLE_BEEP_TUNE,
TONE_HOME_SET,
TONE_NUMBER_OF_TUNES
};
src/drivers/hil/hil.cpp
+83 -15
View File
@@ -280,36 +280,42 @@ HIL::set_mode(Mode mode)
debug("MODE_2PWM");
/* multi-port with flow control lines as PWM */
_update_rate = 50; /* default output rate */
_num_outputs = 2;
break;
case MODE_4PWM:
debug("MODE_4PWM");
/* multi-port as 4 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 4;
break;
case MODE_8PWM:
debug("MODE_8PWM");
/* multi-port as 8 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 8;
break;
case MODE_12PWM:
debug("MODE_12PWM");
/* multi-port as 12 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 12;
break;
case MODE_16PWM:
debug("MODE_16PWM");
/* multi-port as 16 PWM outs */
_update_rate = 50; /* default output rate */
_num_outputs = 16;
break;
case MODE_NONE:
debug("MODE_NONE");
/* disable servo outputs and set a very low update rate */
_update_rate = 10;
_num_outputs = 0;
break;
default:
@@ -480,13 +486,6 @@ HIL::ioctl(device::file_t *filp, int cmd, unsigned long arg)
{
int ret;
debug("ioctl 0x%04x 0x%08x", cmd, arg);
// /* try it as a GPIO ioctl first */
// ret = HIL::gpio_ioctl(filp, cmd, arg);
// if (ret != -ENOTTY)
// return ret;
/* if we are in valid PWM mode, try it as a PWM ioctl as well */
switch(_mode) {
case MODE_2PWM:
@@ -540,6 +539,62 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
// HIL always outputs at the alternate (usually faster) rate
break;
case PWM_SERVO_GET_DEFAULT_UPDATE_RATE:
*(uint32_t *)arg = 400;
break;
case PWM_SERVO_GET_UPDATE_RATE:
*(uint32_t *)arg = 400;
break;
case PWM_SERVO_GET_SELECT_UPDATE_RATE:
*(uint32_t *)arg = 0;
break;
case PWM_SERVO_GET_FAILSAFE_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 850;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_DISARMED_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 900;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_MIN_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 1000;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_GET_MAX_PWM: {
struct pwm_output_values *pwm = (struct pwm_output_values *)arg;
for (unsigned i = 0; i < _num_outputs; i++) {
pwm->values[i] = 2000;
}
pwm->channel_count = _num_outputs;
break;
}
case PWM_SERVO_SET(2):
case PWM_SERVO_SET(3):
if (_mode != MODE_4PWM) {
@@ -560,18 +615,26 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
case PWM_SERVO_GET(2):
case PWM_SERVO_GET(7):
case PWM_SERVO_GET(6):
case PWM_SERVO_GET(5):
case PWM_SERVO_GET(4):
if (_num_outputs < 8) {
ret = -EINVAL;
break;
}
case PWM_SERVO_GET(3):
if (_mode != MODE_4PWM) {
case PWM_SERVO_GET(2):
if (_num_outputs < 4) {
ret = -EINVAL;
break;
}
/* FALLTHROUGH */
case PWM_SERVO_GET(0):
case PWM_SERVO_GET(1): {
// channel = cmd - PWM_SERVO_SET(0);
// *(servo_position_t *)arg = up_pwm_servo_get(channel);
case PWM_SERVO_GET(1):
case PWM_SERVO_GET(0): {
*(servo_position_t *)arg = 1500;
break;
}
@@ -583,11 +646,16 @@ HIL::pwm_ioctl(device::file_t *filp, int cmd, unsigned long arg)
break;
}
case PWM_SERVO_GET_COUNT:
case MIXERIOCGETOUTPUTCOUNT:
if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
if (_mode == MODE_8PWM) {
*(unsigned *)arg = 8;
} else if (_mode == MODE_4PWM) {
*(unsigned *)arg = 4;
} else {
*(unsigned *)arg = 2;
}
src/drivers/hmc5883/hmc5883.cpp
+5 -5
View File
@@ -1124,8 +1124,8 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
}
}
/* read the sensor up to 50x, stopping when we have 10 good values */
for (uint8_t i = 0; i < 50 && good_count < 10; i++) {
/* read the sensor up to 100x, stopping when we have 30 good values */
for (uint8_t i = 0; i < 100 && good_count < 30; i++) {
struct pollfd fds;
/* wait for data to be ready */
@@ -1172,9 +1172,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
scaling[2] = sum_excited[2] / good_count;
/* set scaling in device */
mscale_previous.x_scale = scaling[0];
mscale_previous.y_scale = scaling[1];
mscale_previous.z_scale = scaling[2];
mscale_previous.x_scale = 1.0f / scaling[0];
mscale_previous.y_scale = 1.0f / scaling[1];
mscale_previous.z_scale = 1.0f / scaling[2];
ret = OK;
src/drivers/ms5611/ms5611_nuttx.cpp
+18 -10
View File
@@ -154,10 +154,12 @@ protected:
/**
* Initialize the automatic measurement state machine and start it.
*
* @param delay_ticks the number of queue ticks before executing the next cycle
*
* @note This function is called at open and error time. It might make sense
* to make it more aggressive about resetting the bus in case of errors.
*/
void start_cycle();
void start_cycle(unsigned delay_ticks = 1);
/**
* Stop the automatic measurement state machine.
@@ -515,7 +517,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
}
void
MS5611::start_cycle()
MS5611::start_cycle(unsigned delay_ticks)
{
/* reset the report ring and state machine */
@@ -524,7 +526,7 @@ MS5611::start_cycle()
_reports->flush();
/* schedule a cycle to start things */
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, 1);
work_queue(HPWORK, &_work, (worker_t)&MS5611::cycle_trampoline, this, delay_ticks);
}
void
@@ -564,8 +566,11 @@ MS5611::cycle()
}
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
start_cycle();
/* reset the collection state machine and try again - we need
* to wait 2.8 ms after issuing the sensor reset command
* according to the MS5611 datasheet
*/
start_cycle(USEC2TICK(2800));
return;
}
@@ -594,7 +599,6 @@ MS5611::cycle()
/* measurement phase */
ret = measure();
if (ret != OK) {
//log("measure error %d", ret);
/* issue a reset command to the sensor */
_interface->ioctl(IOCTL_RESET, dummy);
/* reset the collection state machine and try again */
@@ -1182,26 +1186,30 @@ ms5611_main(int argc, char *argv[])
/*
* Start/load the driver.
*/
if (!strcmp(verb, "start"))
if (!strcmp(verb, "start")) {
ms5611::start(busid);
}
/*
* Test the driver/device.
*/
if (!strcmp(verb, "test"))
if (!strcmp(verb, "test")) {
ms5611::test(busid);
}
/*
* Reset the driver.
*/
if (!strcmp(verb, "reset"))
if (!strcmp(verb, "reset")) {
ms5611::reset(busid);
}
/*
* Print driver information.
*/
if (!strcmp(verb, "info"))
if (!strcmp(verb, "info")) {
ms5611::info();
}
/*
* Perform MSL pressure calibration given an altitude in metres
src/drivers/stm32/tone_alarm/tone_alarm.cpp
+2
View File
@@ -339,6 +339,7 @@ ToneAlarm::ToneAlarm() :
_default_tunes[TONE_EKF_WARNING_TUNE] = "MFT255L8ddd#d#eeff"; // ekf warning
_default_tunes[TONE_BARO_WARNING_TUNE] = "MFT255L4gf#fed#d"; // baro warning
_default_tunes[TONE_SINGLE_BEEP_TUNE] = "MFT100a8"; // single beep
_default_tunes[TONE_HOME_SET] = "MFT100L4>G#6A#6B#4";
_tune_names[TONE_STARTUP_TUNE] = "startup"; // startup tune
_tune_names[TONE_ERROR_TUNE] = "error"; // ERROR tone
@@ -354,6 +355,7 @@ ToneAlarm::ToneAlarm() :
_tune_names[TONE_EKF_WARNING_TUNE] = "ekf_warning"; // ekf warning
_tune_names[TONE_BARO_WARNING_TUNE] = "baro_warning"; // baro warning
_tune_names[TONE_SINGLE_BEEP_TUNE] = "beep"; // single beep
_tune_names[TONE_HOME_SET] = "home_set";
}
ToneAlarm::~ToneAlarm()
src/lib/external_lgpl/tecs/tecs.cpp
+1 -1
View File
@@ -89,7 +89,7 @@ void TECS::update_50hz(float baro_altitude, float airspeed, const math::Matrix<3
// take 5 point moving average
//_vel_dot = _vdot_filter.apply(temp);
// XXX resolve this properly
_vel_dot = 0.9f * _vel_dot + 0.1f * temp;
_vel_dot = 0.95f * _vel_dot + 0.05f * temp;
} else {
_vel_dot = 0.0f;
src/modules/commander/calibration_messages.h
+1 -1
View File
@@ -49,7 +49,7 @@
// instead of visual calibration until such a time as QGC is update to the new version.
// The number in the cal started message is used to indicate the version stamp for the current calibration code.
#define CAL_QGC_STARTED_MSG "[cal] calibration started: 1 %s"
#define CAL_QGC_STARTED_MSG "[cal] calibration started: 2 %s"
#define CAL_QGC_DONE_MSG "[cal] calibration done: %s"
#define CAL_QGC_FAILED_MSG "[cal] calibration failed: %s"
#define CAL_QGC_WARNING_MSG "[cal] calibration warning: %s"
src/modules/commander/calibration_routines.cpp
+3 -3
View File
@@ -244,7 +244,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
const float normal_still_thr = 0.25; // normal still threshold
float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2);
float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2
hrt_abstime still_time = lenient_still_position ? 1000000 : 1500000; // still time required in us
hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us
px4_pollfd_struct_t fds[1];
fds[0].fd = accel_sub;
@@ -325,7 +325,7 @@ enum detect_orientation_return detect_orientation(int mavlink_fd, int cancel_sub
/* not still, reset still start time */
if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
usleep(500000);
usleep(200000);
t_still = 0;
}
}
@@ -489,7 +489,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
// Note that this side is complete
side_data_collected[orient] = true;
tune_neutral(true);
usleep(500000);
usleep(200000);
}
if (sub_accel >= 0) {
src/modules/commander/commander.cpp
+36 -12
View File
@@ -185,8 +185,6 @@ static unsigned int leds_counter;
/* To remember when last notification was sent */
static uint64_t last_print_mode_reject_time = 0;
static float takeoff_alt = 5.0f;
static int parachute_enabled = 0;
static float eph_threshold = 5.0f;
static float epv_threshold = 10.0f;
@@ -197,6 +195,8 @@ static struct vehicle_control_mode_s control_mode;
static struct offboard_control_mode_s offboard_control_mode;
static struct home_position_s _home;
static unsigned _last_mission_instance = 0;
/**
* The daemon app only briefly exists to start
* the background job. The stack size assigned in the
@@ -457,7 +457,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// For HIL platforms, require that simulated sensors are connected
if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
if (arm && hrt_absolute_time() > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL &&
is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
return TRANSITION_DENIED;
}
@@ -847,7 +848,7 @@ static void commander_set_home_position(orb_advert_t &homePub, home_position_s &
//Play tune first time we initialize HOME
if (!status.condition_home_position_valid) {
tune_positive(true);
tune_home_set(true);
}
/* mark home position as set */
@@ -866,8 +867,6 @@ int commander_thread_main(int argc, char *argv[])
param_t _param_sys_type = param_find("MAV_TYPE");
param_t _param_system_id = param_find("MAV_SYS_ID");
param_t _param_component_id = param_find("MAV_COMP_ID");
param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
param_t _param_enable_parachute = param_find("NAV_PARACHUTE_EN");
param_t _param_enable_datalink_loss = param_find("COM_DL_LOSS_EN");
param_t _param_datalink_loss_timeout = param_find("COM_DL_LOSS_T");
param_t _param_rc_loss_timeout = param_find("COM_RC_LOSS_T");
@@ -1285,11 +1284,7 @@ int commander_thread_main(int argc, char *argv[])
rc_calibration_check(mavlink_fd);
}
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
/* Safety parameters */
param_get(_param_enable_parachute, &parachute_enabled);
param_get(_param_enable_datalink_loss, &datalink_loss_enabled);
param_get(_param_datalink_loss_timeout, &datalink_loss_timeout);
param_get(_param_rc_loss_timeout, &rc_loss_timeout);
@@ -1778,6 +1773,32 @@ int commander_thread_main(int argc, char *argv[])
}
} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
/* Only evaluate mission state if home is set,
* this prevents false positives for the mission
* rejection. Back off 2 seconds to not overlay
* home tune.
*/
if (status.condition_home_position_valid &&
(hrt_elapsed_time(&_home.timestamp) > 2000000) &&
_last_mission_instance != mission_result.instance_count) {
if (!mission_result.valid) {
/* the mission is invalid */
tune_mission_fail(true);
warnx("mission fail");
} else if (mission_result.warning) {
/* the mission has a warning */
tune_mission_fail(true);
warnx("mission warning");
} else {
/* the mission is valid */
tune_mission_ok(true);
warnx("mission ok");
}
/* prevent further feedback until the mission changes */
_last_mission_instance = mission_result.instance_count;
}
/* RC input check */
if (!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF) && !status.rc_input_blocked && sp_man.timestamp != 0 &&
hrt_absolute_time() < sp_man.timestamp + (uint64_t)(rc_loss_timeout * 1e6f)) {
@@ -2510,12 +2531,15 @@ set_control_mode()
control_mode.flag_control_termination_enabled = false;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER:
/* override is not ok for the RTL and recovery mode */
control_mode.flag_external_manual_override_ok = false;
/* fallthrough */
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL:
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = true;
control_mode.flag_control_rates_enabled = true;
src/modules/commander/commander_helper.cpp
+33
View File
@@ -174,6 +174,39 @@ void set_tune(int tune)
}
}
void tune_home_set(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_HOME_SET);
}
}
void tune_mission_ok(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
}
}
void tune_mission_fail(bool use_buzzer)
{
blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
rgbled_set_color(RGBLED_COLOR_GREEN);
rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
if (use_buzzer) {
set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
}
}
/**
* Blink green LED and play positive tune (if use_buzzer == true).
*/
src/modules/commander/commander_helper.h
+3
View File
@@ -58,6 +58,9 @@ void buzzer_deinit(void);
void set_tune_override(int tune);
void set_tune(int tune);
void tune_home_set(bool use_buzzer);
void tune_mission_ok(bool use_buzzer);
void tune_mission_fail(bool use_buzzer);
void tune_positive(bool use_buzzer);
void tune_neutral(bool use_buzzer);
void tune_negative(bool use_buzzer);
src/modules/commander/mag_calibration.cpp
+95 -21
View File
@@ -68,6 +68,8 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
static constexpr float mag_sphere_radius = 0.2f;
static const unsigned int calibration_sides = 6;
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
@@ -79,7 +81,7 @@ typedef struct {
unsigned int calibration_points_perside;
unsigned int calibration_interval_perside_seconds;
uint64_t calibration_interval_perside_useconds;
unsigned int calibration_counter_total;
unsigned int calibration_counter_total[max_mags];
bool side_data_collected[detect_orientation_side_count];
float* x[max_mags];
float* y[max_mags];
@@ -187,6 +189,24 @@ int do_mag_calibration(int mavlink_fd)
return result;
}
static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
{
float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
for (size_t i = 0; i < count; i++) {
float dx = sx - x[i];
float dy = sy - y[i];
float dz = sz - z[i];
float dist = sqrtf(dx * dx + dy * dy + dz * dz);
if (dist < min_sample_dist) {
return true;
}
}
return false;
}
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
calibrate_return result = calibrate_return_ok;
@@ -289,27 +309,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
int poll_ret = px4_poll(fds, fd_count, 1000);
if (poll_ret > 0) {
int prev_count[max_mags];
bool rejected = false;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
if (worker_data->sub_mag[cur_mag] >= 0) {
struct mag_report mag;
orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
// Check if this measurement is good to go in
rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
worker_data->calibration_counter_total[cur_mag],
calibration_sides * worker_data->calibration_points_perside);
worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
worker_data->calibration_counter_total[cur_mag]++;
}
}
worker_data->calibration_counter_total++;
calibration_counter_side++;
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
// Keep calibration of all mags in lockstep
if (rejected) {
// Reset counts, since one of the mags rejected the measurement
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
}
} else {
calibration_counter_side++;
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
"[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
}
} else {
poll_errcount++;
}
@@ -339,8 +379,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.mavlink_fd = mavlink_fd;
worker_data.done_count = 0;
worker_data.calibration_counter_total = 0;
worker_data.calibration_points_perside = 80;
worker_data.calibration_points_perside = 40;
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
@@ -348,9 +387,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = false;
worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = false;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
@@ -360,9 +399,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
worker_data.x[cur_mag] = NULL;
worker_data.y[cur_mag] = NULL;
worker_data.z[cur_mag] = NULL;
worker_data.calibration_counter_total[cur_mag] = 0;
}
const unsigned int calibration_sides = 3;
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
char str[30];
@@ -441,7 +480,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
// Mag in this slot is available and we should have values for it to calibrate
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
worker_data.calibration_counter_total,
worker_data.calibration_counter_total[cur_mag],
100, 0.0f,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
&sphere_radius[cur_mag]);
@@ -453,6 +492,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
}
}
}
// Print uncalibrated data points
if (result == calibrate_return_ok) {
printf("RAW DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
float x = worker_data.x[cur_mag][i];
float y = worker_data.y[cur_mag][i];
float z = worker_data.z[cur_mag][i];
printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
}
printf(">>>>>>>\n");
}
printf("CALIBRATED DATA:\n--------------------\n");
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
}
printf("SPHERE RADIUS: %8.4f\n", (double)sphere_radius[cur_mag]);
printf(">>>>>>>\n");
}
}
// Data points are no longer needed
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
src/modules/commander/state_machine_helper.cpp
+107 -123
View File
@@ -39,7 +39,6 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
@@ -131,12 +130,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
prearm_ret = prearm_check(status, mavlink_fd);
}
#ifdef __PX4_NUTTX
/*
* Perform an atomic state update
*/
#ifdef __PX4_NUTTX
irqstate_t flags = irqsave();
#endif
#endif
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
@@ -262,10 +261,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
status->arming_state = new_arming_state;
}
#ifdef __PX4_NUTTX
/* end of atomic state update */
#ifdef __PX4_NUTTX
irqrestore(flags);
#endif
#endif
}
if (ret == TRANSITION_DENIED) {
@@ -366,125 +365,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
return ret;
}
#ifdef __PX4_NUTTX
static transition_result_t disable_publication(const int mavlink_fd)
{
transition_result_t ret;
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) {
/* skip serial ports */
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
}
snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
int sensfd = ::open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
return ret;
}
#else
static transition_result_t disable_publication(const int mavlink_fd)
{
transition_result_t ret;
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
break;
/* skip mavlink */
if (!strcmp("/dev/mavlink", devname)) {
continue;
}
int sensfd = px4_open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
return ret;
}
#endif
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -495,7 +379,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
@@ -503,7 +387,107 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
ret = disable_publication(mavlink_fd);
#ifdef __PX4_NUTTX
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) {
/* skip serial ports */
if (!strncmp("tty", direntry->d_name, 3)) {
continue;
}
/* skip mtd devices */
if (!strncmp("mtd", direntry->d_name, 3)) {
continue;
}
/* skip ram devices */
if (!strncmp("ram", direntry->d_name, 3)) {
continue;
}
/* skip MMC devices */
if (!strncmp("mmc", direntry->d_name, 3)) {
continue;
}
/* skip mavlink */
if (!strcmp("mavlink", direntry->d_name)) {
continue;
}
/* skip console */
if (!strcmp("console", direntry->d_name)) {
continue;
}
/* skip null */
if (!strcmp("null", direntry->d_name)) {
continue;
}
snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name);
int sensfd = ::open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
#else
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
break;
/* skip mavlink */
if (!strcmp("/dev/mavlink", devname)) {
continue;
}
int sensfd = px4_open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
#endif
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;
src/modules/dataman/dataman.c
+1 -1
View File
@@ -798,7 +798,7 @@ start(void)
sem_init(&g_init_sema, 1, 0);
/* start the worker thread */
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1800, task_main, NULL)) <= 0) {
if ((task = px4_task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, 1500, task_main, NULL)) <= 0) {
warn("task start failed");
return -1;
}
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+19 -10
View File
@@ -229,6 +229,10 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
_parameter_handles.eas_noise = param_find("PE_EAS_NOISE");
_parameter_handles.pos_stddev_threshold = param_find("PE_POSDEV_INIT");
/* indicate consumers that the current position data is not valid */
_gps.eph = 10000.0f;
_gps.epv = 10000.0f;
/* fetch initial parameter values */
parameters_update();
@@ -700,21 +704,20 @@ void AttitudePositionEstimatorEKF::task_main()
continue;
}
//Run EKF data fusion steps
// Run EKF data fusion steps
updateSensorFusion(_gpsIsGood, _newDataMag, _newRangeData, _newHgtData, _newAdsData);
//Publish attitude estimations
// Publish attitude estimations
publishAttitude();
//Publish Local Position estimations
// Publish Local Position estimations
publishLocalPosition();
//Publish Global Position, but only if it's any good
if (_gps_initialized && (_gpsIsGood || _global_pos.dead_reckoning)) {
publishGlobalPosition();
}
// Publish Global Position, it will have a large uncertainty
// set if only altitude is known
publishGlobalPosition();
//Publish wind estimates
// Publish wind estimates
if (hrt_elapsed_time(&_wind.timestamp) > 99000) {
publishWindEstimate();
}
@@ -903,6 +906,10 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
_global_pos.lat = est_lat;
_global_pos.lon = est_lon;
_global_pos.time_utc_usec = _gps.time_utc_usec;
} else {
_global_pos.lat = 0.0;
_global_pos.lon = 0.0;
_global_pos.time_utc_usec = 0;
}
if (_local_pos.v_xy_valid) {
@@ -919,6 +926,8 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
if (_local_pos.v_z_valid) {
_global_pos.vel_d = _local_pos.vz;
} else {
_global_pos.vel_d = 0.0f;
}
/* terrain altitude */
@@ -1074,7 +1083,7 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
}
// Fuse Airspeed Measurements
if (fuseAirSpeed && _ekf->VtasMeas > 7.0f) {
if (fuseAirSpeed && _airspeed.true_airspeed_m_s > 5.0f) {
_ekf->fuseVtasData = true;
_ekf->RecallStates(_ekf->statesAtVtasMeasTime,
(IMUmsec - _parameters.tas_delay_ms)); // assume 100 msec avg delay for airspeed data
@@ -1332,7 +1341,7 @@ void AttitudePositionEstimatorEKF::pollData()
orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
perf_count(_perf_airspeed);
_ekf->VtasMeas = _airspeed.true_airspeed_m_s;
_ekf->VtasMeas = _airspeed.true_airspeed_unfiltered_m_s;
}
src/modules/fw_att_control/fw_att_control_main.cpp
+19 -12
View File
@@ -193,12 +193,14 @@ private:
float trim_roll;
float trim_pitch;
float trim_yaw;
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
float rollsp_offset_deg; /**< Roll Setpoint Offset in deg */
float pitchsp_offset_deg; /**< Pitch Setpoint Offset in deg */
float rollsp_offset_rad; /**< Roll Setpoint Offset in rad */
float pitchsp_offset_rad; /**< Pitch Setpoint Offset in rad */
float man_roll_max; /**< Max Roll in rad */
float man_pitch_max; /**< Max Pitch in rad */
int vtol_type; /**< VTOL type: 0 = tailsitter, 1 = tiltrotor */
} _parameters; /**< local copies of interesting parameters */
@@ -241,6 +243,8 @@ private:
param_t man_roll_max;
param_t man_pitch_max;
param_t vtol_type;
} _parameter_handles; /**< handles for interesting parameters */
@@ -404,6 +408,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.man_roll_max = param_find("FW_MAN_R_MAX");
_parameter_handles.man_pitch_max = param_find("FW_MAN_P_MAX");
_parameter_handles.vtol_type = param_find("VT_TYPE");
/* fetch initial parameter values */
parameters_update();
}
@@ -481,6 +487,8 @@ FixedwingAttitudeControl::parameters_update()
_parameters.man_roll_max = math::radians(_parameters.man_roll_max);
_parameters.man_pitch_max = math::radians(_parameters.man_pitch_max);
param_get(_parameter_handles.vtol_type, &_parameters.vtol_type);
/* pitch control parameters */
_pitch_ctrl.set_time_constant(_parameters.tconst);
_pitch_ctrl.set_k_p(_parameters.p_p);
@@ -634,8 +642,9 @@ FixedwingAttitudeControl::task_main()
/* rate limit vehicle status updates to 5Hz */
orb_set_interval(_vcontrol_mode_sub, 200);
/* rate limit attitude control to 50 Hz (with some margin, so 17 ms) */
orb_set_interval(_att_sub, 17);
/* do not limit the attitude updates in order to minimize latency.
* actuator outputs are still limited by the individual drivers
* properly to not saturate IO or physical limitations */
parameters_update();
@@ -702,10 +711,8 @@ FixedwingAttitudeControl::task_main()
/* load local copies */
orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
if (_vehicle_status.is_vtol) {
/* vehicle type is VTOL, need to modify attitude!
* The following modification to the attitude is vehicle specific and in this case applies
* to tail-sitter models !!!
if (_vehicle_status.is_vtol && _parameters.vtol_type == 0) {
/* vehicle is a tailsitter, we need to modify the estimated attitude for fw mode
*
* Since the VTOL airframe is initialized as a multicopter we need to
* modify the estimated attitude for the fixed wing operation.
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+139 -47
View File
@@ -98,10 +98,12 @@ static int _control_task = -1; /**< task handle for sensor task */
#define HDG_HOLD_REACHED_DIST 1000.0f // distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
#define HDG_HOLD_SET_BACK_DIST 100.0f // distance by which previous waypoint is set behind the plane
#define HDG_HOLD_YAWRATE_THRESH 0.1f // max yawrate at which plane locks yaw for heading hold mode
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
#define THROTTLE_THRESH 0.05f // max throttle from user which will not lead to motors spinning up in altitude controlled modes
#define HDG_HOLD_MAN_INPUT_THRESH 0.01f // max manual roll input from user which does not change the locked heading
#define TAKEOFF_IDLE 0.2f // idle speed for POSCTRL/ATTCTRL (when landed and throttle stick > 0)
static constexpr float THROTTLE_THRESH = 0.05f; ///< max throttle from user which will not lead to motors spinning up in altitude controlled modes
static constexpr float MANUAL_THROTTLE_CLIMBOUT_THRESH = 0.85f; ///< a throttle / pitch input above this value leads to the system switching to climbout mode
static constexpr float ALTHOLD_EPV_RESET_THRESH = 5.0f;
/**
* L1 control app start / stop handling function
@@ -173,10 +175,10 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
float _hold_alt; /**< hold altitude for altitude mode */
float _ground_alt; /**< ground altitude at which plane was launched */
float _takeoff_ground_alt; /**< ground altitude at which plane was launched */
float _hdg_hold_yaw; /**< hold heading for velocity mode */
bool _hdg_hold_enabled; /**< heading hold enabled */
bool _yaw_lock_engaged; /**< yaw is locked for heading hold */
bool _yaw_lock_engaged; /**< yaw is locked for heading hold */
struct position_setpoint_s _hdg_hold_prev_wp; /**< position where heading hold started */
struct position_setpoint_s _hdg_hold_curr_wp; /**< position to which heading hold flies */
hrt_abstime _control_position_last_called; /**<last call of control_position */
@@ -370,7 +372,7 @@ private:
/**
* Publish navigation capabilities
*/
void navigation_capabilities_publish();
void navigation_capabilities_publish();
/**
* Get a new waypoint based on heading and distance from current position
@@ -386,27 +388,36 @@ private:
/**
* Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
*/
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);
/**
* Check if we are in a takeoff situation
*/
bool in_takeoff_situation();
/**
* Do takeoff help when in altitude controlled modes
* @param hold_altitude altitude setpoint for controller
* @param pitch_limit_min minimum pitch allowed
*/
void do_takeoff_help(float *hold_altitude, float *pitch_limit_min);
/**
* Update desired altitude base on user pitch stick input
*
* @param dt Time step
* @return true if climbout mode was requested by user (climb with max rate and min airspeed)
*/
bool update_desired_altitude(float dt);
/**
* Control position.
*/
/**
* Do takeoff help when in altitude controlled modes
*/
void do_takeoff_help();
/**
* Update desired altitude base on user pitch stick input
*/
void update_desired_altitude(float dt);
bool control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
const struct position_setpoint_triplet_s &_pos_sp_triplet);
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
float calculate_target_airspeed(float airspeed_demand);
void calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed_2d, const struct position_setpoint_triplet_s &pos_sp_triplet);
/**
* Shim for calling task_main from task_create.
@@ -421,12 +432,12 @@ private:
/*
* Reset takeoff state
*/
void reset_takeoff_state();
void reset_takeoff_state();
/*
* Reset landing state
*/
void reset_landing_state();
void reset_landing_state();
/*
* Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
@@ -493,7 +504,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
_hold_alt(0.0f),
_ground_alt(0.0f),
_takeoff_ground_alt(0.0f),
_hdg_hold_yaw(0.0f),
_hdg_hold_enabled(false),
_yaw_lock_engaged(false),
@@ -955,16 +966,35 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
}
}
void FixedwingPositionControl::update_desired_altitude(float dt)
bool FixedwingPositionControl::update_desired_altitude(float dt)
{
const float deadBand = (60.0f/1000.0f);
const float factor = 1.0f - deadBand;
// XXX this should go into a manual stick mapper
// class
static float _althold_epv = 0.0f;
static bool was_in_deadband = false;
bool climbout_mode = false;
/*
* Reset the hold altitude to the current altitude if the uncertainty
* changes significantly.
* This is to guard against uncommanded altitude changes
* when the altitude certainty increases or decreases.
*/
if (fabsf(_althold_epv - _global_pos.epv) > ALTHOLD_EPV_RESET_THRESH) {
_hold_alt = _global_pos.alt;
_althold_epv = _global_pos.epv;
}
// XXX the sign magic in this function needs to be fixed
if (_manual.x > deadBand) {
float pitch = (_manual.x - deadBand) / factor;
_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
was_in_deadband = false;
climbout_mode = (fabsf(_manual.x) > MANUAL_THROTTLE_CLIMBOUT_THRESH);
} else if (_manual.x < - deadBand) {
float pitch = (_manual.x + deadBand) / factor;
_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
@@ -974,19 +1004,32 @@ void FixedwingPositionControl::update_desired_altitude(float dt)
* The aircraft should immediately try to fly at this altitude
* as this is what the pilot expects when he moves the stick to the center */
_hold_alt = _global_pos.alt;
_althold_epv = _global_pos.epv;
was_in_deadband = true;
}
return climbout_mode;
}
void FixedwingPositionControl::do_takeoff_help()
{
bool FixedwingPositionControl::in_takeoff_situation() {
const hrt_abstime delta_takeoff = 10000000;
const float throttle_threshold = 0.3f;
const float delta_alt_takeoff = 30.0f;
const float throttle_threshold = 0.1f;
/* demand 30 m above ground if user switched into this mode during takeoff */
if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) {
_hold_alt = _ground_alt + delta_alt_takeoff;
if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _takeoff_ground_alt + _parameters.climbout_diff) {
return true;
}
return false;
}
void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitch_limit_min)
{
/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
if (in_takeoff_situation()) {
*hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff;
*pitch_limit_min = math::radians(10.0f);
} else {
*pitch_limit_min = _parameters.pitch_limit_min;
}
}
@@ -1011,7 +1054,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
math::Vector<3> accel_earth = _R_nb * accel_body;
if (!_mTecs.getEnabled()) {
if (!_mTecs.getEnabled() && !_vehicle_status.condition_landed) {
_tecs.update_50hz(_global_pos.alt /* XXX might switch to alt err here */, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
}
@@ -1025,7 +1068,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
if (!_was_in_air && !_vehicle_status.condition_landed) {
_was_in_air = true;
_time_went_in_air = hrt_absolute_time();
_ground_alt = _global_pos.alt;
_takeoff_ground_alt = _global_pos.alt;
}
/* reset flag when airplane landed */
if (_vehicle_status.condition_landed) {
@@ -1085,7 +1128,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust = 0.0f;
_att_sp.roll_body = 0.0f;
_att_sp.pitch_body = 0.0f;
} else if (pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_POSITION) {
/* waypoint is a plain navigation waypoint */
_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
_att_sp.roll_body = _l1_control.nav_roll();
@@ -1104,6 +1152,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
}
tecs_update_pitch_throttle(_pos_sp_triplet.current.alt, calculate_target_airspeed(_parameters.airspeed_trim), eas2tas,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1270,7 +1324,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* Inform user that launchdetection is running */
static hrt_abstime last_sent = 0;
if(hrt_absolute_time() - last_sent > 4e6) {
mavlink_log_info(_mavlink_fd, "#audio: Launchdetection running");
mavlink_log_critical(_mavlink_fd, "Launchdetection running");
last_sent = hrt_absolute_time();
}
@@ -1399,10 +1453,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_manual.z;
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
bool climbout_requested = update_desired_altitude(dt);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground
* and set limit to pitch angle to prevent stearing into ground
*/
float pitch_limit_min;
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
do_takeoff_help();
/* throttle limiting */
throttle_max = _parameters.throttle_max;
@@ -1418,8 +1476,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min,
throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
climbout_requested,
pitch_limit_min,
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@@ -1434,6 +1492,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
}
/* user tries to do a takeoff in heading hold mode, reset the yaw setpoint on every iteration
to make sure the plane does not start rolling
*/
if (in_takeoff_situation()) {
_hdg_hold_enabled = false;
_yaw_lock_engaged = true;
}
if (_yaw_lock_engaged) {
/* just switched back from non heading-hold to heading hold */
@@ -1463,6 +1529,12 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
if (in_takeoff_situation()) {
/* limit roll motion to ensure enough lift */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-15.0f),
math::radians(15.0f));
}
}
} else {
_hdg_hold_enabled = false;
@@ -1492,10 +1564,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_manual.z;
/* update desired altitude based on user pitch stick input */
update_desired_altitude(dt);
bool climbout_requested = update_desired_altitude(dt);
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
do_takeoff_help();
/* if we assume that user is taking off then help by demanding altitude setpoint well above ground
* and set limit to pitch angle to prevent stearing into ground
*/
float pitch_limit_min;
do_takeoff_help(&_hold_alt, &pitch_limit_min);
/* throttle limiting */
throttle_max = _parameters.throttle_max;
@@ -1511,8 +1586,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_parameters.throttle_min,
throttle_max,
_parameters.throttle_cruise,
false,
math::radians(_parameters.pitch_limit_min),
climbout_requested,
pitch_limit_min,
_global_pos.alt,
ground_speed,
tecs_status_s::TECS_MODE_NORMAL);
@@ -1544,10 +1619,22 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* making sure again that the correct thrust is used,
* without depending on library calls for safety reasons */
_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
} else if (_control_mode_current == FW_POSCTRL_MODE_AUTO &&
pos_sp_triplet.current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) {
_att_sp.thrust = 0.0f;
} else {
/* Copy thrust and pitch values from tecs */
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max);
if (_vehicle_status.condition_landed &&
(_control_mode_current == FW_POSCTRL_MODE_POSITION || _control_mode_current == FW_POSCTRL_MODE_ALTITUDE))
{
// when we are landed in these modes we want the motor to spin
_att_sp.thrust = math::min(TAKEOFF_IDLE, throttle_max);
} else {
_att_sp.thrust = math::min(_mTecs.getEnabled() ? _mTecs.getThrottleSetpoint() :
_tecs.get_throttle_demand(), throttle_max);
}
}
/* During a takeoff waypoint while waiting for launch the pitch sp is set
@@ -1736,6 +1823,11 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
const math::Vector<3> &ground_speed,
unsigned mode, bool pitch_max_special)
{
/* do not run tecs if we are not in air */
if (_vehicle_status.condition_landed) {
return;
}
if (_mTecs.getEnabled()) {
/* Using mtecs library: prepare arguments for mtecs call */
float flightPathAngle = 0.0f;
src/modules/land_detector/FixedwingLandDetector.cpp
+2 -2
View File
@@ -85,12 +85,12 @@ bool FixedwingLandDetector::update()
bool landDetected = false;
if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
float val = 0.97f * _velocity_xy_filtered + 0.03f * sqrtf(_vehicleLocalPosition.vx *
_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
if (isfinite(val)) {
_velocity_xy_filtered = val;
}
val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
val = 0.99f * _velocity_z_filtered + 0.01f * fabsf(_vehicleLocalPosition.vz);
if (isfinite(val)) {
_velocity_z_filtered = val;
src/modules/land_detector/land_detector_params.c
+1 -1
View File
@@ -96,7 +96,7 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
*
* @group Land Detector
*/
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f);
PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 5.0f);
/**
* Fixedwing max climb rate
src/modules/mavlink/mavlink_parameters.cpp
+18 -5
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2014 PX4 Development Team. All rights reserved.
* 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
@@ -36,6 +36,7 @@
* Mavlink parameters manager implementation.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#include <stdio.h>
@@ -130,7 +131,17 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
} else {
/* when index is >= 0, send this parameter again */
send_param(param_for_used_index(req_read.param_index));
int ret = send_param(param_for_used_index(req_read.param_index));
if (ret == 1) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] unknown param ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
} else if (ret == 2) {
char buf[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN];
sprintf(buf, "[pm] failed loading param from storage ID: %u", req_read.param_index);
_mavlink->send_statustext_info(buf);
}
}
}
break;
@@ -207,11 +218,11 @@ MavlinkParametersManager::send(const hrt_abstime t)
}
}
void
int
MavlinkParametersManager::send_param(param_t param)
{
if (param == PARAM_INVALID) {
return;
return 1;
}
mavlink_param_value_t msg;
@@ -221,7 +232,7 @@ MavlinkParametersManager::send_param(param_t param)
* space during transmission, copy param onto float val_buf
*/
if (param_get(param, &msg.param_value) != OK) {
return;
return 2;
}
msg.param_count = param_count_used();
@@ -248,4 +259,6 @@ MavlinkParametersManager::send_param(param_t param)
}
_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
return 0;
}
src/modules/mavlink/mavlink_parameters.h
+3 -2
View File
@@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
* Copyright (c) 2012-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
@@ -36,6 +36,7 @@
* Mavlink parameters manager definition.
*
* @author Anton Babushkin <anton.babushkin@me.com>
* @author Lorenz Meier <lorenz@px4.io>
*/
#pragma once
@@ -113,7 +114,7 @@ protected:
void send(const hrt_abstime t);
void send_param(param_t param);
int send_param(param_t param);
orb_advert_t _rc_param_map_pub;
struct rc_parameter_map_s _rc_param_map;
src/modules/mavlink/mavlink_receiver.cpp
+46
View File
@@ -197,6 +197,10 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
handle_message_manual_control(msg);
break;
case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE:
handle_message_rc_channels_override(msg);
break;
case MAVLINK_MSG_ID_HEARTBEAT:
handle_message_heartbeat(msg);
break;
@@ -971,6 +975,48 @@ static int decode_switch_pos_n(uint16_t buttons, int sw) {
}
}
void
MavlinkReceiver::handle_message_rc_channels_override(mavlink_message_t *msg)
{
mavlink_rc_channels_override_t man;
mavlink_msg_rc_channels_override_decode(msg, &man);
// Check target
if (man.target_system != 0 && man.target_system != _mavlink->get_system_id()) {
return;
}
struct rc_input_values rc = {};
rc.timestamp_publication = hrt_absolute_time();
rc.timestamp_last_signal = rc.timestamp_publication;
rc.channel_count = 8;
rc.rc_failsafe = false;
rc.rc_lost = false;
rc.rc_lost_frame_count = 0;
rc.rc_total_frame_count = 1;
rc.rc_ppm_frame_length = 0;
rc.input_source = RC_INPUT_SOURCE_MAVLINK;
rc.rssi = RC_INPUT_RSSI_MAX;
/* channels */
rc.values[0] = man.chan1_raw;
rc.values[1] = man.chan2_raw;
rc.values[2] = man.chan3_raw;
rc.values[3] = man.chan4_raw;
rc.values[4] = man.chan5_raw;
rc.values[5] = man.chan6_raw;
rc.values[6] = man.chan7_raw;
rc.values[7] = man.chan8_raw;
if (_rc_pub <= 0) {
_rc_pub = orb_advertise(ORB_ID(input_rc), &rc);
} else {
orb_publish(ORB_ID(input_rc), _rc_pub, &rc);
}
}
void
MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
{
src/modules/mavlink/mavlink_receiver.h
+1
View File
@@ -127,6 +127,7 @@ private:
void handle_message_set_attitude_target(mavlink_message_t *msg);
void handle_message_radio_status(mavlink_message_t *msg);
void handle_message_manual_control(mavlink_message_t *msg);
void handle_message_rc_channels_override(mavlink_message_t *msg);
void handle_message_heartbeat(mavlink_message_t *msg);
void handle_message_ping(mavlink_message_t *msg);
void handle_message_request_data_stream(mavlink_message_t *msg);
src/modules/mc_att_control/mc_att_control_main.cpp
+3 -3
View File
@@ -602,10 +602,10 @@ MulticopterAttitudeControl::vehicle_motor_limits_poll()
}
}
/*
/**
* Attitude controller.
* Input: 'manual_control_setpoint' and 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp', 'vehicle_attitude_setpoint' topic (for manual modes)
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* Output: '_rates_sp' vector, '_thrust_sp'
*/
void
MulticopterAttitudeControl::control_attitude(float dt)
src/modules/mc_att_control/mc_att_control_params.c
+11 -11
View File
@@ -50,7 +50,7 @@
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.5f);
/**
* Roll rate P gain
@@ -60,7 +60,7 @@ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.12f);
/**
* Roll rate I gain
@@ -70,7 +70,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.05f);
/**
* Roll rate D gain
@@ -80,7 +80,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.003f);
/**
* Roll rate feedforward
@@ -101,7 +101,7 @@ PARAM_DEFINE_FLOAT(MC_ROLLRATE_FF, 0.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.5f);
/**
* Pitch rate P gain
@@ -111,7 +111,7 @@ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.12f);
/**
* Pitch rate I gain
@@ -121,7 +121,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.05f);
/**
* Pitch rate D gain
@@ -131,7 +131,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f);
PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.003f);
/**
* Pitch rate feedforward
@@ -152,7 +152,7 @@ PARAM_DEFINE_FLOAT(MC_PITCHRATE_FF, 0.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
PARAM_DEFINE_FLOAT(MC_YAW_P, 2.8f);
/**
* Yaw rate P gain
@@ -162,7 +162,7 @@ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.2f);
/**
* Yaw rate I gain
@@ -172,7 +172,7 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
* @min 0.0
* @group Multicopter Attitude Control
*/
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.1f);
/**
* Yaw rate D gain
src/modules/mc_pos_control/mc_pos_control_main.cpp
+19 -3
View File
@@ -1037,6 +1037,21 @@ MulticopterPositionControl::task_main()
_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
/* make sure velocity setpoint is saturated in xy*/
float vel_norm_xy = sqrtf(_vel_sp(0)*_vel_sp(0) +
_vel_sp(1)*_vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
/* note assumes vel_max(0) == vel_max(1) */
_vel_sp(0) = _vel_sp(0)*_params.vel_max(0)/vel_norm_xy;
_vel_sp(1) = _vel_sp(1)*_params.vel_max(1)/vel_norm_xy;
}
/* make sure velocity setpoint is saturated in z*/
float vel_norm_z = sqrtf(_vel_sp(2)*_vel_sp(2));
if (vel_norm_z > _params.vel_max(2)) {
_vel_sp(2) = _vel_sp(2)*_params.vel_max(2)/vel_norm_z;
}
if (!_control_mode.flag_control_altitude_enabled) {
_reset_alt_sp = true;
_vel_sp(2) = 0.0f;
@@ -1384,14 +1399,15 @@ MulticopterPositionControl::task_main()
}
//Control roll and pitch directly if we no aiding velocity controller is active
if(!_control_mode.flag_control_velocity_enabled) {
if (!_control_mode.flag_control_velocity_enabled) {
_att_sp.roll_body = _manual.y * _params.man_roll_max;
_att_sp.pitch_body = -_manual.x * _params.man_pitch_max;
}
//Control climb rate directly if no aiding altitude controller is active
if(!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
if (!_control_mode.flag_control_climb_rate_enabled) {
_att_sp.thrust = math::min(_manual.z, _params.thr_max);
_att_sp.thrust = math::max(_att_sp.thrust, _params.thr_min);
}
//Construct attitude setpoint rotation matrix
src/modules/mc_pos_control/mc_pos_control_params.c
+3 -2
View File
@@ -50,7 +50,7 @@
* @max 1.0
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.12f);
/**
* Maximum thrust
@@ -107,9 +107,10 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
*
* @unit m/s
* @min 0.0
* @max 8 m/s
* @group Multicopter Position Control
*/
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 3.0f);
/**
* Vertical velocity feed forward
src/modules/navigator/loiter.cpp
+3 -2
View File
@@ -55,7 +55,8 @@
#include "navigator.h"
Loiter::Loiter(Navigator *navigator, const char *name) :
MissionBlock(navigator, name)
MissionBlock(navigator, name),
_param_min_alt(this, "MIS_TAKEOFF_ALT", false)
{
/* load initial params */
updateParams();
@@ -74,7 +75,7 @@ void
Loiter::on_activation()
{
/* set current mission item to loiter */
set_loiter_item(&_mission_item);
set_loiter_item(&_mission_item, _param_min_alt.get());
/* convert mission item to current setpoint */
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
src/modules/navigator/loiter.h
+3
View File
@@ -59,6 +59,9 @@ public:
virtual void on_activation();
virtual void on_active();
private:
control::BlockParamFloat _param_min_alt;
};
#endif

Some files were not shown because too many files have changed in this diff Show More