dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Merged release_v1.0.0 branch into master

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d
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

8
ROMFS/px4fmu_common/init.d/10019_sk450_deadcat
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

3
ROMFS/px4fmu_common/init.d/13001_caipirinha_vtol
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

3
ROMFS/px4fmu_common/init.d/13002_firefly6
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

18
ROMFS/px4fmu_common/init.d/13003_quad_tailsitter Normal file
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

24
ROMFS/px4fmu_common/init.d/3030_io_camflyer
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

10
ROMFS/px4fmu_common/init.d/3033_wingwing
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

8
ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha
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

4
ROMFS/px4fmu_common/init.d/4010_dji_f330
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

13
ROMFS/px4fmu_common/init.d/4012_quad_x_can
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

8
ROMFS/px4fmu_common/init.d/rc.autostart
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+
#

32
ROMFS/px4fmu_common/init.d/rc.mc_defaults
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

6
ROMFS/px4fmu_common/init.d/rc.usb
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

4
ROMFS/px4fmu_common/mixers/firefly6.aux.mix
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

18
ROMFS/px4fmu_common/mixers/quad_x_vtol.main.mix Normal file
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

174
Tools/Matlab/ellipsoid_fit.m Normal file
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

77
Tools/Matlab/plot_mag.m Normal file
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))

3
msg/airspeed.msg
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

12
msg/mission_result.msg Normal file
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

33
nuttx-configs/px4fmu-v2/nsh/defconfig
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"

4
src/drivers/drv_pwm_output.h
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

1
src/drivers/drv_tone_alarm.h
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
};

98
src/drivers/hil/hil.cpp
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;
}

10
src/drivers/hmc5883/hmc5883.cpp
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;

28
src/drivers/ms5611/ms5611_nuttx.cpp
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

2
src/drivers/stm32/tone_alarm/tone_alarm.cpp
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()

2
src/lib/external_lgpl/tecs/tecs.cpp
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;

2
src/modules/commander/calibration_messages.h
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"

6
src/modules/commander/calibration_routines.cpp
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) {

48
src/modules/commander/commander.cpp
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;

33
src/modules/commander/commander_helper.cpp
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).
*/

3
src/modules/commander/commander_helper.h
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);

116
src/modules/commander/mag_calibration.cpp
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++) {

230
src/modules/commander/state_machine_helper.cpp
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;

2
src/modules/dataman/dataman.c
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;
}

29
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
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;
}

31
src/modules/fw_att_control/fw_att_control_main.cpp
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.

186
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
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;

4
src/modules/land_detector/FixedwingLandDetector.cpp
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;

2
src/modules/land_detector/land_detector_params.c
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

23
src/modules/mavlink/mavlink_parameters.cpp
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;
}

5
src/modules/mavlink/mavlink_parameters.h
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;

46
src/modules/mavlink/mavlink_receiver.cpp
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)
{

1
src/modules/mavlink/mavlink_receiver.h
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);

6
src/modules/mc_att_control/mc_att_control_main.cpp
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)

22
src/modules/mc_att_control/mc_att_control_params.c
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

22
src/modules/mc_pos_control/mc_pos_control_main.cpp
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

5
src/modules/mc_pos_control/mc_pos_control_params.c
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

5
src/modules/navigator/loiter.cpp
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();

3
src/modules/navigator/loiter.h
View File

@ -59,6 +59,9 @@ public:
virtual void on_activation();
virtual void on_active();
private:
control::BlockParamFloat _param_min_alt;
};
#endif

139
src/modules/navigator/mission.cpp
View File

@ -78,7 +78,7 @@ Mission::Mission(Navigator *navigator, const char *name) :
_takeoff(false),
_mission_type(MISSION_TYPE_NONE),
_inited(false),
_dist_1wp_ok(false),
_home_inited(false),
_missionFeasiblityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
@ -110,6 +110,24 @@ Mission::on_inactive()
update_offboard_mission();
}
/* check if the home position became valid in the meantime */
if ((_mission_type == MISSION_TYPE_NONE || _mission_type == MISSION_TYPE_OFFBOARD) &&
!_home_inited && _navigator->home_position_valid()) {
dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
_navigator->get_mission_result()->valid = _missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
_home_inited = true;
}
} else {
/* read mission topics on initialization */
_inited = true;
@ -176,6 +194,7 @@ Mission::on_active()
&& _mission_type != MISSION_TYPE_NONE) {
heading_sp_update();
}
}
void
@ -197,6 +216,11 @@ Mission::update_onboard_mission()
/* otherwise, just leave it */
}
// XXX check validity here as well
_navigator->get_mission_result()->valid = true;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
} else {
_onboard_mission.count = 0;
_onboard_mission.current_seq = 0;
@ -232,7 +256,13 @@ Mission::update_offboard_mission()
failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
_navigator->get_home_position()->alt, _navigator->home_position_valid());
_navigator->get_home_position()->alt, _navigator->home_position_valid(),
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon,
_param_dist_1wp.get(), _navigator->get_mission_result()->warning);
_navigator->get_mission_result()->valid = !failed;
_navigator->increment_mission_instance_count();
_navigator->set_mission_result_updated();
} else {
warnx("offboard mission update failed");
@ -281,73 +311,6 @@ Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
}
}
bool
Mission::check_dist_1wp()
{
if (_dist_1wp_ok) {
/* always return true after at least one successful check */
return true;
}
/* check if first waypoint is not too far from home */
if (_param_dist_1wp.get() > 0.0f) {
if (_navigator->get_vstatus()->condition_home_position_valid) {
struct mission_item_s mission_item;
/* find first waypoint (with lat/lon) item in datamanager */
for (unsigned i = 0; i < _offboard_mission.count; i++) {
if (dm_read(DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id), i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* check only items with valid lat/lon */
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon,
_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
if (dist_to_1wp < _param_dist_1wp.get()) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_navigator->get_mavlink_fd(), "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)_param_dist_1wp.get());
return false;
}
}
} else {
/* error reading, mission is invalid */
mavlink_log_info(_navigator->get_mavlink_fd(), "error reading offboard mission");
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
_dist_1wp_ok = true;
return true;
} else {
mavlink_log_info(_navigator->get_mavlink_fd(), "no home position");
return false;
}
} else {
return true;
}
}
void
Mission::set_mission_items()
{
@ -367,24 +330,24 @@ Mission::set_mission_items()
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* get home distance state */
bool home_dist_ok = check_dist_1wp();
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = !home_dist_ok;
bool user_feedback_done = false;
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && read_mission_item(true, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_ONBOARD) {
mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_ONBOARD;
/* try setting offboard mission item */
} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
} else if (read_mission_item(false, true, &_mission_item)) {
/* if mission type changed, notify */
if (_mission_type != MISSION_TYPE_OFFBOARD) {
mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
user_feedback_done = true;
}
_mission_type = MISSION_TYPE_OFFBOARD;
} else {
@ -392,21 +355,17 @@ Mission::set_mission_items()
if (_mission_type != MISSION_TYPE_NONE) {
/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
user_feedback_done = true;
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
} else if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
*/
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
}
_mission_type = MISSION_TYPE_NONE;
/* set loiter mission item */
set_loiter_item(&_mission_item);
/* set loiter mission item and ensure that there is a minimum clearance from home */
set_loiter_item(&_mission_item, _param_takeoff_alt.get());
/* update position setpoint triplet */
pos_sp_triplet->previous.valid = false;
@ -418,6 +377,24 @@ Mission::set_mission_items()
set_mission_finished();
if (!user_feedback_done) {
/* only tell users that we got no mission if there has not been any
* better, more specific feedback yet
* https://en.wikipedia.org/wiki/Loiter_(aeronautics)
*/
if (_navigator->get_vstatus()->condition_landed) {
/* landed, refusing to take off without a mission */
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, refusing takeoff");
} else {
mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
}
user_feedback_done = true;
}
_navigator->set_position_setpoint_triplet_updated();
return;
}

2
src/modules/navigator/mission.h
View File

@ -186,7 +186,7 @@ private:
} _mission_type;
bool _inited;
bool _dist_1wp_ok;
bool _home_inited;
MissionFeasibilityChecker _missionFeasiblityChecker; /**< class that checks if a mission is feasible */

8
src/modules/navigator/mission_block.cpp
View File

@ -228,7 +228,7 @@ MissionBlock::set_previous_pos_setpoint()
}
void
MissionBlock::set_loiter_item(struct mission_item_s *item)
MissionBlock::set_loiter_item(struct mission_item_s *item, float min_clearance)
{
if (_navigator->get_vstatus()->condition_landed) {
/* landed, don't takeoff, but switch to IDLE mode */
@ -246,10 +246,14 @@ MissionBlock::set_loiter_item(struct mission_item_s *item)
item->altitude = pos_sp_triplet->current.alt;
} else {
/* use current position */
/* use current position and use return altitude as clearance */
item->lat = _navigator->get_global_position()->lat;
item->lon = _navigator->get_global_position()->lon;
item->altitude = _navigator->get_global_position()->alt;
if (min_clearance > 0.0f && item->altitude < _navigator->get_home_position()->alt + min_clearance) {
item->altitude = _navigator->get_home_position()->alt + min_clearance;
}
}
item->altitude_is_relative = false;

2
src/modules/navigator/mission_block.h
View File

@ -91,7 +91,7 @@ protected:
/**
* Set a loiter mission item, if possible reuse the position setpoint, otherwise take the current position
*/
void set_loiter_item(struct mission_item_s *item);
void set_loiter_item(struct mission_item_s *item, float min_clearance = -1.0f);
mission_item_s _mission_item;
bool _waypoint_position_reached;

119
src/modules/navigator/mission_feasibility_checker.cpp
View File

@ -57,28 +57,45 @@
#endif
static const int ERROR = -1;
MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capabilities_sub(-1), _initDone(false)
MissionFeasibilityChecker::MissionFeasibilityChecker() :
_mavlink_fd(-1),
_capabilities_sub(-1),
_initDone(false),
_dist_1wp_ok(false)
{
_nav_caps = {0};
}
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing,
dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid, double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued)
{
bool failed = false;
bool warned = false;
/* Init if not done yet */
init();
_mavlink_fd = mavlink_fd;
// check if all mission item commands are supported
failed |= !checkMissionItemValidity(dm_current, nMissionItems);
// first check if we have a valid position
if (!home_valid /* can later use global / local pos for finer granularity */) {
failed = true;
warned = true;
mavlink_log_info(_mavlink_fd, "Not yet ready for mission, no position lock.");
} else {
failed = failed || !check_dist_1wp(dm_current, nMissionItems, curr_lat, curr_lon, max_waypoint_distance, warning_issued);
}
// check if all mission item commands are supported
failed = failed || !checkMissionItemValidity(dm_current, nMissionItems);
failed = failed || !checkGeofence(dm_current, nMissionItems, geofence);
failed = failed || !checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid, warned);
if (isRotarywing) {
failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
failed = failed || !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
} else {
failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
failed = failed || !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
}
if (!failed) {
@ -90,28 +107,20 @@ bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRota
bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
{
/* Perform checks and issue feedback to the user for all checks */
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
/* Mission is only marked as feasible if all checks return true */
return (resGeofence && resHomeAltitude);
/* no custom rotary wing checks yet */
return true;
}
bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
{
/* Update fixed wing navigation capabilites */
updateNavigationCapabilities();
// warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
/* Perform checks and issue feedback to the user for all checks */
bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
/* Mission is only marked as feasible if all checks return true */
return (resLanding && resGeofence && resHomeAltitude);
return resLanding;
}
bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@ -137,7 +146,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
return true;
}
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems,
float home_alt, bool home_valid, bool &warning_issued, bool throw_error)
{
/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
for (size_t i = 0; i < nMissionItems; i++) {
@ -145,17 +155,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
const ssize_t len = sizeof(struct mission_item_s);
if (dm_read(dm_current, i, &missionitem, len) != len) {
warning_issued = true;
/* not supposed to happen unless the datamanager can't access the SD card, etc. */
if (throw_error) {
return false;
} else {
return true;
}
return false;
}
/* always reject relative alt without home set */
if (missionitem.altitude_is_relative && !home_valid) {
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
warning_issued = true;
return false;
}
@ -163,6 +171,9 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
if (home_alt > wp_alt) {
warning_issued = true;
if (throw_error) {
mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
return false;
@ -275,6 +286,68 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size
return true;
}
bool
MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued)
{
if (_dist_1wp_ok) {
/* always return true after at least one successful check */
return true;
}
/* check if first waypoint is not too far from home */
if (dist_first_wp > 0.0f) {
struct mission_item_s mission_item;
/* find first waypoint (with lat/lon) item in datamanager */
for (unsigned i = 0; i < nMissionItems; i++) {
if (dm_read(dm_current, i,
&mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) {
/* check only items with valid lat/lon */
if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED ||
mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
/* check distance from current position to item */
float dist_to_1wp = get_distance_to_next_waypoint(
mission_item.lat, mission_item.lon, curr_lat, curr_lon);
if (dist_to_1wp < dist_first_wp) {
_dist_1wp_ok = true;
if (dist_to_1wp > ((dist_first_wp * 3) / 2)) {
/* allow at 2/3 distance, but warn */
mavlink_log_critical(_mavlink_fd, "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
warning_issued = true;
}
return true;
} else {
/* item is too far from home */
mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
warning_issued = true;
return false;
}
}
} else {
/* error reading, mission is invalid */
mavlink_log_info(_mavlink_fd, "error reading offboard mission");
return false;
}
}
/* no waypoints found in mission, then we will not fly far away */
_dist_1wp_ok = true;
return true;
} else {
return true;
}
}
void MissionFeasibilityChecker::updateNavigationCapabilities()
{
(void)orb_copy(ORB_ID(navigation_capabilities), _capabilities_sub, &_nav_caps);

9
src/modules/navigator/mission_feasibility_checker.h
View File

@ -57,12 +57,14 @@ private:
struct navigation_capabilities_s _nav_caps;
bool _initDone;
bool _dist_1wp_ok;
void init();
/* Checks for all airframes */
bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool &warning_issued, bool throw_error = false);
bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
bool check_dist_1wp(dm_item_t dm_current, size_t nMissionItems, double curr_lat, double curr_lon, float dist_first_wp, bool &warning_issued);
/* Checks specific to fixedwing airframes */
bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
@ -79,8 +81,9 @@ public:
/*
* Returns true if mission is feasible and false otherwise
*/
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
float home_alt, bool home_valid);
bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current,
size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid,
double curr_lat, double curr_lon, float max_waypoint_distance, bool &warning_issued);
};

3
src/modules/navigator/navigator.h
View File

@ -163,6 +163,8 @@ public:
float get_acceptance_radius(float mission_item_radius);
int get_mavlink_fd() { return _mavlink_fd; }
void increment_mission_instance_count() { _mission_instance_count++; }
private:
bool _task_should_exit; /**< if true, sensor task should exit */
@ -205,6 +207,7 @@ private:
bool _home_position_set;
bool _mission_item_valid; /**< flags if the current mission item is valid */
int _mission_instance_count; /**< instance count for the current mission */
perf_counter_t _loop_perf; /**< loop performance counter */

6
src/modules/navigator/navigator_main.cpp
View File

@ -128,6 +128,7 @@ Navigator::Navigator() :
_att_sp{},
_home_position_set(false),
_mission_item_valid(false),
_mission_instance_count(0),
_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
_geofence{},
_geofence_violation_warning_sent(false),
@ -523,7 +524,7 @@ Navigator::start()
_navigator_task = px4_task_spawn_cmd("navigator",
SCHED_DEFAULT,
SCHED_PRIORITY_MAX -5,
1700,
1500,
(px4_main_t)&Navigator::task_main_trampoline,
nullptr);
@ -669,6 +670,8 @@ int navigator_main(int argc, char *argv[])
void
Navigator::publish_mission_result()
{
_mission_result.instance_count = _mission_instance_count;
/* lazily publish the mission result only once available */
if (_mission_result_pub != nullptr) {
/* publish mission result */
@ -685,6 +688,7 @@ Navigator::publish_mission_result()
_mission_result.item_do_jump_changed = false;
_mission_result.item_changed_index = 0;
_mission_result.item_do_jump_remaining = 0;
_mission_result.valid = true;
}
void

98
src/modules/sensors/sensor_params.c
View File

@ -1410,3 +1410,101 @@ PARAM_DEFINE_INT32(RC_RSSI_PWM_MIN, 2000);
* @group Sensor Enable
*/
PARAM_DEFINE_INT32(SENS_EN_LL40LS, 0);
/**
* Set the minimum PWM for the MAIN outputs
*
* IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* Set to 1000 for default or 900 to increase servo travel
*
* @min 800
* @max 1400
* @unit microseconds
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_MIN, 1000);
/**
* Set the maximum PWM for the MAIN outputs
*
* IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* Set to 2000 for default or 2100 to increase servo travel
*
* @min 1600
* @max 2200
* @unit microseconds
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_MAX, 2000);
/**
* Set the disarmed PWM for MAIN outputs
*
* IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* This is the PWM pulse the autopilot is outputting if not armed.
* The main use of this parameter is to silence ESCs when they are disarmed.
*
* @min 0
* @max 2200
* @unit microseconds
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_DISARMED, 0);
/**
* Set the minimum PWM for the MAIN outputs
*
* IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* Set to 1000 for default or 900 to increase servo travel
*
* @min 800
* @max 1400
* @unit microseconds
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_MIN, 1000);
/**
* Set the maximum PWM for the MAIN outputs
*
* IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* Set to 2000 for default or 2100 to increase servo travel
*
* @min 1600
* @max 2200
* @unit microseconds
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_MAX, 2000);
/**
* Set the disarmed PWM for AUX outputs
*
* IMPORTANT: CHANGING THIS PARAMETER REQUIRES A COMPLETE SYSTEM
* REBOOT IN ORDER TO APPLY THE CHANGES. COMPLETELY POWER-CYCLE
* THE SYSTEM TO PUT CHANGES INTO EFFECT.
*
* This is the PWM pulse the autopilot is outputting if not armed.
* The main use of this parameter is to silence ESCs when they are disarmed.
*
* @min 0
* @max 2200
* @unit microseconds
* @group PWM Outputs
*/
PARAM_DEFINE_INT32(PWM_AUX_DISARMED, 1000);

10
src/modules/sensors/sensors.cpp
View File

@ -639,6 +639,12 @@ Sensors::Sensors() :
(void)param_find("SYS_PARAM_VER");
(void)param_find("SYS_AUTOSTART");
(void)param_find("SYS_AUTOCONFIG");
(void)param_find("PWM_MIN");
(void)param_find("PWM_MAX");
(void)param_find("PWM_DISARMED");
(void)param_find("PWM_AUX_MIN");
(void)param_find("PWM_AUX_MAX");
(void)param_find("PWM_AUX_DISARMED");
/* fetch initial parameter values */
parameters_update();
@ -1293,6 +1299,10 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
_airspeed.true_airspeed_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_filtered_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.true_airspeed_unfiltered_m_s = math::max(0.0f,
calc_true_airspeed(_diff_pres.differential_pressure_raw_pa + raw.baro_pres_mbar * 1e2f,
raw.baro_pres_mbar * 1e2f, air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
/* announce the airspeed if needed, just publish else */

7
src/modules/vtol_att_control/module.mk
View File

@ -38,7 +38,10 @@
MODULE_COMMAND = vtol_att_control
SRCS = vtol_att_control_main.cpp \
vtol_att_control_params.c
vtol_att_control_params.c \
tiltrotor_params.c \
tiltrotor.cpp \
vtol_type.cpp \
tailsitter.cpp
EXTRACXXFLAGS = -Wno-write-strings

189
src/modules/vtol_att_control/tailsitter.cpp Normal file
View File

@ -0,0 +1,189 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file tailsitter.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "tailsitter.h"
#include "vtol_att_control_main.h"
Tailsitter::Tailsitter (VtolAttitudeControl *att_controller) :
VtolType(att_controller),
_airspeed_tot(0),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control-tailsitter")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control-tailsitter nonfinite input"))
{
}
Tailsitter::~Tailsitter()
{
}
void Tailsitter::update_vtol_state()
{
// simply switch between the two modes
if (_manual_control_sp->aux1 < 0.0f) {
_vtol_mode = ROTARY_WING;
} else if (_manual_control_sp->aux1 > 0.0f) {
_vtol_mode = FIXED_WING;
}
}
void Tailsitter::update_mc_state()
{
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
}
void Tailsitter::process_mc_data()
{
// scale pitch control with total airspeed
//scale_mc_output();
fill_mc_att_control_output();
}
void Tailsitter::update_fw_state()
{
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
}
void Tailsitter::process_fw_data()
{
fill_fw_att_control_output();
}
void Tailsitter::update_transition_state()
{
}
void Tailsitter::update_external_state()
{
}
void Tailsitter::calc_tot_airspeed()
{
float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status->voltage_filtered_v * _batt_status->current_a / _params->vtol_motor_count;
P = math::constrain(P,1.0f,_params->power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params->prop_eff/_params->power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
_airspeed_tot = _params->arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
Tailsitter::scale_mc_output()
{
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed->true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed->timestamp) > 1e6) {
airspeed = _params->mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params->mc_airspeed_min, _params->mc_airspeed_max);
}
_vtol_vehicle_status->airspeed_tot = airspeed; // save value for logging
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params->mc_airspeed_trim / ((airspeed < _params->mc_airspeed_min) ? _params->mc_airspeed_min : airspeed);
_actuators_mc_in->control[1] = math::constrain(_actuators_mc_in->control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void Tailsitter::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0->control[0] = 0;
_actuators_out_0->control[1] = 0;
_actuators_out_0->control[2] = 0;
_actuators_out_0->control[3] = _actuators_fw_in->control[3];
/*controls for the elevons */
_actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon
_actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon
// unused now but still logged
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle
}
/**
* Prepare message to acutators with data from mc attitude controller.
*/
void Tailsitter::fill_mc_att_control_output()
{
_actuators_out_0->control[0] = _actuators_mc_in->control[0];
_actuators_out_0->control[1] = _actuators_mc_in->control[1];
_actuators_out_0->control[2] = _actuators_mc_in->control[2];
_actuators_out_0->control[3] = _actuators_mc_in->control[3];
if (_params->elevons_mc_lock == 1) {
_actuators_out_1->control[0] = 0;
_actuators_out_1->control[1] = 0;
} else {
_actuators_out_1->control[0] = _actuators_mc_in->control[2]; //roll elevon
_actuators_out_1->control[1] = _actuators_mc_in->control[1]; //pitch elevon
}
}

74
src/modules/vtol_att_control/tailsitter.h Normal file
View File

@ -0,0 +1,74 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TAILSITTER_H
#define TAILSITTER_H
#include "vtol_type.h"
#include <systemlib/perf_counter.h>
class Tailsitter : public VtolType
{
public:
Tailsitter(VtolAttitudeControl * _att_controller);
~Tailsitter();
void update_vtol_state();
void update_mc_state();
void process_mc_data();
void update_fw_state();
void process_fw_data();
void update_transition_state();
void update_external_state();
private:
void fill_mc_att_control_output();
void fill_fw_att_control_output();
void calc_tot_airspeed();
void scale_mc_output();
float _airspeed_tot;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
};
#endif

357
src/modules/vtol_att_control/tiltrotor.cpp Normal file
View File

@ -0,0 +1,357 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file tiltrotor.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "tiltrotor.h"
#include "vtol_att_control_main.h"
#define ARSP_BLEND_START 8.0f // airspeed at which we start blending mc/fw controls
Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
VtolType(attc),
flag_max_mc(true),
_tilt_control(0.0f),
_roll_weight_mc(1.0f)
{
_vtol_schedule.flight_mode = MC_MODE;
_vtol_schedule.transition_start = 0;
_params_handles_tiltrotor.front_trans_dur = param_find("VT_F_TRANS_DUR");
_params_handles_tiltrotor.back_trans_dur = param_find("VT_B_TRANS_DUR");
_params_handles_tiltrotor.tilt_mc = param_find("VT_TILT_MC");
_params_handles_tiltrotor.tilt_transition = param_find("VT_TILT_TRANS");
_params_handles_tiltrotor.tilt_fw = param_find("VT_TILT_FW");
_params_handles_tiltrotor.airspeed_trans = param_find("VT_ARSP_TRANS");
_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
}
Tiltrotor::~Tiltrotor()
{
}
int
Tiltrotor::parameters_update()
{
float v;
int l;
/* vtol duration of a front transition */
param_get(_params_handles_tiltrotor.front_trans_dur, &v);
_params_tiltrotor.front_trans_dur = math::constrain(v,1.0f,5.0f);
/* vtol duration of a back transition */
param_get(_params_handles_tiltrotor.back_trans_dur, &v);
_params_tiltrotor.back_trans_dur = math::constrain(v,0.0f,5.0f);
/* vtol tilt mechanism position in mc mode */
param_get(_params_handles_tiltrotor.tilt_mc, &v);
_params_tiltrotor.tilt_mc = v;
/* vtol tilt mechanism position in transition mode */
param_get(_params_handles_tiltrotor.tilt_transition, &v);
_params_tiltrotor.tilt_transition = v;
/* vtol tilt mechanism position in fw mode */
param_get(_params_handles_tiltrotor.tilt_fw, &v);
_params_tiltrotor.tilt_fw = v;
/* vtol airspeed at which it is ok to switch to fw mode */
param_get(_params_handles_tiltrotor.airspeed_trans, &v);
_params_tiltrotor.airspeed_trans = v;
/* vtol lock elevons in multicopter */
param_get(_params_handles_tiltrotor.elevons_mc_lock, &l);
_params_tiltrotor.elevons_mc_lock = l;
return OK;
}
void Tiltrotor::update_vtol_state()
{
parameters_update();
/* simple logic using a two way switch to perform transitions.
* after flipping the switch the vehicle will start tilting rotors, picking up
* forward speed. After the vehicle has picked up enough speed the rotors are tilted
* forward completely. For the backtransition the motors simply rotate back.
*/
if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == MC_MODE) {
// mc mode
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
_roll_weight_mc = 1.0f;
} else if (_manual_control_sp->aux1 < 0.0f && _vtol_schedule.flight_mode == FW_MODE) {
_vtol_schedule.flight_mode = TRANSITION_BACK;
flag_max_mc = true;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_manual_control_sp->aux1 >= 0.0f && _vtol_schedule.flight_mode == MC_MODE) {
// instant of doeing a front-transition
_vtol_schedule.flight_mode = TRANSITION_FRONT_P1;
_vtol_schedule.transition_start = hrt_absolute_time();
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 > 0.0f) {
// check if we have reached airspeed to switch to fw mode
if (_airspeed->true_airspeed_m_s >= _params_tiltrotor.airspeed_trans) {
_vtol_schedule.flight_mode = TRANSITION_FRONT_P2;
flag_max_mc = true;
_vtol_schedule.transition_start = hrt_absolute_time();
}
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 > 0.0f) {
if (_tilt_control >= _params_tiltrotor.tilt_fw) {
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1 && _manual_control_sp->aux1 < 0.0f) {
// failsave into mc mode
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2 && _manual_control_sp->aux1 < 0.0f) {
// failsave into mc mode
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 < 0.0f) {
if (_tilt_control <= _params_tiltrotor.tilt_mc) {
_vtol_schedule.flight_mode = MC_MODE;
_tilt_control = _params_tiltrotor.tilt_mc;
flag_max_mc = false;
}
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK && _manual_control_sp->aux1 > 0.0f) {
// failsave into fw mode
_vtol_schedule.flight_mode = FW_MODE;
_tilt_control = _params_tiltrotor.tilt_fw;
}
// tilt rotors if necessary
update_transition_state();
// map tiltrotor specific control phases to simple control modes
switch(_vtol_schedule.flight_mode) {
case MC_MODE:
_vtol_mode = ROTARY_WING;
break;
case FW_MODE:
_vtol_mode = FIXED_WING;
break;
case TRANSITION_FRONT_P1:
case TRANSITION_FRONT_P2:
case TRANSITION_BACK:
_vtol_mode = TRANSITION;
break;
}
}
void Tiltrotor::update_mc_state()
{
// adjust max pwm for rear motors to spin up
if (!flag_max_mc) {
set_max_mc();
flag_max_mc = true;
}
// set idle speed for rotary wing mode
if (!flag_idle_mc) {
set_idle_mc();
flag_idle_mc = true;
}
}
void Tiltrotor::process_mc_data()
{
fill_mc_att_control_output();
}
void Tiltrotor::update_fw_state()
{
/* in fw mode we need the rear motors to stop spinning, in backtransition
* mode we let them spin in idle
*/
if (flag_max_mc) {
if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
set_max_fw(1200);
set_idle_mc();
} else {
set_max_fw(950);
set_idle_fw();
}
flag_max_mc = false;
}
// adjust idle for fixed wing flight
if (flag_idle_mc) {
set_idle_fw();
flag_idle_mc = false;
}
}
void Tiltrotor::process_fw_data()
{
fill_fw_att_control_output();
}
void Tiltrotor::update_transition_state()
{
if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P1) {
// tilt rotors forward up to certain angle
if (_tilt_control <= _params_tiltrotor.tilt_transition) {
_tilt_control = _params_tiltrotor.tilt_mc + fabsf(_params_tiltrotor.tilt_transition - _params_tiltrotor.tilt_mc)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.front_trans_dur*1000000.0f);
}
// do blending of mc and fw controls
if (_airspeed->true_airspeed_m_s >= ARSP_BLEND_START) {
_roll_weight_mc = 1.0f - (_airspeed->true_airspeed_m_s - ARSP_BLEND_START) / (_params_tiltrotor.airspeed_trans - ARSP_BLEND_START);
} else {
// at low speeds give full weight to mc
_roll_weight_mc = 1.0f;
}
_roll_weight_mc = math::constrain(_roll_weight_mc, 0.0f, 1.0f);
} else if (_vtol_schedule.flight_mode == TRANSITION_FRONT_P2) {
_tilt_control = _params_tiltrotor.tilt_transition + fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_transition)*(float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(0.5f*1000000.0f);
_roll_weight_mc = 0.0f;
} else if (_vtol_schedule.flight_mode == TRANSITION_BACK) {
// tilt rotors forward up to certain angle
float progress = (float)hrt_elapsed_time(&_vtol_schedule.transition_start)/(_params_tiltrotor.back_trans_dur*1000000.0f);
if (_tilt_control > _params_tiltrotor.tilt_mc) {
_tilt_control = _params_tiltrotor.tilt_fw - fabsf(_params_tiltrotor.tilt_fw - _params_tiltrotor.tilt_mc)*progress;
}
_roll_weight_mc = progress;
}
}
void Tiltrotor::update_external_state()
{
}
/**
* Prepare message to acutators with data from mc attitude controller.
*/
void Tiltrotor::fill_mc_att_control_output()
{
_actuators_out_0->control[0] = _actuators_mc_in->control[0];
_actuators_out_0->control[1] = _actuators_mc_in->control[1];
_actuators_out_0->control[2] = _actuators_mc_in->control[2];
_actuators_out_0->control[3] = _actuators_mc_in->control[3];
_actuators_out_1->control[0] = -_actuators_fw_in->control[0] * (1.0f - _roll_weight_mc); //roll elevon
_actuators_out_1->control[1] = (_actuators_fw_in->control[1] + _params->fw_pitch_trim)* (1.0f -_roll_weight_mc); //pitch elevon
_actuators_out_1->control[4] = _tilt_control; // for tilt-rotor control
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void Tiltrotor::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0->control[0] = _actuators_mc_in->control[0] * _roll_weight_mc;
_actuators_out_0->control[1] = _actuators_mc_in->control[1] * _roll_weight_mc;
_actuators_out_0->control[2] = _actuators_mc_in->control[2] * _roll_weight_mc;
_actuators_out_0->control[3] = _actuators_fw_in->control[3];
/*controls for the elevons */
_actuators_out_1->control[0] = -_actuators_fw_in->control[0]; // roll elevon
_actuators_out_1->control[1] = _actuators_fw_in->control[1] + _params->fw_pitch_trim; // pitch elevon
// unused now but still logged
_actuators_out_1->control[2] = _actuators_fw_in->control[2]; // yaw
_actuators_out_1->control[3] = _actuators_fw_in->control[3]; // throttle
_actuators_out_1->control[4] = _tilt_control;
}
/**
* Kill rear motors for the FireFLY6 when in fw mode.
*/
void
Tiltrotor::set_max_fw(unsigned pwm_value)
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
if (i == 2 || i == 3) {
pwm_values.values[i] = pwm_value;
} else {
pwm_values.values[i] = 2000;
}
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
close(fd);
}
void
Tiltrotor::set_max_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = 2000;
pwm_values.channel_count = _params->vtol_motor_count;
}
ret = ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting max values");}
close(fd);
}

110
src/modules/vtol_att_control/tiltrotor.h Normal file
View File

@ -0,0 +1,110 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file tiltrotor.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef TILTROTOR_H
#define TILTROTOR_H
#include "vtol_type.h"
#include <systemlib/param/param.h>
#include <drivers/drv_hrt.h>
class Tiltrotor : public VtolType
{
public:
Tiltrotor(VtolAttitudeControl * _att_controller);
~Tiltrotor();
void update_vtol_state();
void update_mc_state();
void process_mc_data();
void update_fw_state();
void process_fw_data();
void update_transition_state();
void update_external_state();
private:
struct {
float front_trans_dur;
float back_trans_dur;
float tilt_mc;
float tilt_transition;
float tilt_fw;
float airspeed_trans;
int elevons_mc_lock; // lock elevons in multicopter mode
} _params_tiltrotor;
struct {
param_t front_trans_dur;
param_t back_trans_dur;
param_t tilt_mc;
param_t tilt_transition;
param_t tilt_fw;
param_t airspeed_trans;
param_t elevons_mc_lock;
} _params_handles_tiltrotor;
enum vtol_mode {
MC_MODE = 0,
TRANSITION_FRONT_P1,
TRANSITION_FRONT_P2,
TRANSITION_BACK,
FW_MODE
};
struct {
vtol_mode flight_mode; // indicates in which mode the vehicle is in
hrt_abstime transition_start; // at what time did we start a transition (front- or backtransition)
}_vtol_schedule;
bool flag_max_mc;
float _tilt_control;
float _roll_weight_mc;
void fill_mc_att_control_output();
void fill_fw_att_control_output();
void set_max_mc();
void set_max_fw(unsigned pwm_value);
int parameters_update();
};
#endif

94
src/modules/uORB/topics/mission_result.h → src/modules/vtol_att_control/tiltrotor_params.c
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (C) 2012-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
@ -32,44 +32,76 @@
****************************************************************************/
/**
* @file mission_result.h
* Mission results that navigator needs to pass on to commander and mavlink.
* @file tiltrotor_params.c
* Parameters for vtol attitude controller.
*
* @author Thomas Gubler <thomasgubler@student.ethz.ch>
* @author Julian Oes <joes@student.ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Ban Siesta <bansiesta@gmail.com>
* @author Roman Bapst <bapstroman@gmail.com>
*/
#ifndef TOPIC_MISSION_RESULT_H
#define TOPIC_MISSION_RESULT_H
#include <stdint.h>
#include <stdbool.h>
#include "../uORB.h"
#include <systemlib/param/param.h>
/**
* Duration of a front transition
*
* Time in seconds used for a transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_F_TRANS_DUR,3.0f);
/**
* @addtogroup topics
* @{
* Duration of a back transition
*
* Time in seconds used for a back transition
*
* @min 0.0
* @max 5
* @group VTOL Attitude Control
*/
struct mission_result_s {
unsigned seq_reached; /**< Sequence of the mission item which has been reached */
unsigned seq_current; /**< Sequence of the current mission item */
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 */
unsigned item_changed_index; /**< indicate which item has changed */
unsigned item_do_jump_remaining;/**< set to the number of do jumps remaining for that item */
};
PARAM_DEFINE_FLOAT(VT_B_TRANS_DUR,2.0f);
/**
* @}
* Position of tilt servo in mc mode
*
* Position of tilt servo in mc mode
*
* @min 0.0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_MC,0.0f);
/* register this as object request broker structure */
ORB_DECLARE(mission_result);
/**
* Position of tilt servo in transition mode
*
* Position of tilt servo in transition mode
*
* @min 0.0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_TRANS,0.3f);
#endif
/**
* Position of tilt servo in fw mode
*
* Position of tilt servo in fw mode
*
* @min 0.0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_TILT_FW,1.0f);
/**
* Transition airspeed
*
* Airspeed at which we can switch to fw mode
*
* @min 0.0
* @max 20
* @group VTOL Attitude Control
*/
PARAM_DEFINE_FLOAT(VT_ARSP_TRANS,10.0f);

433
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -44,165 +44,7 @@
*
*/
#include <px4_config.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/battery_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
class VtolAttitudeControl
{
public:
VtolAttitudeControl();
~VtolAttitudeControl();
int start(); /* start the task and return OK on success */
private:
//******************flags & handlers******************************************************
bool _task_should_exit;
int _control_task; //task handle for VTOL attitude controller
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _v_control_mode_sub; //vehicle control mode subscription
int _params_sub; //parameter updates subscription
int _manual_control_sp_sub; //manual control setpoint subscription
int _armed_sub; //arming status subscription
int _local_pos_sub; // sensor subscription
int _airspeed_sub; // airspeed subscription
int _battery_status_sub; // battery status subscription
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
//handlers for publishers
orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s _vtol_vehicle_status;
struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s _armed; //actuator arming status
struct vehicle_local_position_s _local_pos;
struct airspeed_s _airspeed; // airspeed
struct battery_status_s _batt_status; // battery status
struct {
param_t idle_pwm_mc; //pwm value for idle in mc mode
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
float mc_airspeed_trim; // trim airspeed in multicopter mode
float mc_airspeed_max; // max airpseed in multicopter mode
float fw_pitch_trim; // trim for neutral elevon position in fw mode
float power_max; // maximum power of one engine
float prop_eff; // factor to calculate prop efficiency
float arsp_lp_gain; // total airspeed estimate low pass gain
} _params;
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab;
param_t mc_airspeed_min;
param_t mc_airspeed_trim;
param_t mc_airspeed_max;
param_t fw_pitch_trim;
param_t power_max;
param_t prop_eff;
param_t arsp_lp_gain;
} _params_handles;
perf_counter_t _loop_perf; /**< loop performance counter */
perf_counter_t _nonfinite_input_perf; /**< performance counter for non finite input */
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
unsigned _motor_count; // number of motors
float _airspeed_tot;
float _tilt_control;
//*****************Member functions***********************************************************************
void task_main(); //main task
static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
void vehicle_manual_poll(); //Check for changes in manual inputs.
void arming_status_poll(); //Check for arming status updates.
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
void vehicle_rates_sp_mc_poll();
void vehicle_rates_sp_fw_poll();
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_battery_poll(); // Check for battery updates
void parameters_update_poll(); //Check if parameters have changed
int parameters_update(); //Update local paraemter cache
void fill_mc_att_control_output(); //write mc_att_control results to actuator message
void fill_fw_att_control_output(); //write fw_att_control results to actuator message
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
void set_idle_fw();
void set_idle_mc();
void scale_mc_output();
void calc_tot_airspeed(); // estimated airspeed seen by elevons
};
#include "vtol_att_control_main.h"
namespace VTOL_att_control
{
@ -235,14 +77,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
_vtol_vehicle_status_pub(nullptr),
_v_rates_sp_pub(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "vtol_att_control")),
_nonfinite_input_perf(perf_alloc(PC_COUNT, "vtol att control nonfinite input"))
{
flag_idle_mc = true;
_airspeed_tot = 0.0f;
_tilt_control = 0.0f;
memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
_vtol_vehicle_status.vtol_in_rw_mode = true; /* start vtol in rotary wing mode*/
memset(&_v_att, 0, sizeof(_v_att));
@ -276,9 +111,21 @@ VtolAttitudeControl::VtolAttitudeControl() :
_params_handles.power_max = param_find("VT_POWER_MAX");
_params_handles.prop_eff = param_find("VT_PROP_EFF");
_params_handles.arsp_lp_gain = param_find("VT_ARSP_LP_GAIN");
_params_handles.vtol_type = param_find("VT_TYPE");
_params_handles.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
/* fetch initial parameter values */
parameters_update();
if (_params.vtol_type == 0) {
_tailsitter = new Tailsitter(this);
_vtol_type = _tailsitter;
} else if (_params.vtol_type == 1) {
_tiltrotor = new Tiltrotor(this);
_vtol_type = _tiltrotor;
} else {
_task_should_exit = true;
}
}
/**
@ -470,6 +317,7 @@ int
VtolAttitudeControl::parameters_update()
{
float v;
int l;
/* idle pwm for mc mode */
param_get(_params_handles.idle_pwm_mc, &_params.idle_pwm_mc);
@ -507,42 +355,16 @@ VtolAttitudeControl::parameters_update()
param_get(_params_handles.arsp_lp_gain, &v);
_params.arsp_lp_gain = v;
param_get(_params_handles.vtol_type, &l);
_params.vtol_type = l;
/* vtol lock elevons in multicopter */
param_get(_params_handles.elevons_mc_lock, &l);
_params.elevons_mc_lock = l;
return OK;
}
/**
* Prepare message to acutators with data from mc attitude controller.
*/
void VtolAttitudeControl::fill_mc_att_control_output()
{
_actuators_out_0.control[0] = _actuators_mc_in.control[0];
_actuators_out_0.control[1] = _actuators_mc_in.control[1];
_actuators_out_0.control[2] = _actuators_mc_in.control[2];
_actuators_out_0.control[3] = _actuators_mc_in.control[3];
//set neutral position for elevons
_actuators_out_1.control[0] = _actuators_mc_in.control[2]; //roll elevon
_actuators_out_1.control[1] = _actuators_mc_in.control[1];; //pitch elevon
_actuators_out_1.control[4] = _tilt_control; // for tilt-rotor control
}
/**
* Prepare message to acutators with data from fw attitude controller.
*/
void VtolAttitudeControl::fill_fw_att_control_output()
{
/*For the first test in fw mode, only use engines for thrust!!!*/
_actuators_out_0.control[0] = 0;
_actuators_out_0.control[1] = 0;
_actuators_out_0.control[2] = 0;
_actuators_out_0.control[3] = _actuators_fw_in.control[3];
/*controls for the elevons */
_actuators_out_1.control[0] = -_actuators_fw_in.control[0]; // roll elevon
_actuators_out_1.control[1] = _actuators_fw_in.control[1] + _params.fw_pitch_trim; // pitch elevon
// unused now but still logged
_actuators_out_1.control[2] = _actuators_fw_in.control[2]; // yaw
_actuators_out_1.control[3] = _actuators_fw_in.control[3]; // throttle
}
/**
* Prepare message for mc attitude rates setpoint topic
*/
@ -565,109 +387,6 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
_v_rates_sp.thrust = _fw_virtual_v_rates_sp.thrust;
}
/**
* Adjust idle speed for fw mode.
*/
void VtolAttitudeControl::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (unsigned i = 0; i < _params.vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}
/**
* Adjust idle speed for mc mode.
*/
void VtolAttitudeControl::set_idle_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params.idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (unsigned i = 0; i < _params.vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}
void
VtolAttitudeControl::scale_mc_output() {
// scale around tuning airspeed
float airspeed;
calc_tot_airspeed(); // estimate air velocity seen by elevons
// if airspeed is not updating, we assume the normal average speed
if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
airspeed = _params.mc_airspeed_trim;
if (nonfinite) {
perf_count(_nonfinite_input_perf);
}
} else {
airspeed = _airspeed_tot;
airspeed = math::constrain(airspeed,_params.mc_airspeed_min, _params.mc_airspeed_max);
}
_vtol_vehicle_status.airspeed_tot = airspeed; // save value for logging
/*
* For scaling our actuators using anything less than the min (close to stall)
* speed doesn't make any sense - its the strongest reasonable deflection we
* want to do in flight and its the baseline a human pilot would choose.
*
* Forcing the scaling to this value allows reasonable handheld tests.
*/
float airspeed_scaling = _params.mc_airspeed_trim / ((airspeed < _params.mc_airspeed_min) ? _params.mc_airspeed_min : airspeed);
_actuators_mc_in.control[1] = math::constrain(_actuators_mc_in.control[1]*airspeed_scaling*airspeed_scaling,-1.0f,1.0f);
}
void VtolAttitudeControl::calc_tot_airspeed() {
float airspeed = math::max(1.0f, _airspeed.true_airspeed_m_s); // prevent numerical drama
// calculate momentary power of one engine
float P = _batt_status.voltage_filtered_v * _batt_status.current_a / _params.vtol_motor_count;
P = math::constrain(P,1.0f,_params.power_max);
// calculate prop efficiency
float power_factor = 1.0f - P*_params.prop_eff/_params.power_max;
float eta = (1.0f/(1 + expf(-0.4f * power_factor * airspeed)) - 0.5f)*2.0f;
eta = math::constrain(eta,0.001f,1.0f); // live on the safe side
// calculate induced airspeed by propeller
float v_ind = (airspeed/eta - airspeed)*2.0f;
// calculate total airspeed
float airspeed_raw = airspeed + v_ind;
// apply low-pass filter
_airspeed_tot = _params.arsp_lp_gain * (_airspeed_tot - airspeed_raw) + airspeed_raw;
}
void
VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
{
@ -701,8 +420,7 @@ void VtolAttitudeControl::task_main()
_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
// make sure we start with idle in mc mode
set_idle_mc();
flag_idle_mc = true;
_vtol_type->set_idle_mc();
/* wakeup source*/
struct pollfd fds[3]; /*input_mc, input_fw, parameters*/
@ -764,83 +482,80 @@ void VtolAttitudeControl::task_main()
vehicle_airspeed_poll();
vehicle_battery_poll();
// update the vtol state machine which decides which mode we are in
_vtol_type->update_vtol_state();
if (_manual_control_sp.aux1 < 0.0f) { /* vehicle is in mc mode */
// check in which mode we are in and call mode specific functions
if (_vtol_type->get_mode() == ROTARY_WING) {
// vehicle is in rotary wing mode
_vtol_vehicle_status.vtol_in_rw_mode = true;
if (!flag_idle_mc) { /* we want to adjust idle speed for mc mode */
set_idle_mc();
flag_idle_mc = true;
}
_vtol_type->update_mc_state();
/* got data from mc_att_controller */
// got data from mc attitude controller
if (fds[0].revents & POLLIN) {
vehicle_manual_poll(); /* update remote input */
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
// scale pitch control with total airspeed
scale_mc_output();
_vtol_type->process_mc_data();
fill_mc_att_control_output();
fill_mc_att_rates_sp();
/* Only publish if the proper mode(s) are enabled */
if(_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled)
{
if (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
}
if (_manual_control_sp.aux1 >= 0.0f) { /* vehicle is in fw mode */
} else if (_vtol_type->get_mode() == FIXED_WING) {
// vehicle is in fw mode
_vtol_vehicle_status.vtol_in_rw_mode = false;
if (flag_idle_mc) { /* we want to adjust idle speed for fixed wing mode */
set_idle_fw();
flag_idle_mc = false;
_vtol_type->update_fw_state();
// got data from fw attitude controller
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll();
_vtol_type->process_fw_data();
fill_fw_att_rates_sp();
}
} else if (_vtol_type->get_mode() == TRANSITION) {
// vehicle is doing a transition
bool got_new_data = false;
if (fds[0].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
got_new_data = true;
}
if (fds[1].revents & POLLIN) { /* got data from fw_att_controller */
if (fds[1].revents & POLLIN) {
orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
vehicle_manual_poll(); //update remote input
got_new_data = true;
}
fill_fw_att_control_output();
fill_fw_att_rates_sp();
// update transition state if got any new data
if (got_new_data) {
_vtol_type->update_transition_state();
}
/* Only publish if the proper mode(s) are enabled */
if(_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||
_v_control_mode.flag_control_manual_enabled)
{
if (_actuators_0_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else if (_vtol_type->get_mode() == EXTERNAL) {
// we are using external module to generate attitude/thrust setpoint
_vtol_type->update_external_state();
}
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
if (_actuators_1_pub != nullptr) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
/* Only publish if the proper mode(s) are enabled */
if(_v_control_mode.flag_control_attitude_enabled ||
_v_control_mode.flag_control_rates_enabled ||
_v_control_mode.flag_control_manual_enabled)
{
if (_actuators_0_pub > 0) {
orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
} else {
_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
}
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
if (_actuators_1_pub > 0) {
orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
} else {
_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
}
}
}
// publish the attitude rates setpoint
if(_v_rates_sp_pub != nullptr) {

213
src/modules/vtol_att_control/vtol_att_control_main.h Normal file
View File

@ -0,0 +1,213 @@
/****************************************************************************
*
* Copyright (c) 2013, 2014 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.
*
****************************************************************************/
/**
* @file VTOL_att_control_main.cpp
* Implementation of an attitude controller for VTOL airframes. This module receives data
* from both the fixed wing- and the multicopter attitude controllers and processes it.
* It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
* flight or transition). It also publishes the resulting controls on the actuator controls topics.
*
* @author Roman Bapst <bapstr@ethz.ch>
* @author Lorenz Meier <lm@inf.ethz.ch>
* @author Thomas Gubler <thomasgubler@gmail.com>
*
*/
#ifndef VTOL_ATT_CONTROL_MAIN_H
#define VTOL_ATT_CONTROL_MAIN_H
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <errno.h>
#include <math.h>
#include <poll.h>
#include <drivers/drv_hrt.h>
#include <arch/board/board.h>
#include <drivers/drv_pwm_output.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_attitude_setpoint.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/actuator_controls_virtual_mc.h>
#include <uORB/topics/actuator_controls_virtual_fw.h>
#include <uORB/topics/vehicle_rates_setpoint.h>
#include <uORB/topics/mc_virtual_rates_setpoint.h>
#include <uORB/topics/fw_virtual_rates_setpoint.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/vtol_vehicle_status.h>
#include <uORB/topics/actuator_armed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/battery_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>
#include <systemlib/circuit_breaker.h>
#include <lib/mathlib/mathlib.h>
#include <lib/geo/geo.h>
#include <nuttx/fs/ioctl.h>
#include <fcntl.h>
#include "tiltrotor.h"
#include "tailsitter.h"
extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
class VtolAttitudeControl
{
public:
VtolAttitudeControl();
~VtolAttitudeControl();
int start(); /* start the task and return OK on success */
struct vehicle_attitude_s* get_att () {return &_v_att;}
struct vehicle_attitude_setpoint_s* get_att_sp () {return &_v_att_sp;}
struct vehicle_rates_setpoint_s* get_rates_sp () {return &_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_mc_virtual_rates_sp () {return &_mc_virtual_v_rates_sp;}
struct vehicle_rates_setpoint_s* get_fw_virtual_rates_sp () {return &_fw_virtual_v_rates_sp;}
struct manual_control_setpoint_s* get_manual_control_sp () {return &_manual_control_sp;}
struct vehicle_control_mode_s* get_control_mode () {return &_v_control_mode;}
struct vtol_vehicle_status_s* get_vehicle_status () {return &_vtol_vehicle_status;}
struct actuator_controls_s* get_actuators_out0 () {return &_actuators_out_0;}
struct actuator_controls_s* get_actuators_out1 () {return &_actuators_out_1;}
struct actuator_controls_s* get_actuators_mc_in () {return &_actuators_mc_in;}
struct actuator_controls_s* get_actuators_fw_in () {return &_actuators_fw_in;}
struct actuator_armed_s* get_armed () {return &_armed;}
struct vehicle_local_position_s* get_local_pos () {return &_local_pos;}
struct airspeed_s* get_airspeed () {return &_airspeed;}
struct battery_status_s* get_batt_status () {return &_batt_status;}
struct Params* get_params () {return &_params;}
private:
//******************flags & handlers******************************************************
bool _task_should_exit;
int _control_task; //task handle for VTOL attitude controller
/* handlers for subscriptions */
int _v_att_sub; //vehicle attitude subscription
int _v_att_sp_sub; //vehicle attitude setpoint subscription
int _mc_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _fw_virtual_v_rates_sp_sub; //vehicle rates setpoint subscription
int _v_control_mode_sub; //vehicle control mode subscription
int _params_sub; //parameter updates subscription
int _manual_control_sp_sub; //manual control setpoint subscription
int _armed_sub; //arming status subscription
int _local_pos_sub; // sensor subscription
int _airspeed_sub; // airspeed subscription
int _battery_status_sub; // battery status subscription
int _actuator_inputs_mc; //topic on which the mc_att_controller publishes actuator inputs
int _actuator_inputs_fw; //topic on which the fw_att_controller publishes actuator inputs
//handlers for publishers
orb_advert_t _actuators_0_pub; //input for the mixer (roll,pitch,yaw,thrust)
orb_advert_t _actuators_1_pub;
orb_advert_t _vtol_vehicle_status_pub;
orb_advert_t _v_rates_sp_pub;
//*******************data containers***********************************************************
struct vehicle_attitude_s _v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s _v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s _v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s _mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s _fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s _manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s _v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s _vtol_vehicle_status;
struct actuator_controls_s _actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s _actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s _actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s _actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s _armed; //actuator arming status
struct vehicle_local_position_s _local_pos;
struct airspeed_s _airspeed; // airspeed
struct battery_status_s _batt_status; // battery status
Params _params; // struct holding the parameters
struct {
param_t idle_pwm_mc;
param_t vtol_motor_count;
param_t vtol_fw_permanent_stab;
param_t mc_airspeed_min;
param_t mc_airspeed_trim;
param_t mc_airspeed_max;
param_t fw_pitch_trim;
param_t power_max;
param_t prop_eff;
param_t arsp_lp_gain;
param_t vtol_type;
param_t elevons_mc_lock;
} _params_handles;
/* for multicopters it is usual to have a non-zero idle speed of the engines
* for fixed wings we want to have an idle speed of zero since we do not want
* to waste energy when gliding. */
unsigned _motor_count; // number of motors
float _airspeed_tot;
VtolType * _vtol_type; // base class for different vtol types
Tiltrotor * _tiltrotor; // tailsitter vtol type
Tailsitter * _tailsitter; // tiltrotor vtol type
//*****************Member functions***********************************************************************
void task_main(); //main task
static void task_main_trampoline(int argc, char *argv[]); //Shim for calling task_main from task_create.
void vehicle_control_mode_poll(); //Check for changes in vehicle control mode.
void vehicle_manual_poll(); //Check for changes in manual inputs.
void arming_status_poll(); //Check for arming status updates.
void actuator_controls_mc_poll(); //Check for changes in mc_attitude_control output
void actuator_controls_fw_poll(); //Check for changes in fw_attitude_control output
void vehicle_rates_sp_mc_poll();
void vehicle_rates_sp_fw_poll();
void vehicle_local_pos_poll(); // Check for changes in sensor values
void vehicle_airspeed_poll(); // Check for changes in airspeed
void vehicle_battery_poll(); // Check for battery updates
void parameters_update_poll(); //Check if parameters have changed
int parameters_update(); //Update local paraemter cache
void fill_mc_att_rates_sp();
void fill_fw_att_rates_sp();
};
#endif

19
src/modules/vtol_att_control/vtol_att_control_params.c
View File

@ -142,3 +142,22 @@ PARAM_DEFINE_FLOAT(VT_PROP_EFF,0.0f);
*/
PARAM_DEFINE_FLOAT(VT_ARSP_LP_GAIN,0.3f);
/**
* VTOL Type (Tailsitter=0, Tiltrotor=1)
*
* @min 0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_TYPE, 0);
/**
* Lock elevons in multicopter mode
*
* If set to 1 the elevons are locked in multicopter mode
*
* @min 0
* @max 1
* @group VTOL Attitude Control
*/
PARAM_DEFINE_INT32(VT_ELEV_MC_LOCK,0);

133
src/modules/vtol_att_control/vtol_type.cpp Normal file
View File

@ -0,0 +1,133 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file airframe.cpp
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#include "vtol_type.h"
#include "drivers/drv_pwm_output.h"
#include <nuttx/fs/ioctl.h>
#include "vtol_att_control_main.h"
VtolType::VtolType(VtolAttitudeControl *att_controller) :
_attc(att_controller),
_vtol_mode(ROTARY_WING)
{
_v_att = _attc->get_att();
_v_att_sp = _attc->get_att_sp();
_v_rates_sp = _attc->get_rates_sp();
_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
_manual_control_sp = _attc->get_manual_control_sp();
_v_control_mode = _attc->get_control_mode();
_vtol_vehicle_status = _attc->get_vehicle_status();
_actuators_out_0 = _attc->get_actuators_out0();
_actuators_out_1 = _attc->get_actuators_out1();
_actuators_mc_in = _attc->get_actuators_mc_in();
_actuators_fw_in = _attc->get_actuators_fw_in();
_armed = _attc->get_armed();
_local_pos = _attc->get_local_pos();
_airspeed = _attc->get_airspeed();
_batt_status = _attc->get_batt_status();
_params = _attc->get_params();
flag_idle_mc = true;
}
VtolType::~VtolType()
{
}
/**
* Adjust idle speed for mc mode.
*/
void VtolType::set_idle_mc()
{
int ret;
unsigned servo_count;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
unsigned pwm_value = _params->idle_pwm_mc;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
flag_idle_mc = true;
}
/**
* Adjust idle speed for fw mode.
*/
void VtolType::set_idle_fw()
{
int ret;
char *dev = PWM_OUTPUT0_DEVICE_PATH;
int fd = open(dev, 0);
if (fd < 0) {err(1, "can't open %s", dev);}
unsigned pwm_value = PWM_LOWEST_MIN;
struct pwm_output_values pwm_values;
memset(&pwm_values, 0, sizeof(pwm_values));
for (int i = 0; i < _params->vtol_motor_count; i++) {
pwm_values.values[i] = pwm_value;
pwm_values.channel_count++;
}
ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
if (ret != OK) {errx(ret, "failed setting min values");}
close(fd);
}

116
src/modules/vtol_att_control/vtol_type.h Normal file
View File

@ -0,0 +1,116 @@
/****************************************************************************
*
* 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.
*
****************************************************************************/
/**
* @file airframe.h
*
* @author Roman Bapst <bapstroman@gmail.com>
*
*/
#ifndef VTOL_YYPE_H
#define VTOL_YYPE_H
struct Params {
int idle_pwm_mc; // pwm value for idle in mc mode
int vtol_motor_count; // number of motors
int vtol_fw_permanent_stab; // in fw mode stabilize attitude also in manual mode
float mc_airspeed_min; // min airspeed in multicoper mode (including prop-wash)
float mc_airspeed_trim; // trim airspeed in multicopter mode
float mc_airspeed_max; // max airpseed in multicopter mode
float fw_pitch_trim; // trim for neutral elevon position in fw mode
float power_max; // maximum power of one engine
float prop_eff; // factor to calculate prop efficiency
float arsp_lp_gain; // total airspeed estimate low pass gain
int vtol_type;
int elevons_mc_lock; // lock elevons in multicopter mode
};
enum mode {
ROTARY_WING = 0,
FIXED_WING,
TRANSITION,
EXTERNAL
};
class VtolAttitudeControl;
class VtolType
{
public:
VtolType(VtolAttitudeControl *att_controller);
virtual ~VtolType();
virtual void update_vtol_state() = 0;
virtual void update_mc_state() = 0;
virtual void process_mc_data() = 0;
virtual void update_fw_state() = 0;
virtual void process_fw_data() = 0;
virtual void update_transition_state() = 0;
virtual void update_external_state() = 0;
void set_idle_mc();
void set_idle_fw();
mode get_mode () {return _vtol_mode;};
protected:
VtolAttitudeControl *_attc;
mode _vtol_mode;
struct vehicle_attitude_s *_v_att; //vehicle attitude
struct vehicle_attitude_setpoint_s *_v_att_sp; //vehicle attitude setpoint
struct vehicle_rates_setpoint_s *_v_rates_sp; //vehicle rates setpoint
struct vehicle_rates_setpoint_s *_mc_virtual_v_rates_sp; // virtual mc vehicle rates setpoint
struct vehicle_rates_setpoint_s *_fw_virtual_v_rates_sp; // virtual fw vehicle rates setpoint
struct manual_control_setpoint_s *_manual_control_sp; //manual control setpoint
struct vehicle_control_mode_s *_v_control_mode; //vehicle control mode
struct vtol_vehicle_status_s *_vtol_vehicle_status;
struct actuator_controls_s *_actuators_out_0; //actuator controls going to the mc mixer
struct actuator_controls_s *_actuators_out_1; //actuator controls going to the fw mixer (used for elevons)
struct actuator_controls_s *_actuators_mc_in; //actuator controls from mc_att_control
struct actuator_controls_s *_actuators_fw_in; //actuator controls from fw_att_control
struct actuator_armed_s *_armed; //actuator arming status
struct vehicle_local_position_s *_local_pos;
struct airspeed_s *_airspeed; // airspeed
struct battery_status_s *_batt_status; // battery status
struct Params *_params;
bool flag_idle_mc; //false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
};
#endif

2
src/systemcmds/param/param.c
View File

@ -192,7 +192,7 @@ param_main(int argc, char *argv[])
}
}
warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare', 'select' or 'save'");
warnx("expected a command, try 'load', 'import', 'show', 'set', 'compare',\n'index', 'index_used', 'select' or 'save'");
return 1;
}

32
src/systemcmds/pwm/pwm.c
View File

@ -59,6 +59,7 @@
#include "systemlib/systemlib.h"
#include "systemlib/err.h"
#include "systemlib/param/param.h"
#include "drivers/drv_pwm_output.h"
static void usage(const char *reason);
@ -187,10 +188,35 @@ pwm_main(int argc, char *argv[])
break;
case 'p':
pwm_value = strtoul(optarg, &ep, 0);
{
/* check if this is a param name */
if (strncmp("p:", optarg, 2) == 0) {
if (*ep != '\0') {
usage("BAD PWM VAL");
char buf[32];
strncpy(buf, optarg + 2, 16);
/* user wants to use a param name */
param_t parm = param_find(buf);
if (parm != PARAM_INVALID) {
int32_t pwm_parm;
int gret = param_get(parm, &pwm_parm);
if (gret == 0) {
pwm_value = pwm_parm;
} else {
usage("PARAM LOAD FAIL");
}
} else {
usage("PARAM NAME NOT FOUND");
}
} else {
pwm_value = strtoul(optarg, &ep, 0);
}
if (*ep != '\0') {
usage("BAD PWM VAL");
}
}
break;