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

Merge branch 'release_v1.0.0' of github.com:PX4/Firmware into beta

This commit is contained in:
Lorenz Meier 2015-06-25 21:20:21 +02:00
parent 79a690cd17 5b93d92339
commit d6f05fbada
8 changed files with 408 additions and 47 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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))

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;

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

@ -243,7 +243,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
struct pollfd fds[1];
fds[0].fd = accel_sub;
@ -324,7 +324,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;
}
}
@ -488,7 +488,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) {

116
src/modules/commander/mag_calibration.cpp
View File

@ -65,6 +65,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]);
@ -76,7 +78,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];
@ -184,6 +186,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;
@ -286,27 +306,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
int poll_ret = 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++;
}
@ -336,8 +376,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;
@ -345,9 +384,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
@ -357,9 +396,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];
@ -438,7 +477,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]);
@ -450,6 +489,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++) {

25
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
View File

@ -221,6 +221,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();
@ -686,21 +690,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();
}
@ -891,6 +894,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) {
@ -907,6 +914,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 */

45
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 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 */
@ -502,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),
@ -968,9 +970,24 @@ 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) {
@ -987,6 +1004,7 @@ bool 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;
}
@ -997,7 +1015,7 @@ bool FixedwingPositionControl::in_takeoff_situation() {
const hrt_abstime delta_takeoff = 10000000;
const float throttle_threshold = 0.1f;
if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + _parameters.climbout_diff) {
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;
}
@ -1008,7 +1026,7 @@ void FixedwingPositionControl::do_takeoff_help(float *hold_altitude, float *pitc
{
/* demand "climbout_diff" m above ground if user switched into this mode during takeoff */
if (in_takeoff_situation()) {
*hold_altitude = _ground_alt + _parameters.climbout_diff;
*hold_altitude = _takeoff_ground_alt + _parameters.climbout_diff;
*pitch_limit_min = math::radians(10.0f);
} else {
*pitch_limit_min = _parameters.pitch_limit_min;
@ -1050,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) {
@ -1606,8 +1624,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
_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