[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6351] adding some test programs for ahrs
From: |
antoine drouin |
Subject: |
[paparazzi-commits] [6351] adding some test programs for ahrs |
Date: |
Thu, 04 Nov 2010 16:07:14 +0000 |
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) {}
[Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6351] adding some test programs for ahrs,
antoine drouin <=