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

Commander: updated mag calibration routine, matlab script updates

This commit is contained in:
Lorenz Meier 2015-06-25 13:20:43 +02:00
parent 338404b4b3
commit c402d0c2f7
2 changed files with 27 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
Tools/Matlab/plot_mag.m
View File

@ -13,6 +13,12 @@
% MATLAB: 0.5122, 0.5065, 0.4915 % 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; close all;
clear all; clear all;
@ -26,11 +32,11 @@ ymin = -ymax;
zmax = plot_scale; zmax = plot_scale;
zmin = -zmax; zmin = -zmax;
mag0_raw = load('../../mag0_raw2.csv'); mag0_raw = load('../../mag0_raw3.csv');
mag1_raw = load('../../mag1_raw2.csv'); mag1_raw = load('../../mag1_raw3.csv');
mag0_cal = load('../../mag0_cal2.csv'); mag0_cal = load('../../mag0_cal3.csv');
mag1_cal = load('../../mag1_cal2.csv'); mag1_cal = load('../../mag1_cal3.csv');
fm0r = figure(); fm0r = figure();
@ -39,10 +45,11 @@ mag0_y_scale = 0.99;
mag0_z_scale = 0.95; mag0_z_scale = 0.95;
plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r'); plot3(mag0_raw(:,1), mag0_raw(:,2), mag0_raw(:,3), '*r');
[center, radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] ); [mag0_raw_center, mag0_raw_radii, evecs, pars ] = ellipsoid_fit( [mag0_raw(:,1) mag0_raw(:,2) mag0_raw(:,3)] );
center mag0_raw_center
radii mag0_raw_radii
axis([xmin xmax ymin ymax zmin zmax]) axis([xmin xmax ymin ymax zmin zmax])
viscircles([mag0_raw_center(1), mag0_raw_center(2)], [mag0_raw_radii(1)]);
fm1r = figure(); fm1r = figure();
plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r'); plot3(mag1_raw(:,1), mag1_raw(:,2), mag1_raw(:,3), '*r');
@ -53,11 +60,18 @@ axis([xmin xmax ymin ymax zmin zmax])
fm0c = figure(); fm0c = figure();
plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b'); plot3(mag0_cal(:,1) .* mag0_x_scale, mag0_cal(:,2) .* mag0_y_scale, mag0_cal(:,3) .* mag0_z_scale, '*b');
[center, radii, evecs, pars ] = ellipsoid_fit( [mag1_raw(:,1) mag1_raw(:,2) mag1_raw(:,3)] ); [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] );
center mag0_cal_center
radii mag0_cal_radii
axis([xmin xmax ymin ymax zmin zmax]) axis([xmin xmax ymin ymax zmin zmax])
viscircles([0, 0], [mag0_cal_radii(3)]);
fm1c = figure(); fm1c = figure();
plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b'); plot3(mag1_cal(:,1), mag1_cal(:,2), mag1_cal(:,3), '*b');
axis([xmin xmax ymin ymax zmin zmax]) 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))

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 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 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 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]; struct pollfd fds[1];
fds[0].fd = accel_sub; 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 */ /* not still, reset still start time */
if (t_still != 0) { if (t_still != 0) {
mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still..."); mavlink_and_console_log_info(mavlink_fd, "[cal] detected motion, hold still...");
usleep(500000); usleep(200000);
t_still = 0; t_still = 0;
} }
} }
@ -488,7 +488,7 @@ calibrate_return calibrate_from_orientation(int mavlink_fd,
// Note that this side is complete // Note that this side is complete
side_data_collected[orient] = true; side_data_collected[orient] = true;
tune_neutral(true); tune_neutral(true);
usleep(500000); usleep(200000);
} }
if (sub_accel >= 0) { if (sub_accel >= 0) {