[paparazzi-commits] [4698]

[antoine drouin]

Revision: 4698
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4698
Author:   poine
Date:     2010-03-15 15:53:31 +0000 (Mon, 15 Mar 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/booz/test/Makefile

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/booz/test/imu_dummy.h
    paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c
    paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.sce
    paparazzi3/trunk/sw/airborne/stm32/i2c_hw.h

Modified: paparazzi3/trunk/sw/airborne/booz/test/Makefile
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/Makefile     2010-03-15 15:48:12 UTC 
(rev 4697)
+++ paparazzi3/trunk/sw/airborne/booz/test/Makefile     2010-03-15 15:53:31 UTC 
(rev 4698)
@@ -22,11 +22,13 @@
 ##
 
 CC = gcc
-CFLAGS = -Wall -I.. -I../.. -I../../test/ -I../../../include -Wall
+CFLAGS = -std=gnu99 -Wall -I.. -I../.. -I../../test/ -I../../../include -I 
../../booz_priv
 LDFLAGS = -lm
 
+CFLAGS += -I../../../../var/BOOZ2_A1P
+test_mlkf: test_mlkf.c ../booz_ahrs.c ../../booz_priv/ahrs/booz_ahrs_mlkf.c 
../../booz_priv/ahrs/booz_ahrs_mlkf_opt.c  ../booz_imu.c 
../../math/pprz_trig_int.c ./imu_dummy.c ../ahrs/booz_ahrs_aligner.c
+       $(CC) $(CFLAGS) -DBOOZ_IMU_TYPE_H=\"test/imu_dummy.h\" -o $@ $^ 
$(LDFLAGS)
 
-
 test_vg_ref: test_vg_ref.c
        $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
@@ -40,4 +42,13 @@
 test_scaling: test_scaling.c
        $(CC) $(CFLAGS) -o $@ $^ $(LDFLAGS)
 
+TEST_ATT_CFLAGS =  -DSTABILISATION_ATTITUDE_TYPE_INT \
+                  
-DSTABILISATION_ATTITUDE_H=\"stabilization/booz_stabilization_attitude_int.h\" \
+                  
-DSTABILISATION_ATTITUDE_REF_H=\"stabilization/booz_stabilization_attitude_ref_quat_int.h\"
 
+test_att_ref: test_att_ref.c 
../stabilization/booz_stabilization_attitude_ref_quat_int.c
+       $(CC) $(CFLAGS) $(TEST_ATT_CFLAGS) -I 
/home/poine/work/savannah/paparazzi3/trunk/var/BOOZ2_A1 -o $@ $^ $(LDFLAGS)
+
+
+clean:
+       rm -f *~ test_att_ref
\ No newline at end of file

Added: paparazzi3/trunk/sw/airborne/booz/test/imu_dummy.h
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/imu_dummy.h                          
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/test/imu_dummy.h  2010-03-15 15:53:31 UTC 
(rev 4698)
@@ -0,0 +1,2 @@
+
+

Added: paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c                          
(rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.c  2010-03-15 15:53:31 UTC 
(rev 4698)
@@ -0,0 +1,116 @@
+#include <stdio.h>
+#include <string.h>
+
+#include "math/pprz_algebra_double.h"
+#include "booz_imu.h"
+#include "booz_ahrs.h"
+#include "ahrs/booz_ahrs_mlkf.h"
+
+static void read_data(const char* filename);
+static void feed_imu(int i);
+static void store_filter_output(int i);
+static void dump_output(const char* filename);
+
+#define IN_FILE 
"../../../simulator/scilab/q6d/data/stop_stop_state_sensors.txt"
+#define OUT_FILE "./out.txt"
+
+#define MAX_SAMPLE 15000
+struct test_sample {
+  double time;
+  struct DoubleQuat  quat_true;
+  struct DoubleRates omega_true;
+  struct DoubleRates gyro;
+  struct DoubleVect3 accel;
+  struct DoubleVect3 mag;
+};
+static struct test_sample samples[MAX_SAMPLE];
+static int nb_samples;
+
+struct test_output {
+  struct FloatQuat  quat_est;
+  struct FloatRates bias_est;
+  struct FloatRates rate_est;
+  float P[6][6];
+};
+static struct test_output output[MAX_SAMPLE];
+
+
+int main(int argc, char** argv) {
+  
+  read_data(IN_FILE);
+
+  booz_imu_init();
+  booz_ahrs_init();
+
+  for (int i=0; i<nb_samples; i++) {
+    feed_imu(i);
+    booz_ahrs_propagate();
+    booz_ahrs_update_accel();
+    booz_ahrs_update_mag();
+    store_filter_output(i);
+  }
+  dump_output(OUT_FILE);
+
+  return 0;
+}
+
+
+static void read_data(const char* filename) {
+  FILE* fd = fopen(filename, "r");
+  nb_samples = 0;
+  int ret = 0;
+  do {
+    struct test_sample* s = &samples[nb_samples];
+    ret = fscanf(fd, "%lf [%lf %lf %lf %lf] [%lf %lf %lf] [%lf %lf %lf] [%lf 
%lf %lf] [%lf %lf %lf]",
+                &s->time,
+                &s->quat_true.qi, &s->quat_true.qx, &s->quat_true.qy, 
&s->quat_true.qz,
+                &s->omega_true.p, &s->omega_true.q, &s->omega_true.r,
+                &s->gyro.p, &s->gyro.q, &s->gyro.r,
+                &s->accel.x, &s->accel.y, &s->accel.z,
+                &s->mag.x, &s->mag.y, &s->mag.z );
+    nb_samples++;
+  }
+  while (ret == 17  && nb_samples < MAX_SAMPLE);
+  nb_samples--;
+  fclose(fd);
+  printf("read %d points in file %s\n", nb_samples, filename);
+}
+
+
+static void feed_imu(int i) {
+  if (i>0) {
+    RATES_COPY(booz_imu.gyro_prev, booz_imu.gyro);
+  }
+  else {
+    RATES_BFP_OF_REAL(booz_imu.gyro_prev, samples[0].gyro);
+  }
+  RATES_BFP_OF_REAL(booz_imu.gyro, samples[i].gyro);
+  ACCELS_BFP_OF_REAL(booz_imu.accel, samples[i].accel);
+  MAGS_BFP_OF_REAL(booz_imu.mag, samples[i].mag);
+}
+
+
+static void store_filter_output(int i) {
+  
+  QUAT_COPY(output[i].quat_est, booz_ahrs_float.ltp_to_imu_quat);
+  RATES_COPY(output[i].bias_est, booz_ahrs_mlkf.gyro_bias);
+  RATES_COPY(output[i].rate_est, booz_ahrs_float.imu_rate);
+  memcpy(output[i].P, booz_ahrs_mlkf.P, sizeof(booz_ahrs_mlkf.P));
+
+}
+
+static void dump_output(const char* filename) {
+  FILE* fd = fopen(filename, "w");
+  int i;
+  for (i=0; i<nb_samples; i++) {
+    fprintf(fd, "%.16f [%.16f %.16f %.16f %.16f] [%.16f %.16f %.16f] [%.16f 
%.16f %.16f] [%.16f %.16f %.16f %.16f %.16f %.16f]\n", 
+           samples[i].time,
+           output[i].quat_est.qi, output[i].quat_est.qx, 
output[i].quat_est.qy, output[i].quat_est.qz, // quaternion 
+           output[i].rate_est.p, output[i].rate_est.q, output[i].rate_est.r,   
                        // omega
+           output[i].bias_est.p, output[i].bias_est.q, output[i].bias_est.r,   
                        // bias
+            output[i].P[0][0], output[i].P[1][1], output[i].P[2][2],           
                         // covariance
+            output[i].P[3][3], output[i].P[4][4], output[i].P[5][5] );
+  }
+  fclose(fd);
+  printf("wrote %d points in file %s\n", nb_samples, filename);
+}

Added: paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.sce
===================================================================
--- paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.sce                        
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/booz/test/test_mlkf.sce        2010-03-15 
15:53:31 UTC (rev 4698)
@@ -0,0 +1,121 @@
+clear();
+clearglobal();
+
+
+exec('../../../simulator/scilab/q6d/q6d_fdm.sci');
+exec('../../../simulator/scilab/q6d/q6d_algebra.sci');
+
+global ahrs_quat;
+global ahrs_omega;
+global ahrs_bias;
+global ahrs_cov;
+
+function read_ahrs_output(filename)
+  global ahrs_quat;
+  global ahrs_omega;
+  global ahrs_bias;
+  global ahrs_cov;
+  ahrs_quat  = zeros(4,length(time));
+  ahrs_omega = zeros(3,length(time));
+  ahrs_bias  = zeros(3,length(time));
+  ahrs_cov   = zeros(6,length(time));
+  i=1;
+  u=mopen(filename, 'r');
+  while meof(u) == 0 & i <= length(time)
+    line = mgetl(u, 1);
+    if line ~= []
+      [nb_scan, t, qi, qx, qy, qz, p, q, r, bp, bq, br, c11, c22, c33, c44, 
c55, c66] = ...
+         msscanf(1, line, '%f [%f %f %f %f] [%f %f %f] [%f %f %f] [%f %f %f %f 
%f %f]');
+      if nb_scan == 17
+       ahrs_quat(:,i)  = [qi qx qy qz]';
+       ahrs_omega(:,i) = [p q r]';
+       ahrs_bias(:,i)  = [bp bq br]';
+       ahrs_cov(:,i)   = [c11 c22 c33 c44 c55 c66]';
+       i = i+1;
+      end
+    end
+  end
+  mclose(u);
+endfunction
+
+function display_ahrs_state()
+  clf();
+  f=get("current_figure");
+  f.figure_name="AHRS state";
+
+  eul_fdm  = zeros(3,length(time));
+  eul_ahrs = zeros(3,length(time));
+  
+  for i=1:length(time)
+    eul_fdm(:,i)  = euler_of_quat(fdm_state(FDM_SQI:FDM_SQZ,i));
+    eul_ahrs(:,i) = euler_of_quat(ahrs_quat(:,i));
+  end
+  
+  subplot(4,3,1);
+  plot2d(time, deg_of_rad(eul_ahrs(1,:)), 3);
+  plot2d(time, deg_of_rad(eul_fdm(1,:)), 2);
+  legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul"); 
+  xtitle('Phi');
+
+  subplot(4,3,2);
+  plot2d(time, deg_of_rad(eul_ahrs(2,:)), 3);
+  plot2d(time, deg_of_rad(eul_fdm(2,:)), 2);
+  legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul"); 
+  xtitle('Theta');
+  
+  subplot(4,3,3);
+  plot2d(time, deg_of_rad(eul_ahrs(3,:)), 3);
+  plot2d(time, deg_of_rad(eul_fdm(3,:)), 2);
+  legends(["fdm", "ahrs_c"],[2 3], with_box=%f, opt="ul"); 
+  xtitle('Psi');
+  
+  subplot(4,3,4);
+  plot2d(time, deg_of_rad(ahrs_omega(1,:)), 3);
+  plot2d(time, deg_of_rad(fdm_state(FDM_SP,:)), 2);
+  xtitle('p');
+  
+  subplot(4,3,5);
+  plot2d(time, deg_of_rad(ahrs_omega(2,:)), 3);
+  plot2d(time, deg_of_rad(fdm_state(FDM_SQ,:)), 2);
+  xtitle('q');
+
+  subplot(4,3,6);
+  plot2d(time, deg_of_rad(ahrs_omega(3,:)), 3);
+  plot2d(time, deg_of_rad(fdm_state(FDM_SR,:)), 2);
+  xtitle('r');
+
+  subplot(4,3,7);
+  plot2d(time, deg_of_rad(ahrs_bias(1,:)), 3);
+  xtitle('bp');
+  
+  subplot(4,3,8);
+  plot2d(time, deg_of_rad(ahrs_bias(2,:)), 3);
+  xtitle('bq');
+
+  subplot(4,3,9);
+  plot2d(time, deg_of_rad(ahrs_bias(3,:)), 3);
+  xtitle('br');
+ 
+  subplot(4,3,10);
+
+  subplot(4,3,11);
+  plot2d(time, ahrs_cov(1,:),3);
+  plot2d(time, ahrs_cov(2,:),3);
+  plot2d(time, ahrs_cov(3,:),3);
+  
+  legends(["ahrs_c", "ahrs_s"],[3 4], with_box=%f, opt="ul"); 
+  xtitle('cov angles'); 
+  
+  subplot(4,3,12);
+  plot2d(time, ahrs_cov(4,:),3);
+  plot2d(time, ahrs_cov(5,:),3);
+  plot2d(time, ahrs_cov(6,:),3);
+ 
+  xtitle('cov bias'); 
+  
+endfunction
+
+load('../../../simulator/scilab/q6d/data/stop_stop_state_sensors.dat', 'time', 
'fdm_state', 'sensor_gyro', 'sensor_accel', 'sensor_mag');
+read_ahrs_output('out.txt');
+
+display_ahrs_state();
\ No newline at end of file

Added: paparazzi3/trunk/sw/airborne/stm32/i2c_hw.h
===================================================================
--- paparazzi3/trunk/sw/airborne/stm32/i2c_hw.h                         (rev 0)
+++ paparazzi3/trunk/sw/airborne/stm32/i2c_hw.h 2010-03-15 15:53:31 UTC (rev 
4698)
@@ -0,0 +1,47 @@
+/*
+ * $Id$
+ *  
+ * Copyright (C) 2009 Antoine Drouin <address@hidden>
+ *
+ * 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. 
+ */
+
+/*
+ * Hardware level I2C handling
+ */
+
+#ifndef I2C_HW_H
+#define I2C_HW_H
+
+
+#ifdef USE_I2C1
+
+extern void i2c1_hw_init(void);
+
+#endif /* USE_I2C1 */
+
+
+
+#ifdef USE_I2C2
+
+extern void i2c2_hw_init(void);
+
+#endif /* USE_I2C2 */
+
+
+#endif /* I2C_HW_H */