diff --git a/conf/airframes/booz2_a2.xml b/conf/airframes/booz2_a2.xml index 55129f6508..87801b1f9f 100644 --- a/conf/airframes/booz2_a2.xml +++ b/conf/airframes/booz2_a2.xml @@ -92,7 +92,7 @@
- +
diff --git a/conf/settings/settings_booz2.xml b/conf/settings/settings_booz2.xml index ac23688dd6..77551700d7 100644 --- a/conf/settings/settings_booz2.xml +++ b/conf/settings/settings_booz2.xml @@ -37,8 +37,7 @@ - - + diff --git a/sw/airborne/booz/booz2_guidance_v.c b/sw/airborne/booz/booz2_guidance_v.c index cf9c86a325..4c4ab28d30 100644 --- a/sw/airborne/booz/booz2_guidance_v.c +++ b/sw/airborne/booz/booz2_guidance_v.c @@ -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; diff --git a/sw/airborne/booz/booz2_guidance_v_adpt.h b/sw/airborne/booz/booz2_guidance_v_adpt.h index b637cf2d47..2dfb606a85 100644 --- a/sw/airborne/booz/booz2_guidance_v_adpt.h +++ b/sw/airborne/booz/booz2_guidance_v_adpt.h @@ -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); + /* 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 */ diff --git a/sw/tools/calibration/150_ADC_GENERIC_1.bz2 b/sw/tools/calibration/150_ADC_GENERIC_1.bz2 new file mode 100644 index 0000000000..8463618ad0 Binary files /dev/null and b/sw/tools/calibration/150_ADC_GENERIC_1.bz2 differ diff --git a/sw/tools/calibration/150_ADC_GENERIC_2.bz2 b/sw/tools/calibration/150_ADC_GENERIC_2.bz2 new file mode 100644 index 0000000000..131c78304a Binary files /dev/null and b/sw/tools/calibration/150_ADC_GENERIC_2.bz2 differ diff --git a/sw/tools/calibration/151_ADC_GENERIC_1.bz2 b/sw/tools/calibration/151_ADC_GENERIC_1.bz2 new file mode 100644 index 0000000000..ccc1b41d47 Binary files /dev/null and b/sw/tools/calibration/151_ADC_GENERIC_1.bz2 differ diff --git a/sw/tools/calibration/151_ADC_GENERIC_2.bz2 b/sw/tools/calibration/151_ADC_GENERIC_2.bz2 new file mode 100644 index 0000000000..e871e1eda0 Binary files /dev/null and b/sw/tools/calibration/151_ADC_GENERIC_2.bz2 differ diff --git a/sw/tools/calibration/calib_accel_mag.sce b/sw/tools/calibration/calib_accel_mag.sce new file mode 100644 index 0000000000..495f9d49fc --- /dev/null +++ b/sw/tools/calibration/calib_accel_mag.sce @@ -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); \ No newline at end of file diff --git a/sw/tools/calibration/calib_baro.sce b/sw/tools/calibration/calib_baro.sce new file mode 100644 index 0000000000..957abda4f4 --- /dev/null +++ b/sw/tools/calibration/calib_baro.sce @@ -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(); \ No newline at end of file diff --git a/sw/tools/calibration/calibrate_utils.sci b/sw/tools/calibration/calibrate_utils.sci new file mode 100644 index 0000000000..ad8c284419 --- /dev/null +++ b/sw/tools/calibration/calibrate_utils.sci @@ -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("\n", s_name, -p(1)*resolution); +printf("\n", s_name, p(2)*resolution); +printf("\n", s_name, -p(3)*resolution); +printf("\n"); +printf("\n", s_name, round(p(4))); +printf("\n", s_name, round(p(5))); +printf("\n", s_name, round(p(6))); + +endfunction +