[paparazzi-commits] [6351] adding some test programs for ahrs

[antoine drouin]

Revision: 6351
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6351
Author:   poine
Date:     2010-11-04 16:07:14 +0000 (Thu, 04 Nov 2010)
Log Message:
-----------
adding some test programs for ahrs

Added Paths:
-----------
    paparazzi3/trunk/sw/airborne/test/ahrs/
    paparazzi3/trunk/sw/airborne/test/ahrs/Makefile
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/algebra_common.m
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/deg_of_rad.m
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/eulers_of_quat.m
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/plot_ahrs_on_flight_log.m
    paparazzi3/trunk/sw/airborne/test/ahrs/plot/unwrap.m
    paparazzi3/trunk/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c

Added: paparazzi3/trunk/sw/airborne/test/ahrs/Makefile
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/Makefile                             
(rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/Makefile     2010-11-04 16:07:14 UTC 
(rev 6351)
@@ -0,0 +1,38 @@
+
+
+CC = gcc
+CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz 
 -Wall
+LDFLAGS = -lm
+
+# imu wants airframe to fetch its neutrals
+# ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES
+CFLAGS += -I../../../../var/BOOZ2_A7
+
+
+#CFLAGS += -DIMU_BODY_TO_IMU_PHI=0 -DIMU_BODY_TO_IMU_THETA=0 
-DIMU_BODY_TO_IMU_PSI=0
+# toulouse 0.51562740288882, -0.05707735220832, 0.85490967783446
+CFLAGS += -DAHRS_H_X=0.51562740288882 -DAHRS_H_Y=-0.05707735220832 
-DAHRS_H_Z=0.85490967783446
+
+#CFLAGS += -DOUTPUT_IN_BODY_FRAME
+CFLAGS += -DENABLE_MAG_UPDATE
+CFLAGS += -DENABLE_ACCEL_UPDATE
+
+SRCS= run_ahrs_on_flight_log.c                    \
+      ../../math/pprz_trig_int.c                  \
+      ../../subsystems/ahrs.c                     \
+      ../../subsystems/ahrs/ahrs_aligner.c        \
+      ../../subsystems/imu.c
+
+all: run_ahrs_flq_on_flight_log run_ahrs_fcr_on_flight_log 
run_ahrs_ice_on_flight_log
+
+run_ahrs_flq_on_flight_log: ../../subsystems/ahrs/ahrs_float_lkf_quat.c $(SRCS)
+       $(CC) -DAHRS_TYPE=AHRS_TYPE_FLQ $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+run_ahrs_fcr_on_flight_log: ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c 
$(SRCS)
+       $(CC) -DAHRS_TYPE=AHRS_TYPE_FCR $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+run_ahrs_ice_on_flight_log: ../../subsystems/ahrs/ahrs_int_cmpl_euler.c $(SRCS)
+       $(CC) -DAHRS_TYPE=AHRS_TYPE_ICE $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean:
+       rm -f *~ run_ahrs_*_on_flight_log

Added: paparazzi3/trunk/sw/airborne/test/ahrs/plot/algebra_common.m
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/plot/algebra_common.m                
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/plot/algebra_common.m        
2010-11-04 16:07:14 UTC (rev 6351)
@@ -0,0 +1,5 @@
+Q_QI = 1;
+Q_QX = 2;
+Q_QY = 3;
+Q_QZ = 4;
+Q_SIZE = 4;

Added: paparazzi3/trunk/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m    
                        (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/plot/compare_ahrs_on_flight_log.m    
2010-11-04 16:07:14 UTC (rev 6351)
@@ -0,0 +1,122 @@
+clear();
+
+flt1 = 'flq';
+%flt2 = 'ice';
+flt2 = 'fcr';
+
+filename1 = ["out_" flt1 ".txt"];
+filename2 = ["out_" flt2 ".txt"];
+
+trim_log=true;%false;
+time_start=7;
+time_stop=25;
+
+plot_true_state = true;
+true_state_filename="/home/poine/work/savannah/paparazzi3.broken_stm32_deletion/trunk/sw/simulator/scilab/q6d/data/stop_stop_fdm.txt"
+
+true_dat = load(true_state_filename);
+dat1 = load(filename1);
+dat2 = load(filename2);
+% extract the part of interrest
+if (trim_log)
+  idx_roi=find(dat1(:,1)>time_start & dat1(:,1)<time_stop);
+  dat1 = dat1(idx_roi,:);
+  idx_roi=find(dat2(:,1)>time_start & dat2(:,1)<time_stop);
+  dat2 = dat2(idx_roi,:);
+   if plot_true_state
+     idx_roi=find(true_dat(:,1)>time_start & true_dat(:,1)<time_stop);
+     true_dat = true_dat(idx_roi,:);
+   end
+  end
+
+% name fields
+time1=dat1(:,1);
+quat1=dat1(:,2:5);
+%rate1=dat1(:,6:8);
+bias1=dat1(:,9:11);
+%P1=dat1(:,12:17);
+
+time2=dat2(:,1);
+quat2=dat2(:,2:5);
+%rate2=dat2(:,6:8);
+bias2=dat2(:,9:11);
+%P2=dat2(:,12:17);
+
+% computes euler angles
+eulers1 = eulers_of_quat(quat1);
+eulers2 = eulers_of_quat(quat2);
+if plot_true_state
+  eulers_true = eulers_of_quat(true_dat(:,2:5));
+  eulers_true(:,3) = unwrap(eulers_true(:,3));
+end
+
+eulers1(:,3) = unwrap(eulers1(:,3));
+eulers2(:,3) = unwrap(eulers2(:,3));
+
+figure(1);
+clf();
+subplot(3,1,1);
+hold on
+plot(time1, deg_of_rad(eulers1(:,1)), 'r');
+plot(time2, deg_of_rad(eulers2(:,1)), 'b');
+if plot_true_state
+  plot(true_dat(:,1), deg_of_rad(eulers_true(:,1)), 'g');
+  legend(flt1, flt2, 'true');
+else
+legend(flt1, flt2);    
+end
+plot(time2, zeros(length(time2), 1), 'k');
+title('phi');
+
+
+
+subplot(3,1,2);
+hold on
+plot(time1, deg_of_rad(eulers1(:,2)), 'r');
+plot(time2, deg_of_rad(eulers2(:,2)), 'b');
+if plot_true_state
+  plot(true_dat(:,1), deg_of_rad(eulers_true(:,2)), 'g');
+  legend(flt1, flt2, 'true');
+else
+legend(flt1, flt2);    
+end
+plot(time2, zeros(length(time2), 1), 'k');
+title('theta');
+
+subplot(3,1,3);
+hold on
+plot(time1, deg_of_rad(eulers1(:,3)), 'r');
+plot(time2, deg_of_rad(eulers2(:,3)), 'b');
+if plot_true_state
+  plot(true_dat(:,1), deg_of_rad(eulers_true(:,3)), 'g');
+  legend(flt1, flt2, 'true');
+else
+legend(flt1, flt2);    
+end
+title('psi');
+
+
+figure(2);
+clf();
+subplot(3,1,1);
+hold on
+plot(time1, deg_of_rad(bias1(:,1)), 'r');
+plot(time2, deg_of_rad(bias2(:,1)), 'b');
+plot(time2, zeros(length(time2), 1), 'k');
+title('bp');
+legend(flt1, flt2);
+
+subplot(3,1,2);
+hold on
+plot(time1, deg_of_rad(bias1(:,2)), 'r');
+plot(time2, deg_of_rad(bias2(:,2)), 'b');
+plot(time2, zeros(length(time2), 1), 'k');
+title('bq');
+legend(flt1, flt2);
+
+subplot(3,1,3);
+hold on
+plot(time1, deg_of_rad(bias1(:,3)), 'r');
+plot(time2, deg_of_rad(bias2(:,3)), 'b');
+title('br');
+legend(flt1, flt2);
\ No newline at end of file

Added: paparazzi3/trunk/sw/airborne/test/ahrs/plot/deg_of_rad.m
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/plot/deg_of_rad.m                    
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/plot/deg_of_rad.m    2010-11-04 
16:07:14 UTC (rev 6351)
@@ -0,0 +1,10 @@
+%% degres of radians
+%
+% [deg] = deg_of_rad(rad)
+%
+% 
+function [deg] = deg_of_rad(rad)
+       
+       deg = rad * 180 / pi;
+
+endfunction
\ No newline at end of file

Added: paparazzi3/trunk/sw/airborne/test/ahrs/plot/eulers_of_quat.m
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/plot/eulers_of_quat.m                
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/plot/eulers_of_quat.m        
2010-11-04 16:07:14 UTC (rev 6351)
@@ -0,0 +1,29 @@
+%% EULERS OF QUATERNION
+%
+% [euler] = eulers_of_quat(quat)
+%
+% transposes a quaternion to euler angles
+function [euler] = eulers_of_quat(quat)
+  
+   algebra_common;
+
+  if size(quat)(2)==4
+     quat = quat';
+     transpose = 1;
+  end
+
+  dcm00 = 1.0 - 2*(quat(Q_QY,:).*quat(Q_QY,:) + quat(Q_QZ,:).*quat(Q_QZ,:));
+   dcm01 =       2*(quat(Q_QX,:).*quat(Q_QY,:) + quat(Q_QI,:).*quat(Q_QZ,:));
+   dcm02 =       2*(quat(Q_QX,:).*quat(Q_QZ,:) - quat(Q_QI,:).*quat(Q_QY,:));
+  dcm12 =       2*(quat(Q_QY,:).*quat(Q_QZ,:) + quat(Q_QI,:).*quat(Q_QX,:));
+  dcm22 = 1.0 - 2*(quat(Q_QX,:).*quat(Q_QX,:) + quat(Q_QY,:).*quat(Q_QY,:));
+
+  phi = atan2( dcm12, dcm22 );
+  theta = -asin( dcm02 );
+  psi = atan2( dcm01, dcm00 );
+
+  euler = [phi; theta; psi];
+ if transpose
+  euler = euler';
+ end
+endfunction

Added: paparazzi3/trunk/sw/airborne/test/ahrs/plot/plot_ahrs_on_flight_log.m
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/plot/plot_ahrs_on_flight_log.m       
                        (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/plot/plot_ahrs_on_flight_log.m       
2010-11-04 16:07:14 UTC (rev 6351)
@@ -0,0 +1,42 @@
+clear();
+
+algebra_common;
+
+filename = "out.txt";
+trim_log=false;
+time_start=28;
+time_stop=80;
+
+dat = load(filename);
+
+% extract the part of interrest
+if (trim_log)
+  idx_roi=find(dat(:,1)>time_start & dat(:,1)<time_stop);
+  dat = dat(idx_roi,:);
+end
+
+% name fields
+time=dat(:,1);
+quat=dat(:,2:5);
+rate=dat(:,6:8);
+bias=dat(:,9:11);
+P=dat(:,12:17);
+
+
+% computes euler angles
+eulers = eulers_of_quat(quat);
+
+
+
+figure(1);
+clf();
+plot(time, rate);
+figure(2);
+clf();
+plot(time, bias);
+figure(3);
+clf();
+plot(time, eulers,'.');
+figure(4);
+clf();
+plot(time, P,'.');
\ No newline at end of file

Added: paparazzi3/trunk/sw/airborne/test/ahrs/plot/unwrap.m
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/plot/unwrap.m                        
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/plot/unwrap.m        2010-11-04 
16:07:14 UTC (rev 6351)
@@ -0,0 +1,19 @@
+%% unwrap
+%
+% [unwraped] = unwrap(wraped)
+%
+% 
+function [unwraped] = unwrap(wraped)
+
+   unwraped = zeros(length(wraped), 1);
+   cnt = 0;
+   for i=2:length(wraped)
+       dif = wraped(i) - wraped(i-1);
+       if (dif > pi/2)
+               cnt=cnt-1;
+       elseif (dif <-pi/2)
+               cnt=cnt+1;
+       end
+       unwraped(i) = wraped(i)+2*pi*cnt;
+    end
+endfunction
\ No newline at end of file

Added: paparazzi3/trunk/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c
===================================================================
--- paparazzi3/trunk/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c             
                (rev 0)
+++ paparazzi3/trunk/sw/airborne/test/ahrs/run_ahrs_on_flight_log.c     
2010-11-04 16:07:14 UTC (rev 6351)
@@ -0,0 +1,214 @@
+
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+#include <string.h>
+
+#include "std.h"
+
+#include "math/pprz_algebra_float.h"
+#include "math/pprz_algebra_double.h"
+#include "math/pprz_algebra_int.h"
+#include "pprz_algebra_print.h"
+
+#include "subsystems/ahrs.h"
+#include "subsystems/ahrs/ahrs_aligner.h"
+#include "subsystems/imu.h"
+
+#define AHRS_TYPE_FLG 0
+#define AHRS_TYPE_FCR 1
+#define AHRS_TYPE_ICE 2
+
+#if   defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FLQ
+#include "subsystems/ahrs/ahrs_float_lkf_quat.h"
+#define OUT_FILE "./out_flq.txt"
+#elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FCR
+#include "subsystems/ahrs/ahrs_float_cmpl_rmat.h"
+#define OUT_FILE "./out_fcr.txt"
+#elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_ICE
+#include "subsystems/ahrs/ahrs_int_cmpl_euler.h"
+#define OUT_FILE "./out_ice.txt"
+#endif
+
+
+#define MAX_SAMPLE 1000000
+//#define MAX_SAMPLE 5000
+struct test_sample {
+  double time;
+  uint8_t flag;
+  struct DoubleQuat  quat_true;
+  struct DoubleRates omega_true;
+  struct DoubleRates gyro;
+  struct DoubleVect3 accel;
+  struct DoubleVect3 mag;
+  struct DoubleVect3 gps_pecef;
+  struct DoubleVect3 gps_vecef;
+  double baro;
+};
+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];
+
+
+
+//#define IN_FILE  "../../fms/libeknav/flight9_spin_circle.ascii"
+//#define IN_FILE  "../../fms/libeknav/flight9_2climbs.ascii"
+#define IN_FILE  
"/home/poine/work/savannah/paparazzi3.broken_stm32_deletion/trunk/sw/simulator/scilab/q6d/data/stop_stop_sensors.txt"
+
+
+static void read_ascii_flight_log(const char* filename);
+static void feed_imu(int i);
+static void store_filter_output(int i);
+static void dump_output(const char* filename);
+
+/* from fms_autopilot_msg.h */
+#define VI_IMU_DATA_VALID      0
+#define VI_MAG_DATA_VALID      1
+#define IMU_AVAILABLE(_flag) (_flag & (1<<VI_IMU_DATA_VALID))
+#define MAG_AVAILABLE(_flag) (_flag & (1<<VI_MAG_DATA_VALID))
+
+int main(int argc, char** argv) {
+
+  read_ascii_flight_log(IN_FILE);
+
+  imu_init();
+  ahrs_init();
+
+  for (int i=0; i<nb_samples; i++) {
+    feed_imu(i);
+    if (ahrs.status == AHRS_UNINIT) {
+      ahrs_aligner_run();
+      if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
+       ahrs_align();
+    }
+    else {
+      ahrs_propagate();
+#ifdef ENABLE_MAG_UPDATE
+      if (MAG_AVAILABLE(samples[i].flag))
+       ahrs_update_mag();
+#endif
+#ifdef ENABLE_ACCEL_UPDATE
+      if (IMU_AVAILABLE(samples[i].flag) && (!MAG_AVAILABLE(samples[i].flag)))
+       ahrs_update_accel();
+#endif
+    }
+    store_filter_output(i);
+  }
+
+  dump_output(OUT_FILE);
+
+}
+
+/*
+ * Read an ascii flight log
+ */
+static void read_ascii_flight_log(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\t%hhu\t%lf %lf %lf\t%lf %lf %lf\t%lf %lf %lf\t%lf 
%lf %lf\t%lf %lf %lf\t%lf",
+                &s->time, &s->flag,
+                &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,
+                &s->gps_pecef.x, &s->gps_pecef.y, &s->gps_pecef.z,
+                &s->gps_vecef.x, &s->gps_vecef.y, &s->gps_vecef.z,
+                &s->baro);
+    nb_samples++;
+  }
+  while (ret == 18  && 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(imu.gyro_prev, imu.gyro);
+  }
+  else {
+    RATES_BFP_OF_REAL(imu.gyro_prev, samples[0].gyro);
+  }
+  RATES_BFP_OF_REAL(imu.gyro, samples[i].gyro);
+  ACCELS_BFP_OF_REAL(imu.accel, samples[i].accel);
+  MAGS_BFP_OF_REAL(imu.mag, samples[i].mag);
+}
+
+/*
+ *
+ */
+#if   defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FLQ  
+static void store_filter_output(int i) {
+#ifdef OUTPUT_IN_BODY_FRAME
+  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_body_quat);
+  RATES_COPY(output[i].rate_est, ahrs_float.body_rate);
+#else
+  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
+  RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
+#endif /* OUTPUT_IN_BODY_FRAME */
+  RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
+  memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
+}
+#elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_FCR
+static void store_filter_output(int i) {
+#ifdef OUTPUT_IN_BODY_FRAME
+  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_body_quat);
+  RATES_COPY(output[i].rate_est, ahrs_float.body_rate);
+#else
+  QUAT_COPY(output[i].quat_est, ahrs_float.ltp_to_imu_quat);
+  RATES_COPY(output[i].rate_est, ahrs_float.imu_rate);
+#endif /* OUTPUT_IN_BODY_FRAME */
+  RATES_COPY(output[i].bias_est, ahrs_impl.gyro_bias);
+  //  memcpy(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
+}
+#elif defined AHRS_TYPE && AHRS_TYPE == AHRS_TYPE_ICE
+static void store_filter_output(int i) {
+#ifdef OUTPUT_IN_BODY_FRAME
+  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_body_quat);
+  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.body_rate);
+#else
+  QUAT_FLOAT_OF_BFP(output[i].quat_est, ahrs.ltp_to_imu_quat);
+  RATES_FLOAT_OF_BFP(output[i].rate_est, ahrs.imu_rate);
+#endif /* OUTPUT_IN_BODY_FRAME */
+  RATES_ASSIGN(output[i].bias_est, 0., 0., 0.);
+  //  memset(output[i].P, ahrs_impl.P, sizeof(ahrs_impl.P));
+}
+#endif
+
+/*
+ *
+ */
+static void dump_output(const char* filename) {
+  FILE* fd = fopen(filename, "w");
+  int i;
+  for (i=0; i<nb_samples; i++) {
+    fprintf(fd, "%.16f\t%.16f %.16f %.16f %.16f\t%.16f %.16f %.16f\t%.16f 
%.16f %.16f\t%.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);
+}
+
+
+/* imu.h wants that */
+void imu_impl_init(void) {}