mirror of
https://github.com/paparazzi/paparazzi.git
synced 2026-06-06 07:53:43 +08:00
adding some test programs for ahrs
This commit is contained in:
Antoine Drouin
@@ -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
|
||||
@@ -0,0 +1,5 @@
|
||||
Q_QI = 1;
|
||||
Q_QX = 2;
|
||||
Q_QY = 3;
|
||||
Q_QZ = 4;
|
||||
Q_SIZE = 4;
|
||||
@@ -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);
|
||||
@@ -0,0 +1,10 @@
|
||||
%% degres of radians
|
||||
%
|
||||
% [deg] = deg_of_rad(rad)
|
||||
%
|
||||
%
|
||||
function [deg] = deg_of_rad(rad)
|
||||
|
||||
deg = rad * 180 / pi;
|
||||
|
||||
endfunction
|
||||
@@ -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
|
||||
@@ -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,'.');
|
||||
@@ -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
|
||||
@@ -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) {}
|
||||
Reference in New Issue
Block a user