Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-10 06:59:54 +08:00
Code Issues Actions 25 Packages Projects Releases Wiki Activity

*** empty log message ***

Browse Source
This commit is contained in:
Antoine Drouin
2007-12-11 21:15:33 +00:00
parent 2a3899a56d
commit aa6ea5d487
4 changed files with 184 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/telemetry/booz.xml
+7 -5
View File
@@ -27,11 +27,13 @@
<process name="Filter">
<mode name="default">
<message name="IMU_GYRO" period=".017"/>
<message name="IMU_GYRO_LP" period=".017"/>
<message name="AHRS_STATE" period=".05"/>
<message name="AHRS_COV" period=".2"/>
<message name="AHRS_MEASURE" period=".05"/>
<!-- <message name="IMU_GYRO" period=".017"/> -->
<message name="IMU_GYRO_RAW" period=".017"/>
<!-- <message name="IMU_GYRO_LP" period=".017"/> -->
<!-- <message name="AHRS_STATE" period=".05"/> -->
<!-- <message name="AHRS_COV" period=".2"/> -->
<!-- <message name="AHRS_MEASURE" period=".05"/> -->
<message name="IMU_ACCEL_RAW" period=".017"/>
</mode>
<mode name="raw_sensors">
<message name="IMU_GYRO_RAW" period=".017"/>
sw/airborne/AMI601.c
+3 -3
View File
@@ -92,9 +92,9 @@ void ami601_periodic( void ) {
while (!ami601_i2c_done);
// LED_OFF(2);
ami601_foo1 = i2c_buf[0];
ami601_foo2 = i2c_buf[1];
ami601_foo3 = i2c_buf[2];
ami601_foo1 = i2c_buf[0]; // AA ?
ami601_foo2 = i2c_buf[1]; // 55 ?
ami601_foo3 = i2c_buf[2]; // ERR ?
uint8_t i;
for (i=0; i< AMI601_NB_CHAN; i++) {
sw/airborne/booz_filter_main.c
+8 -5
View File
@@ -44,8 +44,8 @@ STATIC_INLINE void booz_filter_main_init( void ) {
uart1_init_tx();
adc_init();
// i2c_init();
// ami601_init();
i2c_init();
ami601_init();
#endif
imu_v3_init();
@@ -65,9 +65,7 @@ STATIC_INLINE void booz_filter_main_event_task( void ) {
}
STATIC_INLINE void booz_filter_main_periodic_task( void ) {
/* triger measurements */
// ami601_periodic();
// DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
ImuPeriodic();
static uint8_t _62hz = 0;
_62hz++;
@@ -76,6 +74,11 @@ STATIC_INLINE void booz_filter_main_periodic_task( void ) {
case 0:
booz_filter_telemetry_periodic_task();
break;
case 1:
/* triger measurements */
ami601_periodic();
// Z Y X
DOWNLINK_SEND_IMU_MAG_RAW(&ami601_val[0], &ami601_val[4], &ami601_val[2]);
}
}
sw/in_progress/inertial/scilab/calib_rotation.sce
+166
View File
@@ -0,0 +1,166 @@
clear();
clf();
//##########
//##########
function [rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log(filename)
rm_x=[];
rm_y=[];
rm_z=[];
ra_x=[];
ra_y=[];
ra_z=[];
u=mopen(filename,'r');
while meof(u) == 0,
line = mgetl(u, 1);
if strindex(line, '#') ~= 1 & length(line) ~= 0,
[nb_scan, _mz, _my, _mx] = msscanf(1, line, '148 IMU_MAG_RAW %f %f %f');
if nb_scan == 3,
rm_z = [rm_z _mz];
rm_y = [rm_y _my];
rm_x = [rm_x _mx];
else
[nb_scan, _ax, _ay, _az] = msscanf(1, line, '148 IMU_ACCEL_RAW %f %f %f');
if nb_scan == 3,
ra_x = [ra_x _ax];
ra_y = [ra_y _ay];
ra_z = [ra_z _az];
end
end
end
end
mclose(u);
endfunction
//##########
//##########
function [nx, ny, nz, gx, gy, gz] = min_max_calib(dx, dy, dz)
min_x = min(dx);
max_x = max(dx);
nx = (max_x + min_x) / 2.;
gx = (max_x - min_x) / 2.;
min_y = min(dy);
max_y = max(dy);
ny = (max_y + min_y) / 2.;
gy = (max_y - min_y) / 2.;
min_z = min(dz);
max_z = max(dz);
nz = (max_z + min_z) / 2.;
gz = (max_z - min_z) / 2.;
endfunction
[rm_z, rm_y, rm_x, ra_x, ra_y, ra_z] = read_log('log_magnetometer');
[m_mm_nx, m_mm_ny, m_mm_nz, m_mm_gx, m_mm_gy, m_mm_gz] = ...
min_max_calib(rm_x, rm_y, rm_z);
[a_mm_nx, a_mm_ny, a_mm_nz, a_mm_gx, a_mm_gy, a_mm_gz] = ...
min_max_calib(ra_x, ra_y, ra_z);
m_mm_x = (rm_x - m_mm_nx) / m_mm_gx;
m_mm_y = (rm_y - m_mm_ny) / m_mm_gy;
m_mm_z = (rm_z - m_mm_nz) / m_mm_gz;
printf('mx : n -> %f g -> %f\n', m_mm_nx, m_mm_gx);
printf('my : n -> %f g -> %f\n', m_mm_ny, m_mm_gy);
printf('mz : n -> %f g -> %f\n', m_mm_nz, m_mm_gz);
m_mm_norm = sqrt(m_mm_x^2 + m_mm_y^2 + m_mm_z^2);
idx_m = 1:length(rm_z);
a_mm_x = (ra_x - a_mm_nx) / a_mm_gx;
a_mm_y = (ra_y - a_mm_ny) / a_mm_gy;
a_mm_z = (ra_z - a_mm_nz) / a_mm_gz;
a_mm_norm = sqrt(a_mm_x^2 + a_mm_y^2 + a_mm_z^2);
idx_a = 1:length(ra_z);
subplot(3,2,1);
param3d(rm_x, rm_y, rm_z);
subplot(3,2,2);
param3d(ra_x, ra_y, ra_z);
subplot(3,2,3);
plot(idx_m, m_mm_norm, idx_m, ones(1, length(rm_z)));
subplot(3,2,4);
plot(idx_a, a_mm_norm, idx_a, ones(1, length(ra_z)));
subplot(3,2,5);
alpha = 0:0.1:2*%pi;
c_x = m_mm_nx+ m_mm_gx * cos(alpha);
c_y = m_mm_ny+ m_mm_gy * sin(alpha);
plot(rm_x, rm_y, c_x, c_y)
subplot(3,2,6);
c_x = a_mm_nx+ a_mm_gx * cos(alpha);
c_y = a_mm_ny+ a_mm_gy * sin(alpha);
plot(ra_x, ra_y, c_x, c_y)
//
//
//
function err = cost_fun(p, z)
err = (z(1) - p(1))^2 / p(4)^2 + ...
(z(2) - p(2))^2 / p(5)^2 + ...
(z(3) - p(3))^2 / p(6)^2 - 1;
endfunction
if 0
p0 = [ mx_neutral; my_neutral; mz_neutral; mx_gain; my_gain; mz_gain];
Z = [rm_x; rm_y; rm_z];
[p, err] = datafit(cost_fun, Z, p0)
else
// expe 0
p = [ 1925.0482
2124.7774
1975.155
446.40699
426.51219
412.88952 ];
// expe 1 cut
p = [ 1933.2636
2136.651
1977.4088
464.52769
452.71114
433.81507 ];
// expe 2 uncut
p = [ 1931.7855
2114.7077
1976.6819
444.15179
472.04251
479.76116 ];
// expe 2 cut
p = [ 1931.1511
2145.2051
1977.5055
467.83817
450.52012
447.26642 ];
end
opt_nx = p(1);
opt_ny = p(2);
opt_nz = p(3);
opt_gx = p(4);
opt_gy = p(5);
opt_gz = p(6);
opt_mx = (rm_x - opt_nx) / opt_gx;
opt_my = (rm_y - opt_ny) / opt_gy;
opt_mz = (rm_z - opt_nz) / opt_gz;
//opt_norm = sqrt(opt_mx^2 + opt_my^2 + opt_mz^2);
//subplot(2,2,3);
//plot(idx, opt_norm, 'cya+');