mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 16:58:48 +08:00
*** empty log message ***
This commit is contained in:
Antoine Drouin
@@ -92,7 +92,7 @@
|
||||
</section>
|
||||
|
||||
<section name="INS" prefix="BOOZ_INS_">
|
||||
<define name="BARO_SENS" value="18.1" integer="16"/>
|
||||
<define name="BARO_SENS" value="14.9" integer="16"/>
|
||||
</section>
|
||||
|
||||
<section name="GUIDANCE_V" prefix="BOOZ2_GUIDANCE_V_">
|
||||
|
||||
@@ -37,8 +37,7 @@
|
||||
<dl_settings NAME="Vert Loop">
|
||||
<dl_setting var="booz2_guidance_v_kp" min="-600" step="1" max="0" module="booz2_guidance_v" shortname="kp"/>
|
||||
<dl_setting var="booz2_guidance_v_kd" min="-600" step="1" max="0" module="booz2_guidance_v" shortname="kd"/>
|
||||
<dl_setting var="booz2_guidance_v_ki" min="-400" step="1" max="0" module="booz2_guidance_v" shortname="ki"/>
|
||||
<dl_setting var="booz2_guidance_v_z_sp" min="50" step="1" max="950" module="booz2_guidance_v" shortname="sp"/>
|
||||
<dl_setting var="booz2_guidance_v_z_sp" min="-20" step="1" max="10" module="booz2_guidance_v" shortname="sp"/>
|
||||
</dl_settings>
|
||||
|
||||
<dl_settings NAME="Horiz Loop">
|
||||
|
||||
@@ -136,16 +136,16 @@ static inline void run_hover_loop(bool_t in_flight) {
|
||||
else
|
||||
booz2_guidance_v_z_sum_err = 0;
|
||||
|
||||
/* our nominal command : (g + zdd)/m */
|
||||
// const int32_t inv_m = BOOZ_INT_OF_FLOAT(0.140, IACCEL_RES);
|
||||
const int32_t inv_m = b2_gv_adapt_X>>(B2_GV_ADAPT_X_FRAC - IACCEL_RES);
|
||||
booz2_guidance_v_ff_cmd = (BOOZ_INT_OF_FLOAT(9.81, IACCEL_RES) - booz2_guidance_v_zdd_ref) / inv_m;
|
||||
// booz2_guidance_v_ff_cmd = BOOZ2_GUIDANCE_V_HOVER_POWER;
|
||||
|
||||
booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) +
|
||||
((-booz2_guidance_v_kd * err_zd) >> 21) +
|
||||
((-booz2_guidance_v_ki * booz2_guidance_v_z_sum_err) >> 24);
|
||||
|
||||
|
||||
/* our error command */
|
||||
booz2_guidance_v_fb_cmd = ((-booz2_guidance_v_kp * err_z) >> 12) +
|
||||
((-booz2_guidance_v_kd * err_zd) >> 21);
|
||||
|
||||
booz2_guidance_v_delta_t = booz2_guidance_v_ff_cmd + booz2_guidance_v_fb_cmd;
|
||||
|
||||
|
||||
|
||||
@@ -1,24 +1,62 @@
|
||||
/*
|
||||
* $Id$
|
||||
*
|
||||
* Copyright (C) 2009 Pascal Brisset, Antoine Drouin, Gautier Hatenberger
|
||||
*
|
||||
* This file is part of paparazzi.
|
||||
*
|
||||
* paparazzi is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 2, or (at your option)
|
||||
* any later version.
|
||||
*
|
||||
* paparazzi is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with paparazzi; see the file COPYING. If not, write to
|
||||
* the Free Software Foundation, 59 Temple Place - Suite 330,
|
||||
* Boston, MA 02111-1307, USA.
|
||||
*
|
||||
*
|
||||
* Adaptation bloc of the vertical guidance
|
||||
* This is a dimension one kalman filter estimating the invert of the mass
|
||||
* needed by the invert dynamic model to produce a nominal command
|
||||
*
|
||||
*/
|
||||
|
||||
#ifndef BOOZ2_GUIDANCE_V_ADPT
|
||||
#define BOOZ2_GUIDANCE_V_ADPT
|
||||
|
||||
extern int32_t b2_gv_adapt_X;
|
||||
extern int32_t b2_gv_adapt_P;
|
||||
|
||||
|
||||
|
||||
#ifdef B2_GUIDANCE_V_C
|
||||
|
||||
/* */
|
||||
/* Our State
|
||||
Q13.18
|
||||
*/
|
||||
int32_t b2_gv_adapt_X;
|
||||
#define B2_GV_ADAPT_X_FRAC 18
|
||||
|
||||
/* Q5.26 */
|
||||
/* Our covariance
|
||||
Q13.18
|
||||
*/
|
||||
int32_t b2_gv_adapt_P;
|
||||
#define B2_GV_ADAPT_P_FRAC 18
|
||||
|
||||
|
||||
/* Initial State and Covariance */
|
||||
#define B2_GV_ADAPT_X0_F 0.15
|
||||
#define B2_GV_ADAPT_X0 BOOZ_INT_OF_FLOAT(B2_GV_ADAPT_X0_F, B2_GV_ADAPT_X_FRAC)
|
||||
#define B2_GV_ADAPT_P0_F 0.5
|
||||
#define B2_GV_ADAPT_P0 BOOZ_INT_OF_FLOAT(B2_GV_ADAPT_P0_F, B2_GV_ADAPT_P_FRAC)
|
||||
|
||||
/* System and Measuremement noises */
|
||||
#define B2_GV_ADAPT_SYS_NOISE_F 0.00005
|
||||
#define B2_GV_ADAPT_SYS_NOISE BOOZ_INT_OF_FLOAT(B2_GV_ADAPT_SYS_NOISE_F, B2_GV_ADAPT_P_FRAC)
|
||||
#define B2_GV_ADAPT_MEAS_NOISE_F 2.0
|
||||
@@ -31,24 +69,42 @@ static inline void b2_gv_adapt_init(void) {
|
||||
}
|
||||
|
||||
/*
|
||||
zdd_meas : IACCEL_RES ... shall we or shall we not unbias ? :)
|
||||
zdd_meas : IACCEL_RES
|
||||
thrust_applied : controller input [2-200]
|
||||
*/
|
||||
#define K_FRAC 12
|
||||
static inline void b2_gv_adapt_run(int32_t zdd_meas, int32_t thrust_applied) {
|
||||
|
||||
/* Do you really think we want to divide by zero ? */
|
||||
if (thrust_applied == 0) return;
|
||||
/* propagate covariance */
|
||||
/* We don't propagate state, it's constant ! */
|
||||
/* We propagate our covariance */
|
||||
b2_gv_adapt_P = b2_gv_adapt_P + B2_GV_ADAPT_SYS_NOISE;
|
||||
/* Compute our measurement. If zdd_meas is in the range +/-5g, meas is less than 24 bits */
|
||||
int32_t meas = (((int32_t)BOOZ_INT_OF_FLOAT(9.81, IACCEL_RES) - zdd_meas)<<(B2_GV_ADAPT_X_FRAC - IACCEL_RES)) / thrust_applied;
|
||||
/* Compute a residual */
|
||||
int32_t residual = meas - b2_gv_adapt_X;
|
||||
/* Covariance Error */
|
||||
int32_t E = b2_gv_adapt_P + B2_GV_ADAPT_MEAS_NOISE;
|
||||
/* Kalman gain */
|
||||
int32_t K = (b2_gv_adapt_P<<K_FRAC) / E;
|
||||
/* Update Covariance */
|
||||
b2_gv_adapt_P = b2_gv_adapt_P - ((K * b2_gv_adapt_P)>>K_FRAC);
|
||||
/* Don't let it climb over initial value */
|
||||
if (b2_gv_adapt_P > B2_GV_ADAPT_P0) b2_gv_adapt_P = B2_GV_ADAPT_P0;
|
||||
/* Update State */
|
||||
b2_gv_adapt_X = b2_gv_adapt_X + ((K * residual)>>K_FRAC);
|
||||
/* Again don't let it climb over a value that would
|
||||
give less than zero throttle and don't let it down to zero.
|
||||
30254 = MAX_ACCEL*B2_GV_ADAPT_X_FRAC/MAX_THROTTLE
|
||||
aka
|
||||
30254 = 3*9.81*2^10/255
|
||||
2571632 = 9.81*2^18
|
||||
*/
|
||||
Bound(b2_gv_adapt_X, 30254, 2571632);
|
||||
}
|
||||
|
||||
|
||||
#endif /* B2_GUIDANCE_V_C */
|
||||
|
||||
|
||||
#endif /* BOOZ2_GUIDANCE_V_ADPT */
|
||||
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
@@ -0,0 +1,73 @@
|
||||
//
|
||||
// compute neutral and scale factor from a serie of idealy homogeneously spread raw
|
||||
// measurements components, assuming the norme of the measured quantity is constant
|
||||
//
|
||||
|
||||
clear();
|
||||
|
||||
ac_id = 155;
|
||||
//log_name = '/home/john.stower/paparazzi3/var/logs/08_11_24__12_53_41.data';
|
||||
log_name = 'calib_mag4.data';
|
||||
//log_name = 'log_accel_booz2_a1_2';
|
||||
//log_name = 'log_accel_booz2_a1_5';
|
||||
|
||||
SENSOR_ACCEL = 0;
|
||||
SENSOR_MAG = 1;
|
||||
|
||||
sensor_type = SENSOR_MAG;
|
||||
|
||||
select sensor_type
|
||||
|
||||
case SENSOR_ACCEL then
|
||||
sensor_name = "ACCEL";
|
||||
ref_norm = 9.81;
|
||||
resolution = 2^10;
|
||||
reject_noisy_data = 1;
|
||||
threshold = 500;
|
||||
size_avg = 10;
|
||||
|
||||
case SENSOR_MAG then
|
||||
sensor_name = "MAG";
|
||||
ref_norm = 1.;
|
||||
resolution = 2^11;
|
||||
reject_noisy_data = 0;
|
||||
|
||||
end
|
||||
|
||||
exec("calibrate_utils.sci");
|
||||
|
||||
|
||||
// read log
|
||||
[time, sensor_raw] = read_log_sensor_raw(ac_id, sensor_name, log_name);
|
||||
|
||||
// plot raw sensors
|
||||
scf();
|
||||
display_raw_sensors(time, sensor_raw);
|
||||
|
||||
// reject noisy data
|
||||
if reject_noisy_data
|
||||
[time_filtered, sensor_filtered] = filter_noisy_data(time, sensor_raw, threshold, size_avg);
|
||||
time = time_filtered;
|
||||
sensor_raw = sensor_filtered;
|
||||
end
|
||||
|
||||
// compute intial calibration
|
||||
[p0] = min_max_calib(sensor_raw, ref_norm);
|
||||
|
||||
// plot initial guess calibrated sensors
|
||||
[mic, gmic] = apply_scaling( p0(1:3), p0(4:6), sensor_raw);
|
||||
scf();
|
||||
display_calib_sensor(time, mic, gmic, 'initial calibration', ref_norm);
|
||||
|
||||
// optimise initial calibration
|
||||
[p, err] = datafit_calib(sensor_raw, ref_norm, p0);
|
||||
//p = p0;
|
||||
printf("datafit error : %f\n", err);
|
||||
|
||||
// plot optimized calibrated sensors
|
||||
[mfc, gmfc] = apply_scaling( p(1:3), p(4:6), sensor_raw);
|
||||
scf();
|
||||
display_calib_sensor(time, mfc, gmfc, 'final calibration', ref_norm);
|
||||
|
||||
// print xml for airframe file
|
||||
print_xml(sensor_name, p, resolution);
|
||||
@@ -0,0 +1,44 @@
|
||||
clear();
|
||||
|
||||
ac_id = 150
|
||||
test = 1;
|
||||
|
||||
M=fscanfMat(sprintf('%d_ADC_GENERIC_%d', ac_id, test));
|
||||
|
||||
time = M(:,1);
|
||||
id = M(:,2);
|
||||
off = M(:,3);
|
||||
adc = M(:,4);
|
||||
|
||||
select test
|
||||
case 1
|
||||
t1 = find(time > 0 & time < 40);
|
||||
t2 = find(time > 70 & time < 120);
|
||||
t3 = find(time > 220 & time < 250);
|
||||
case 2
|
||||
t3 = find(time > 40 & time < 50);
|
||||
t2 = find(time > 140 & time < 160);
|
||||
t1 = find(time > 190 & time < 210);
|
||||
end
|
||||
|
||||
v1 = mean(adc(t1))
|
||||
v2 = mean(adc(t2))
|
||||
v3 = mean(adc(t3))
|
||||
|
||||
k1 = (v1-v3)/9.
|
||||
k2 = (v1-v2)/4.5
|
||||
k3 = (v2-v3)/4.5
|
||||
|
||||
k = (k1 + k2 + k3) / 3
|
||||
|
||||
clf();
|
||||
|
||||
drawlater();
|
||||
|
||||
xtitle('X', 'time (s)','');
|
||||
plot2d(time, adc, 4);
|
||||
plot2d(time(t1), adc(t1), 1);
|
||||
plot2d(time(t2), adc(t2), 2);
|
||||
plot2d(time(t3), adc(t3), 3);
|
||||
|
||||
drawnow();
|
||||
@@ -0,0 +1,140 @@
|
||||
|
||||
|
||||
|
||||
function [time, gyro_raw, turntable] = read_gyro_log(ac_id, tt_id, filename)
|
||||
fmt_gyro = sprintf('%%f %d IMU_GYRO_RAW %%f %%f %%f', ac_id);
|
||||
fmt_tt = sprintf('%%f %d IMU_TURNTABLE %%f', tt_id);
|
||||
time = [];
|
||||
gyro_raw = [];
|
||||
turntable = [];
|
||||
tt = 0;
|
||||
u=mopen(filename,'r');
|
||||
while meof(u) == 0,
|
||||
line = mgetl(u, 1);
|
||||
if strindex(line, '#') ~= 1 & length(line) ~= 0,
|
||||
[nb_scan, _time, _tt] = ...
|
||||
msscanf(1, line, fmt_tt);
|
||||
if nb_scan == 2
|
||||
tt = _tt;
|
||||
end
|
||||
[nb_scan, _time, _rgx, _rgy, _rgz] = ...
|
||||
msscanf(1, line, fmt_gyro);
|
||||
if nb_scan == 4
|
||||
time = [time _time];
|
||||
gyro_raw = [gyro_raw [_rgx; _rgy; _rgz]];
|
||||
turntable = [turntable tt];
|
||||
end
|
||||
end
|
||||
end
|
||||
mclose(u);
|
||||
endfunction
|
||||
|
||||
|
||||
|
||||
|
||||
function [time, sensor_raw] = read_log_sensor_raw(ac_id, sensor_name, filename)
|
||||
|
||||
fmt = sprintf('%%f %d IMU_GYRO_RAW %%f %%f %%f', ac_id, sensor_name);
|
||||
time = [];
|
||||
sensor_raw = [];
|
||||
u=mopen(filename,'r');
|
||||
while meof(u) == 0,
|
||||
line = mgetl(u, 1);
|
||||
if strindex(line, '#') ~= 1 & length(line) ~= 0,
|
||||
[nb_scan, _time, _rax, _ray, _raz] = ...
|
||||
msscanf(1, line, fmt);
|
||||
if nb_scan == 4
|
||||
time = [time _time];
|
||||
sensor_raw = [sensor_raw [_rax; _ray; _raz]];
|
||||
end
|
||||
end
|
||||
end
|
||||
mclose(u);
|
||||
|
||||
endfunction
|
||||
|
||||
function [p] = min_max_calib(sensor_raw, ref_norm)
|
||||
min_raw = min(sensor_raw, 'c');
|
||||
max_raw = max(sensor_raw, 'c');
|
||||
neutral = (min_raw + max_raw) / 2;
|
||||
sensitivity = 2 * [ref_norm; ref_norm; ref_norm] ./ (max_raw - min_raw);
|
||||
p = [ sensitivity; neutral ];
|
||||
endfunction
|
||||
|
||||
function [p, err] = datafit_calib(sensor_raw, ref_norm, p0)
|
||||
function e = err_norm(p, z)
|
||||
g = p(1:3);
|
||||
n = p(4:6);
|
||||
a = g .* (z-n);
|
||||
e = ref_norm^2 - a(1)^2 - a(2)^2 - a(3)^2;
|
||||
endfunction
|
||||
[p, err] = datafit(err_norm, sensor_raw, p0, 'gc');
|
||||
endfunction
|
||||
|
||||
|
||||
function display_raw_sensors(time, sensor_raw)
|
||||
plot2d([time; time; time]', sensor_raw', leg="X@Y@Z");
|
||||
xtitle('raw sensor');
|
||||
endfunction
|
||||
|
||||
|
||||
function display_calib_sensor(time, _comp, _norm, lab, ref)
|
||||
|
||||
subplot(2,1,1);
|
||||
plot2d(time, ref * ones(1,length(time)));
|
||||
plot2d(time, -ref * ones(1,length(time)));
|
||||
plot2d(time, zeros(1,length(time)));
|
||||
plot2d([time; time; time]', _comp', leg="X@Y@Z");
|
||||
|
||||
xtitle("Components:"+lab);
|
||||
subplot(2,1,2);
|
||||
plot2d(time, ref * ones(1,length(time)));
|
||||
// plot2d3(time, _norm, 2);
|
||||
plot2d(time, _norm, 2);
|
||||
xtitle("Norme:"+lab);
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [time_f, sensor_f] = filter_noisy_data(time_raw, sensor_raw, threshold, size_avg)
|
||||
|
||||
sensor_f = [];
|
||||
time_f = [];
|
||||
for i=size_avg+1:length(sensor_raw(1,:))-size_avg
|
||||
s = variance(sensor_raw(:, i-size_avg:i+size_avg), 'c');
|
||||
if norm(s) < threshold
|
||||
sensor_f = [sensor_f sensor_raw(:,i)];
|
||||
time_f = [time_f time(i)];
|
||||
end
|
||||
end
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function [ac, gc] = apply_scaling(gain, neutral, ar)
|
||||
|
||||
ac = [];
|
||||
gc = [];
|
||||
for i=1:length(ar(1,:))
|
||||
_ac = (ar(:,i) - neutral) .* gain;
|
||||
ac = [ac _ac];
|
||||
_gc = sqrt(_ac(1)^2 + _ac(2)^2 + _ac(3)^2);
|
||||
gc = [gc _gc];
|
||||
end
|
||||
|
||||
endfunction
|
||||
|
||||
|
||||
function print_xml(s_name, p, resolution)
|
||||
|
||||
printf("\n");
|
||||
printf("<define name=""%s_X_SENS"" value=""%.8f"" integer=""16""/>\n", s_name, -p(1)*resolution);
|
||||
printf("<define name=""%s_Y_SENS"" value=""%.8f"" integer=""16""/>\n", s_name, p(2)*resolution);
|
||||
printf("<define name=""%s_Z_SENS"" value=""%.8f"" integer=""16""/>\n", s_name, -p(3)*resolution);
|
||||
printf("\n");
|
||||
printf("<define name=""%s_X_NEUTRAL"" value=""%d""/>\n", s_name, round(p(4)));
|
||||
printf("<define name=""%s_Y_NEUTRAL"" value=""%d""/>\n", s_name, round(p(5)));
|
||||
printf("<define name=""%s_Z_NEUTRAL"" value=""%d""/>\n", s_name, round(p(6)));
|
||||
|
||||
endfunction
|
||||
|
||||
Reference in New Issue
Block a user