Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-26 08:22:43 +08:00
Code Issues Actions 29 Packages Projects Releases Wiki Activity

adding a new AHRS and ahrs test programs

Browse Source
This commit is contained in:
Antoine Drouin
2011-03-22 04:27:26 +01:00
parent 88c54f6e8b
commit d733952721
22 changed files with 2020 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/math/pprz_algebra.h
+21
View File
@@ -26,6 +26,7 @@
#define PPRZ_ALGEBRA_H
#include <float.h> /* for FLT_EPSILON */
#include <string.h> /* for memcpy */
#define SQUARE(_a) ((_a)*(_a))
@@ -128,6 +129,13 @@
(_c).z = (_a).z + (_b).z; \
}
/* a += b*s */
#define VECT3_ADD_SCALED(_a, _b, _s) { \
(_a).x += ((_b).x * (_s)); \
(_a).y += ((_b).y * (_s)); \
(_a).z += ((_b).z * (_s)); \
}
/* c = a + _s * b */
#define VECT3_SUM_SCALED(_c, _a, _b, _s) { \
(_c).x = (_a).x + (_s)*(_b).x; \
@@ -500,6 +508,19 @@
RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
}
#define RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) { \
(_vout).x = RMAT_ELMT((_rmat), 0, 0) * (_vin).x + \
RMAT_ELMT((_rmat), 1, 0) * (_vin).y + \
RMAT_ELMT((_rmat), 2, 0) * (_vin).z; \
(_vout).y = RMAT_ELMT((_rmat), 0, 1) * (_vin).x + \
RMAT_ELMT((_rmat), 1, 1) * (_vin).y + \
RMAT_ELMT((_rmat), 2, 1) * (_vin).z; \
(_vout).z = RMAT_ELMT((_rmat), 0, 2) * (_vin).x + \
RMAT_ELMT((_rmat), 1, 2) * (_vin).y + \
RMAT_ELMT((_rmat), 2, 2) * (_vin).z; \
}
#define RMAT_COPY(_o, _i) { memcpy(&(_o), &(_i), sizeof(_o));}
sw/airborne/math/pprz_algebra_float.h
+71 -1
View File
@@ -153,6 +153,14 @@ struct FloatRates {
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
}
#define FLOAT_VECT3_INTEGRATE_FI(_vo, _dv, _dt) { \
(_vo).x += (_dv).x * (_dt); \
(_vo).y += (_dv).y * (_dt); \
(_vo).z += (_dv).z * (_dt); \
}
#define FLOAT_VECT3_NORMALIZE(_v) { \
const float n = FLOAT_VECT3_NORM(_v); \
FLOAT_VECT3_SMUL(_v, _v, 1./n); \
@@ -170,6 +178,34 @@ struct FloatRates {
_ro.r += _v.z * _s; \
}
#define FLOAT_RATES_LIN_CMB(_ro, _r1, _s1, _r2, _s2) { \
_ro.p = _s1 * _r1.p + _s2 * _r2.p; \
_ro.q = _s1 * _r1.q + _s2 * _r2.q; \
_ro.r = _s1 * _r1.r + _s2 * _r2.r; \
}
#define FLOAT_RATES_SCALE(_ro,_s) { \
_ro.p *= _s; \
_ro.q *= _s; \
_ro.r *= _s; \
}
#define FLOAT_RATES_INTEGRATE_FI(_ra, _racc, _dt) { \
(_ra).p += (_racc).p * (_dt); \
(_ra).q += (_racc).q * (_dt); \
(_ra).r += (_racc).r * (_dt); \
}
#define FLOAT_RATES_OF_EULER_DOT(_ra, _e, _ed) { \
_ra.p = _ed.phi - sinf(_e.theta) *_ed.psi; \
_ra.q = cosf(_e.phi)*_ed.theta + sinf(_e.phi)*cosf(_e.theta)*_ed.psi; \
_ra.r = -sinf(_e.phi)*_ed.theta + cosf(_e.phi)*cosf(_e.theta)*_ed.psi; \
}
/*
* 3x3 matrices
*/
@@ -232,6 +268,7 @@ struct FloatRates {
/* multiply _vin by _rmat, store in _vout */
#define FLOAT_RMAT_VECT3_MUL(_vout, _rmat, _vin) RMAT_VECT3_MUL(_vout, _rmat, _vin)
#define FLOAT_RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin) RMAT_VECT3_TRANSP_MUL(_vout, _rmat, _vin)
#define FLOAT_RMAT_TRANSP_RATEMULT(_vb, _m_b2a, _va) { \
(_vb).p = ( (_m_b2a).m[0]*(_va).p + (_m_b2a).m[3]*(_va).q + (_m_b2a).m[6]*(_va).r); \
@@ -395,6 +432,18 @@ struct FloatRates {
}
#endif
/* in place first order integration of a rotation matrix */
#define FLOAT_RMAT_INTEGRATE_FI(_rm, _omega, _dt ) { \
struct FloatRMat exp_omega_dt = { \
{ 1. , dt*omega.r, -dt*omega.q, \
-dt*omega.r, 1. , dt*omega.p, \
dt*omega.q, -dt*omega.p, 1. }}; \
struct FloatRMat R_tdt; \
FLOAT_RMAT_COMP(R_tdt, _rm, exp_omega_dt); \
memcpy(&(_rm), &R_tdt, sizeof(R_tdt)); \
}
static inline float renorm_factor(float n) {
if (n < 1.5625f && n > 0.64f)
return .5 * (3-n);
@@ -509,7 +558,7 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
(_b2c).qz = (_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi; \
}
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
#define FLOAT_QUAT_DIFFERENTIAL(q_out, w, dt) { \
const float v_norm = sqrt((w).p*(w).p + (w).q*(w).q + (w).r*(w).r); \
const float c2 = cos(dt*v_norm/2.0); \
const float s2 = sin(dt*v_norm/2.0); \
@@ -526,6 +575,27 @@ static inline float float_rmat_reorthogonalize(struct FloatRMat* rm) {
} \
}
/* in place quaternion integration with constante rotational velocity */
#define FLOAT_QUAT_INTEGRATE(_q, _omega, _dt) { \
const float no = FLOAT_RATES_NORM(_omega); \
if (no > FLT_MIN) { \
const float a = 0.5*no*_dt; \
const float ca = cosf(a); \
const float sa_ov_no = sinf(a)/no; \
const float dp = sa_ov_no*_omega.p; \
const float dq = sa_ov_no*_omega.q; \
const float dr = sa_ov_no*_omega.r; \
const float qi = _q.qi; \
const float qx = _q.qx; \
const float qy = _q.qy; \
const float qz = _q.qz; \
_q.qi = ca*qi - dp*qx - dq*qy - dr*qz; \
_q.qx = dp*qi + ca*qx + dr*qy - dq*qz; \
_q.qy = dq*qi - dr*qx + ca*qy + dp*qz; \
_q.qz = dr*qi + dq*qx - dp*qy + ca*qz; \
} \
}
#ifdef ALGEBRA_FLOAT_USE_SLOW_FUNCTIONS
#define FLOAT_QUAT_VMULT(v_out, q, v_in) { \
const float qi2 = q.qi*q.qi; \
sw/airborne/math/pprz_algebra_int.h
+59 -10
View File
@@ -75,7 +75,7 @@ struct Int32Vect3 {
/* Rotation quaternions */
#define INT32_QUAT_FRAC 15
/**
* @brief Roation quaternion
* @brief Rotation quaternion
* @details Units: INT32_QUAT_FRAC */
struct Int32Quat {
int32_t qi;
@@ -84,6 +84,15 @@ struct Int32Quat {
int32_t qz;
};
struct Int64Quat {
int32_t qi;
int32_t qx;
int32_t qy;
int32_t qz;
};
/* Euler angles */
#define INT32_ANGLE_FRAC 12
#define INT32_RATE_FRAC 12
@@ -154,6 +163,13 @@ struct Int32Rates {
int32_t r; ///< in rad/s^2 with INT32_RATE_FRAC
};
struct Int64Rates {
int64_t p; ///< in rad/s^2 with INT32_RATE_FRAC
int64_t q; ///< in rad/s^2 with INT32_RATE_FRAC
int64_t r; ///< in rad/s^2 with INT32_RATE_FRAC
};
struct Int64Vect2 {
int64_t x;
int64_t y;
@@ -253,6 +269,11 @@ struct Int64Vect3 {
(_o).z = ((_i).z << (_l)); \
}
#define INT32_VECT3_CROSS_PRODUCT(_vo, _v1, _v2) { \
(_vo).x = (_v1).y*(_v2).z - (_v1).z*(_v2).y; \
(_vo).y = (_v1).z*(_v2).x - (_v1).x*(_v2).z; \
(_vo).z = (_v1).x*(_v2).y - (_v1).y*(_v2).x; \
}
@@ -272,15 +293,15 @@ struct Int64Vect3 {
}
#define INT32_MAT33_DIAG(_m, _d00, _d11, _d22) { \
MAT33_ELMT((_m), 0, 0) = (_d00); \
MAT33_ELMT((_m), 0, 0) = (_d00); \
MAT33_ELMT((_m), 0, 1) = 0; \
MAT33_ELMT((_m), 0, 2) = 0; \
MAT33_ELMT((_m), 1, 0) = 0; \
MAT33_ELMT((_m), 1, 1) = (_d11); \
MAT33_ELMT((_m), 1, 1) = (_d11); \
MAT33_ELMT((_m), 1, 2) = 0; \
MAT33_ELMT((_m), 2, 0) = 0; \
MAT33_ELMT((_m), 2, 1) = 0; \
MAT33_ELMT((_m), 2, 2) = (_d22); \
MAT33_ELMT((_m), 2, 2) = (_d22); \
}
@@ -507,9 +528,9 @@ struct Int64Vect3 {
#define INT32_QUAT_ZERO(_q) { \
(_q).qi = QUAT1_BFP_OF_REAL(1); \
(_q).qx = 0; \
(_q).qy = 0; \
(_q).qz = 0; \
(_q).qx = 0; \
(_q).qy = 0; \
(_q).qz = 0; \
}
#define INT32_QUAT_INVERT(_qo, _qi) QUAT_INVERT(_qo, _qi)
@@ -520,11 +541,11 @@ struct Int64Vect3 {
}
#define INT32_QUAT_WRAP_SHORTEST(q) { \
if ((q).qi < 0) \
if ((q).qi < 0) \
QUAT_EXPLEMENTARY(q,q); \
}
#define INT32_QUAT_NORMALISE(q) { \
#define INT32_QUAT_NORMALIZE(q) { \
int32_t n; \
INT32_QUAT_NORM(n, q); \
(q).qi = (q).qi * QUAT1_BFP_OF_REAL(1) / n; \
@@ -557,6 +578,8 @@ struct Int64Vect3 {
(_b2c).qz = ((_a2b).qi*(_a2c).qz - (_a2b).qx*(_a2c).qy + (_a2b).qy*(_a2c).qx - (_a2b).qz*(_a2c).qi)>>INT32_QUAT_FRAC; \
}
#ifdef ALGEBRA_INT_USE_SLOW_FUNCTIONS
#define INT32_QUAT_VMULT(v_out, q, v_in) { \
const int32_t qi2 = ((q).qi*(q).qi)>>INT32_QUAT_FRAC; \
@@ -793,7 +816,33 @@ struct Int64Vect3 {
#define INT_RATES_ZERO(_e) RATES_ASSIGN(_e, 0, 0, 0)
#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \
#define INT_RATES_ADD_SCALED_VECT(_ro, _v, _s) { \
_ro.p += _v.x * _s; \
_ro.q += _v.y * _s; \
_ro.r += _v.z * _s; \
}
#define INT_RATES_SDIV(_ro, _s, _ri) { \
_ro.p = _ri.p / _s; \
_ro.q = _ri.q / _s; \
_ro.r = _ri.r / _s; \
}
#define INT_RATES_RSHIFT(_o, _i, _r) { \
(_o).p = ((_i).p >> (_r)); \
(_o).q = ((_i).q >> (_r)); \
(_o).r = ((_i).r >> (_r)); \
}
#define INT_RATES_LSHIFT(_o, _i, _r) { \
(_o).p = ((_i).p << (_r)); \
(_o).q = ((_i).q << (_r)); \
(_o).r = ((_i).r << (_r)); \
}
#define INT32_RATES_OF_EULERS_DOT_321(_r, _e, _ed) { \
\
int32_t sphi; \
PPRZ_ITRIG_SIN(sphi, (_e).phi); \
sw/airborne/subsystems/ahrs/ahrs_aligner.c
+2
View File
@@ -51,7 +51,9 @@ void ahrs_aligner_init(void) {
#ifndef LOW_NOISE_THRESHOLD
#define LOW_NOISE_THRESHOLD 90000
#endif
#ifndef LOW_NOISE_TIME
#define LOW_NOISE_TIME 5
#endif
void ahrs_aligner_run(void) {
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.c
+121 -51
View File
@@ -31,12 +31,15 @@
#include "math/pprz_simple_matrix.h"
#include "generated/airframe.h"
#include <string.h>
#include "../../test/pprz_algebra_print.h"
void ahrs_update_mag_full(void);
void ahrs_update_mag_2d(void);
void ahrs_update_mag_2d_dumb(void);
static inline void compute_imu_quat_and_euler_from_rmat(void);
static inline void compute_imu_rmat_and_euler_from_quat(void);
static inline void compute_body_orientation_and_rates(void);
@@ -45,25 +48,23 @@ struct AhrsFloatCmplRmat ahrs_impl;
void ahrs_init(void) {
ahrs_float.status = AHRS_UNINIT;
/*
* Initialises our IMU alignement variables
* This should probably done in the IMU code instead
*/
/* Initialises IMU alignement */
struct FloatEulers body_to_imu_euler =
{IMU_BODY_TO_IMU_PHI, IMU_BODY_TO_IMU_THETA, IMU_BODY_TO_IMU_PSI};
FLOAT_QUAT_OF_EULERS(ahrs_impl.body_to_imu_quat, body_to_imu_euler);
FLOAT_RMAT_OF_EULERS(ahrs_impl.body_to_imu_rmat, body_to_imu_euler);
/* set ltp_to_body to zero */
/* Set ltp_to_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_body_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_body_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_body_rmat);
FLOAT_RATES_ZERO(ahrs_float.body_rate);
/* set ltp_to_imu so that body is zero */
/* Set ltp_to_imu so that body is zero */
QUAT_COPY(ahrs_float.ltp_to_imu_quat, ahrs_impl.body_to_imu_quat);
RMAT_COPY(ahrs_float.ltp_to_imu_rmat, ahrs_impl.body_to_imu_rmat);
EULERS_COPY(ahrs_float.ltp_to_imu_euler, body_to_imu_euler);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
}
@@ -95,30 +96,32 @@ void ahrs_propagate(void) {
struct FloatRates gyro_float;
RATES_FLOAT_OF_BFP(gyro_float, imu.gyro_prev);
/* unbias measurement */
RATES_DIFF(ahrs_float.imu_rate, gyro_float, ahrs_impl.gyro_bias);
const float dt = 1./512.;
RATES_SUB(gyro_float, ahrs_impl.gyro_bias);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
const float alpha = 0.1;
FLOAT_RATES_LIN_CMB(ahrs_float.imu_rate, ahrs_float.imu_rate, (1.-alpha), gyro_float, alpha);
#else
RATES_COPY(ahrs_float.imu_rate,gyro_float);
#endif
/* add correction */
struct FloatRates omega;
RATES_SUM(omega, ahrs_float.imu_rate, ahrs_impl.rate_correction);
// DISPLAY_FLOAT_RATES("omega ", omega);
RATES_SUM(omega, gyro_float, ahrs_impl.rate_correction);
/* and zeros it */
FLOAT_RATES_ZERO(ahrs_impl.rate_correction);
/* first order integration of rotation matrix */
struct FloatRMat exp_omega_dt = {
{ 1. , dt*omega.r, -dt*omega.q,
-dt*omega.r, 1. , dt*omega.p,
dt*omega.q, -dt*omega.p, 1. }};
struct FloatRMat R_tdt;
FLOAT_RMAT_COMP(R_tdt, ahrs_float.ltp_to_imu_rmat, exp_omega_dt);
memcpy(&ahrs_float.ltp_to_imu_rmat, &R_tdt, sizeof(R_tdt));
const float dt = 1./AHRS_PROPAGATE_FREQUENCY;
#ifdef AHRS_PROPAGATE_RMAT
FLOAT_RMAT_INTEGRATE_FI(ahrs_float.ltp_to_imu_rmat, omega, dt );
float_rmat_reorthogonalize(&ahrs_float.ltp_to_imu_rmat);
// struct FloatRMat test;
// FLOAT_RMAT_COMP_INV(test, ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_rmat);
// DISPLAY_FLOAT_RMAT("foo", test);
compute_imu_quat_and_euler_from_rmat();
#endif
#ifdef AHRS_PROPAGATE_QUAT
FLOAT_QUAT_INTEGRATE(ahrs_float.ltp_to_imu_quat, omega, dt);
FLOAT_QUAT_NORMALISE(ahrs_float.ltp_to_imu_quat);
compute_imu_rmat_and_euler_from_quat();
#endif
compute_body_orientation_and_rates();
}
@@ -127,23 +130,102 @@ void ahrs_update_accel(void) {
struct FloatVect3 accel_float;
ACCELS_FLOAT_OF_BFP(accel_float, imu.accel);
struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
float v = FLOAT_VECT3_NORM(ahrs_impl.est_ltp_speed);
accel_float.y -= v * ahrs_float.imu_rate.r;
accel_float.z -= -v * ahrs_float.imu_rate.q;
#endif
struct FloatVect3 c2 = { RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,2)};
struct FloatVect3 residual;
FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, (*r2));
FLOAT_VECT3_CROSS_PRODUCT(residual, accel_float, c2);
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
/* heuristic on acceleration norm */
const float acc_norm = FLOAT_VECT3_NORM(accel_float);
const float weight = Chop(1.-2*fabs(1-acc_norm/9.81), 0., 1.);
//const float weight = 1.;
const float weight = Chop(1.-6*fabs((9.81-acc_norm)/9.81), 0., 1.);
#else
const float weight = 1.;
#endif
/* compute correction */
const float gravity_rate_update_gain = 5e-2;
const float gravity_rate_update_gain = -5e-2; // -5e-2
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, weight*gravity_rate_update_gain);
const float gravity_bias_update_gain = -5e-6;
#if 1
const float gravity_bias_update_gain = 1e-5; // -5e-6
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, weight*gravity_bias_update_gain);
#else
const float alpha = 5e-4;
FLOAT_RATES_SCALE(ahrs_impl.gyro_bias, 1.-alpha);
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual, alpha);
#endif
/* FIXME: saturate bias */
}
void ahrs_update_mag(void) {
#ifdef AHRS_MAG_UPDATE_YAW_ONLY
ahrs_update_mag_2d();
// ahrs_update_mag_2d_dumb();
#else
ahrs_update_mag_full();
#endif
}
void ahrs_update_mag_full(void) {
const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual_imu;
FLOAT_VECT3_CROSS_PRODUCT(residual_imu, measured_imu, expected_imu);
// DISPLAY_FLOAT_VECT3("# expected", expected_imu);
// DISPLAY_FLOAT_VECT3("# measured", measured_imu);
// DISPLAY_FLOAT_VECT3("# residual", residual);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
const float mag_bias_update_gain = -2.5e-3;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_mag_2d(void) {
const struct FloatVect2 expected_ltp = {AHRS_H_X, AHRS_H_Y};
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 measured_ltp;
FLOAT_RMAT_VECT3_TRANSP_MUL(measured_ltp, ahrs_float.ltp_to_imu_rmat, measured_imu);
const struct FloatVect3 residual_ltp =
{ 0,
0,
measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x };
// printf("res : %f\n", residual_ltp.z);
struct FloatVect3 residual_imu;
FLOAT_RMAT_VECT3_MUL(residual_imu, ahrs_float.ltp_to_imu_rmat, residual_ltp);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual_imu, mag_rate_update_gain);
const float mag_bias_update_gain = -2.5e-3;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, residual_imu, mag_bias_update_gain);
}
void ahrs_update_mag_full_2d_dumb(void) {
/* project mag on local tangeant plane */
struct FloatVect3 magf;
@@ -156,39 +238,19 @@ void ahrs_update_mag(void) {
const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
const float res_norm = -RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 0,0)*me + RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 1,0)*mn;
printf("res norm %f\n", res_norm);
struct FloatVect3* r2 = (struct FloatVect3*)(&RMAT_ELMT(ahrs_float.ltp_to_imu_rmat, 2,0));
DISPLAY_FLOAT_VECT3("r2", (*r2));
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, (*r2), (mag_rate_update_gain*res_norm));
DISPLAY_FLOAT_RATES("corr", ahrs_impl.rate_correction);
const float mag_bias_update_gain = -2.5e-4;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.gyro_bias, (*r2), (mag_bias_update_gain*res_norm));
/* FIXME: saturate bias */
}
void ahrs_update_mag2(void) {
const struct FloatVect3 expected_ltp = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 expected_imu;
FLOAT_RMAT_VECT3_MUL(expected_imu, ahrs_float.ltp_to_imu_rmat, expected_ltp);
struct FloatVect3 measured_imu;
MAGS_FLOAT_OF_BFP(measured_imu, imu.mag);
struct FloatVect3 residual;
FLOAT_VECT3_CROSS_PRODUCT(residual, measured_imu, expected_imu);
// FLOAT_VECT3_DIFF(residual, expected_imu, measured_imu);
DISPLAY_FLOAT_VECT3(" expected", expected_imu);
DISPLAY_FLOAT_VECT3(" measured", measured_imu);
DISPLAY_FLOAT_VECT3("residual", residual);
const float mag_rate_update_gain = 2.5;
FLOAT_RATES_ADD_SCALED_VECT(ahrs_impl.rate_correction, residual, mag_rate_update_gain);
}
/*
* Compute ltp to imu rotation in euler angles and quaternion representations
@@ -199,6 +261,14 @@ static inline void compute_imu_quat_and_euler_from_rmat(void) {
FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat);
}
static inline void compute_imu_rmat_and_euler_from_quat(void) {
FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
FLOAT_EULERS_OF_RMAT(ahrs_float.ltp_to_imu_euler, ahrs_float.ltp_to_imu_rmat);
}
/*
* Compute body orientation and rates from imu orientation and rates
*/
sw/airborne/subsystems/ahrs/ahrs_float_cmpl_rmat.h
+3
View File
@@ -27,6 +27,9 @@
struct AhrsFloatCmplRmat {
struct FloatRates gyro_bias;
struct FloatRates rate_correction;
/* for gravity correction during coordinated turns */
struct FloatVect3 est_ltp_speed;
/*
Holds float version of IMU alignement
in order to be able to run against the fixed point
sw/airborne/subsystems/ahrs/ahrs_float_utils.h
+4 -1
View File
@@ -8,6 +8,7 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st
const float phi = atan2f(-accelf.y, -accelf.z);
const float cphi = cosf(phi);
const float theta = atan2f(cphi*accelf.x, -accelf.z);
/* get psi from magnetometer */
/* project mag on local tangeant plane */
struct FloatVect3 magf;
@@ -17,8 +18,10 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st
const float stheta = sinf(theta);
const float mn = ctheta * magf.x + sphi*stheta*magf.y + cphi*stheta*magf.z;
const float me = 0. * magf.x + cphi *magf.y - sphi *magf.z;
const float psi = -atan2f(me, mn);
float psi = -atan2f(me, mn) + atan2(AHRS_H_Y, AHRS_H_X);
if (psi > M_PI) psi -= 2.*M_PI; if (psi < -M_PI) psi+= 2.*M_PI;
EULERS_ASSIGN(*e, phi, theta, psi);
}
#endif /* AHRS_FLOAT_UTILS_H */
sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c
+322
View File
@@ -0,0 +1,322 @@
/*
* $Id$
*
* Copyright (C) 2008-2011 The Paparazzi Team
*
* 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.
*/
// TODO
//
// gravity heuristic
// gps based gravity correction
// gps update for yaw on fixed wing ?
//
#include "subsystems/ahrs/ahrs_int_cmpl.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "subsystems/ahrs/ahrs_int_utils.h"
#include "subsystems/imu.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"
//#include "../../test/pprz_algebra_print.h"
static inline void ahrs_update_mag_full(void);
static inline void ahrs_update_mag_2d(void);
/* in place quaternion first order integration with constante rotational velocity */
/* */
#define INT32_QUAT_INTEGRATE_FI(_q, _hr, _omega, _f) { \
_hr.qi += -_omega.p*_q.qx - _omega.q*_q.qy - _omega.r*_q.qz; \
_hr.qx += _omega.p*_q.qi + _omega.r*_q.qy - _omega.q*_q.qz; \
_hr.qy += _omega.q*_q.qi - _omega.r*_q.qx + _omega.p*_q.qz; \
_hr.qz += _omega.r*_q.qi + _omega.q*_q.qx - _omega.p*_q.qy; \
\
ldiv_t _div = ldiv(_hr.qi, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qi+= _div.quot; \
_hr.qi = _div.rem; \
\
_div = ldiv(_hr.qx, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qx+= _div.quot; \
_hr.qx = _div.rem; \
\
_div = ldiv(_hr.qy, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qy+= _div.quot; \
_hr.qy = _div.rem; \
\
_div = ldiv(_hr.qz, ((1<<INT32_RATE_FRAC)*_f*2)); \
_q.qz+= _div.quot; \
_hr.qz = _div.rem; \
\
}
struct AhrsIntCmpl ahrs_impl;
static inline void compute_imu_quat_and_rmat_from_euler(void);
static inline void compute_imu_euler_and_rmat_from_quat(void);
static inline void compute_body_orientation(void);
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
// FIXME: make ltp_to_imu and ltp_to_body coherent
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
INT_EULERS_ZERO(ahrs.ltp_to_imu_euler);
INT32_QUAT_ZERO(ahrs.ltp_to_body_quat);
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT32_RMAT_ZERO(ahrs.ltp_to_body_rmat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(ahrs_impl.gyro_bias);
INT_RATES_ZERO(ahrs_impl.rate_correction);
INT_RATES_ZERO(ahrs_impl.high_rez_bias);
}
void ahrs_align(void) {
/* Compute an initial orientation using euler angles */
ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
/* Convert initial orientation in quaternion and rotation matrice representations. */
compute_imu_quat_and_rmat_from_euler();
compute_body_orientation();
/* Use low passed gyro value as initial bias */
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
RATES_COPY( ahrs_impl.high_rez_bias, ahrs_aligner.lp_gyro);
INT_RATES_LSHIFT(ahrs_impl.high_rez_bias, ahrs_impl.high_rez_bias, 28);
ahrs.status = AHRS_RUNNING;
}
/*
*
*
*
*/
void ahrs_propagate(void) {
/* unbias gyro */
struct Int32Rates omega;
RATES_DIFF(omega, imu.gyro_prev, ahrs_impl.gyro_bias);
/* low pass rate */
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
RATES_SMUL(ahrs.imu_rate, ahrs.imu_rate,2);
RATES_ADD(ahrs.imu_rate, omega);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 3);
#else
RATES_COPY(ahrs.imu_rate, omega);
#endif
/* add correction */
RATES_ADD(omega, ahrs_impl.rate_correction);
/* and zeros it */
INT_RATES_ZERO(ahrs_impl.rate_correction);
/* integrate quaternion */
INT32_QUAT_INTEGRATE_FI(ahrs.ltp_to_imu_quat, ahrs_impl.high_rez_quat, omega, AHRS_PROPAGATE_FREQUENCY);
INT32_QUAT_NORMALIZE(ahrs.ltp_to_imu_quat);
compute_imu_euler_and_rmat_from_quat();
compute_body_orientation();
}
void ahrs_update_accel(void) {
struct Int32Vect3 c2 = { RMAT_ELMT(ahrs.ltp_to_imu_rmat, 0,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 1,2),
RMAT_ELMT(ahrs.ltp_to_imu_rmat, 2,2)};
struct Int32Vect3 residual;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
// FIXME: check overflow ?
const struct Int32Vect3 Xdd_imu = {
0,
((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.r)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC),
-((ahrs_impl.ltp_vel_norm>>INT32_ACCEL_FRAC) * ahrs.imu_rate.q)
>>(INT32_SPEED_FRAC+INT32_RATE_FRAC-INT32_ACCEL_FRAC-INT32_ACCEL_FRAC)
};
struct Int32Vect3 corrected_gravity;
VECT3_DIFF(corrected_gravity, imu.accel, Xdd_imu);
INT32_VECT3_CROSS_PRODUCT(residual, corrected_gravity, c2);
#else
INT32_VECT3_CROSS_PRODUCT(residual, imu.accel, c2);
#endif
// residual FRAC : ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^24 * 5e-2 = 1/81920
ahrs_impl.rate_correction.p += -residual.x/82000;
ahrs_impl.rate_correction.q += -residual.y/82000;
ahrs_impl.rate_correction.r += -residual.z/82000;
// residual FRAC = ACCEL_FRAC + TRIG_FRAC = 10 + 14 = 24
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^24 * 5e-6 = 1/3.05
// ahrs_impl.high_rez_bias.p += residual.x*3;
// ahrs_impl.high_rez_bias.q += residual.y*3;
// ahrs_impl.high_rez_bias.r += residual.z*3;
ahrs_impl.high_rez_bias.p += residual.x;
ahrs_impl.high_rez_bias.q += residual.y;
ahrs_impl.high_rez_bias.r += residual.z;
/* */
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
void ahrs_update_mag(void) {
#ifdef AHRS_MAG_UPDATE_YAW_ONLY
ahrs_update_mag_2d();
#else
ahrs_update_mag_full();
#endif
}
static inline void ahrs_update_mag_full(void) {
const struct Int32Vect3 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y),
MAG_BFP_OF_REAL(AHRS_H_Z)};
struct Int32Vect3 expected_imu;
INT32_RMAT_VMULT(expected_imu, ahrs.ltp_to_imu_rmat, expected_ltp);
struct Int32Vect3 residual;
INT32_VECT3_CROSS_PRODUCT(residual, imu.mag, expected_imu);
ahrs_impl.rate_correction.p += residual.x/32/16;
ahrs_impl.rate_correction.q += residual.y/32/16;
ahrs_impl.rate_correction.r += residual.z/32/16;
ahrs_impl.high_rez_bias.p += -residual.x/32*1024;
ahrs_impl.high_rez_bias.q += -residual.y/32*1024;
ahrs_impl.high_rez_bias.r += -residual.z/32*1024;
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
static inline void ahrs_update_mag_2d(void) {
const struct Int32Vect2 expected_ltp = {MAG_BFP_OF_REAL(AHRS_H_X),
MAG_BFP_OF_REAL(AHRS_H_Y)};
struct Int32Vect3 measured_ltp;
INT32_RMAT_TRANSP_VMULT(measured_ltp, ahrs.ltp_to_imu_rmat, imu.mag);
struct Int32Vect3 residual_ltp =
{ 0,
0,
(measured_ltp.x * expected_ltp.y - measured_ltp.y * expected_ltp.x)/(1<<5)};
struct Int32Vect3 residual_imu;
INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// rate_correction FRAC = RATE_FRAC = 12
// 2^12 / 2^22 * 2.5 = 1/410
// ahrs_impl.rate_correction.p += residual_imu.x*(1<<5)/410;
// ahrs_impl.rate_correction.q += residual_imu.y*(1<<5)/410;
// ahrs_impl.rate_correction.r += residual_imu.z*(1<<5)/410;
ahrs_impl.rate_correction.p += residual_imu.x/16;
ahrs_impl.rate_correction.q += residual_imu.y/16;
ahrs_impl.rate_correction.r += residual_imu.z/16;
// residual_ltp FRAC = 2 * MAG_FRAC = 22
// high_rez_bias = RATE_FRAC+28 = 40
// 2^40 / 2^22 * 2.5e-3 = 655
// ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<5)*655;
// ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<5)*655;
// ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<5)*655;
ahrs_impl.high_rez_bias.p -= residual_imu.x*1024;
ahrs_impl.high_rez_bias.q -= residual_imu.y*1024;
ahrs_impl.high_rez_bias.r -= residual_imu.z*1024;
INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
}
/* Compute ltp to imu rotation in quaternion and rotation matrice representation
from the euler angle representation */
__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {
/* Compute LTP to IMU quaternion */
INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
/* Compute LTP to IMU rotation matrix */
INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
}
/* Compute ltp to imu rotation in euler angles and rotation matrice representation
from the quaternion representation */
__attribute__ ((always_inline)) static inline void compute_imu_euler_and_rmat_from_quat(void) {
/* Compute LTP to IMU euler */
INT32_EULERS_OF_QUAT(ahrs.ltp_to_imu_euler, ahrs.ltp_to_imu_quat);
/* Compute LTP to IMU rotation matrix */
INT32_RMAT_OF_QUAT(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_quat);
}
__attribute__ ((always_inline)) static inline void compute_body_orientation(void) {
/* Compute LTP to BODY quaternion */
INT32_QUAT_COMP_INV(ahrs.ltp_to_body_quat, ahrs.ltp_to_imu_quat, imu.body_to_imu_quat);
/* Compute LTP to BODY rotation matrix */
INT32_RMAT_COMP_INV(ahrs.ltp_to_body_rmat, ahrs.ltp_to_imu_rmat, imu.body_to_imu_rmat);
/* compute LTP to BODY eulers */
INT32_EULERS_OF_RMAT(ahrs.ltp_to_body_euler, ahrs.ltp_to_body_rmat);
/* compute body rates */
INT32_RMAT_TRANSP_RATEMULT(ahrs.body_rate, imu.body_to_imu_rmat, ahrs.imu_rate);
}
sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h
+43
View File
@@ -0,0 +1,43 @@
/*
* $Id$
*
* Copyright (C) 2011 The Paparazzi Team
*
* 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.
*/
#ifndef AHRS_INT_CMPL_H
#define AHRS_INT_CMPL_H
#include "subsystems/ahrs.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
struct AhrsIntCmpl {
struct Int32Rates gyro_bias;
struct Int32Rates rate_correction;
struct Int64Quat high_rez_quat;
struct Int64Rates high_rez_bias;
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
int32_t ltp_vel_norm;
#endif
};
extern struct AhrsIntCmpl ahrs_impl;
#endif /* AHRS_INT_CMPL_H */
sw/airborne/subsystems/ahrs/ahrs_int_utils.h
+47
View File
@@ -0,0 +1,47 @@
#ifndef AHRS_INT_UTILS_H
#define AHRS_INT_UTILS_H
//#include "../../test/pprz_algebra_print.h"
#include "math/pprz_algebra_int.h"
#include "generated/airframe.h"
static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
// DISPLAY_INT32_VECT3("# accel", (*accel));
const float fphi = atan2f(-accel->y, -accel->z);
// printf("# atan float %f\n", DegOfRad(fphi));
e->phi = ANGLE_BFP_OF_REAL(fphi);
int32_t cphi;
PPRZ_ITRIG_COS(cphi, e->phi);
int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel->x, INT32_TRIG_FRAC);
const float ftheta = atan2f(-cphi_ax, -accel->z);
e->theta = ANGLE_BFP_OF_REAL(ftheta);
int32_t sphi;
PPRZ_ITRIG_SIN(sphi, e->phi);
int32_t stheta;
PPRZ_ITRIG_SIN(stheta, e->theta);
int32_t ctheta;
PPRZ_ITRIG_COS(ctheta, e->theta);
int32_t sphi_stheta = (sphi*stheta)>>INT32_TRIG_FRAC;
int32_t cphi_stheta = (cphi*stheta)>>INT32_TRIG_FRAC;
//int32_t sphi_ctheta = (sphi*ctheta)>>INT32_TRIG_FRAC;
//int32_t cphi_ctheta = (cphi*ctheta)>>INT32_TRIG_FRAC;
const int32_t mn = ctheta * mag->x + sphi_stheta * mag->y + cphi_stheta * mag->z;
const int32_t me = 0 * mag->x + cphi * mag->y - sphi * mag->z;
//const int32_t md =
// -stheta * imu.mag.x +
// sphi_ctheta * imu.mag.y +
// cphi_ctheta * imu.mag.z;
// float m_psi = -atan2(me, mn);
const float mag_dec = atan2(-AHRS_H_Y, AHRS_H_X);
const float fpsi = atan2f(-me, mn) - mag_dec;
e->psi = ANGLE_BFP_OF_REAL(fpsi);
INT32_ANGLE_NORMALIZE(e->psi);
}
#endif /* AHRS_INT_UTILS_H */
sw/airborne/subsystems/imu/imu_dummy.c
+7
View File
@@ -0,0 +1,7 @@
void imu_impl_init(void) {
}
sw/airborne/test/ahrs/Makefile
+113 -6
View File
@@ -1,21 +1,31 @@
# Launch with "make Q=''" to get full command display
Q=@
CC = gcc
CFLAGS = -std=c99 -I.. -I../.. -I../../../include -I../../booz -I../../../booz -Wall
LDFLAGS = -lm
LDFLAGS = -lm -lgsl -lgslcblas
CFLAGS +=
# imu wants airframe to fetch its neutrals
# ahrs wants airframe to fetch IMU_BODY_TO_IMU_ANGLES
CFLAGS += -I../../../../var/BOOZ2_A7
#CFLAGS += -I../../../../var/BOOZ2_A7
CFLAGS += -I../../../../var/TA
#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
#CFLAGS += -DDISABLE_ALIGNEMENT
#CFLAGS += -DDISABLE_PROPAGATE
#CFLAGS += -DDISABLE_ACCEL_UPDATE
#CFLAGS += -DDISABLE_MAG_UPDATE
#CFLAGS += -DPERFECT_SENSORS
#CFLAGS += -DLOW_NOISE_THRESHOLD='ACCEL_BFP_OF_REAL(5.5)'
CFLAGS += -DLOW_NOISE_THRESHOLD=500000
CFLAGS += -DLOW_NOISE_TIME=2
SRCS= run_ahrs_on_flight_log.c \
../../math/pprz_trig_int.c \
@@ -34,5 +44,102 @@ run_ahrs_fcr_on_flight_log: ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c $(SRCS)
run_ahrs_ice_on_flight_log: ../../subsystems/ahrs/ahrs_int_cmpl_euler.c $(SRCS)
$(CC) -DAHRS_TYPE=AHRS_TYPE_ICE $(CFLAGS) -o $@ $^ $(LDFLAGS)
ifndef AHRS_TYPE
#AHRS_TYPE = AHRS_TYPE_ICE
AHRS_TYPE = AHRS_TYPE_ICQ
#AHRS_TYPE = AHRS_TYPE_FLQ
#AHRS_TYPE = AHRS_TYPE_FCR # doesn't work - inverted accel
#AHRS_TYPE = AHRS_TYPE_FCR2
#AHRS_TYPE = AHRS_TYPE_FCQ
endif
ifndef PROPAGATE_LOW_PASS_RATES
PROPAGATE_LOW_PASS_RATES = 0
endif
ifndef GRAVITY_UPDATE_NORM_HEURISTIC
GRAVITY_UPDATE_NORM_HEURISTIC = 0
endif
ifndef GRAVITY_UPDATE_COORDINATED_TURN
GRAVITY_UPDATE_COORDINATED_TURN = 0
endif
ifndef MAG_UPDATE_YAW_ONLY
MAG_UPDATE_YAW_ONLY = 0
endif
AHRS_CFLAGS += -DAHRS_TYPE=$(AHRS_TYPE)
AHRS_CFLAGS += -DPERIODIC_FREQUENCY=512
AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
ifeq ($(PROPAGATE_LOW_PASS_RATES), 1)
AHRS_CFLAGS += -DAHRS_PROPAGATE_LOW_PASS_RATES
endif
ifeq ($(GRAVITY_UPDATE_NORM_HEURISTIC), 1)
AHRS_CFLAGS += -DAHRS_GRAVITY_UPDATE_NORM_HEURISTIC
endif
ifeq ($(GRAVITY_UPDATE_COORDINATED_TURN), 1)
AHRS_CFLAGS += -DAHRS_GRAVITY_UPDATE_COORDINATED_TURN
endif
ifeq ($(MAG_UPDATE_YAW_ONLY), 1)
AHRS_CFLAGS += -DAHRS_MAG_UPDATE_YAW_ONLY
endif
AHRS_SRCS += ../../subsystems/ahrs.c \
../../subsystems/ahrs/ahrs_aligner.c \
ifeq ($(AHRS_TYPE), AHRS_TYPE_ICE)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl_euler.h\"
AHRS_CFLAGS += -DFACE_REINJ_1=1024
AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl_euler.c
endif
ifeq ($(AHRS_TYPE), AHRS_TYPE_ICQ)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_int_cmpl.h\"
AHRS_SRCS += ../../subsystems/ahrs/ahrs_int_cmpl.c
endif
ifeq ($(AHRS_TYPE), AHRS_TYPE_FLQ)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_lkf_quat.h\"
AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_lkf_quat.c
endif
ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_dcm.h\"
AHRS_CFLAGS += -DINS_ROLL_NEUTRAL_DEFAULT=0
AHRS_CFLAGS += -DINS_PITCH_NEUTRAL_DEFAULT=0
AHRS_CFLAGS += -DAHRS_PROPAGATE_FREQUENCY=512
AHRS_CFLAGS += -DDCM_UPDATE_AFTER_PROPAGATE
AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_dcm.c
endif
ifeq ($(AHRS_TYPE), AHRS_TYPE_FCR2)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_RMAT
AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c
endif
ifeq ($(AHRS_TYPE), AHRS_TYPE_FCQ)
AHRS_CFLAGS += -DAHRS_TYPE_H=\"subsystems/ahrs/ahrs_float_cmpl_rmat.h\"
AHRS_CFLAGS += -DAHRS_PROPAGATE_QUAT
AHRS_SRCS += ../../subsystems/ahrs/ahrs_float_cmpl_rmat.c
endif
RAOS_SRCS = ./ahrs_on_synth.c \
../../subsystems/imu.c \
../../subsystems/imu/imu_dummy.c \
../../math/pprz_trig_int.c \
../../../simulator/nps/nps_random.c
run_ahrs_on_synth: run_ahrs_on_synth.c $(RAOS_SRCS) $(AHRS_SRCS)
@echo "Building run_ahrs_on_synth for $(AHRS_TYPE)"
$(Q) $(CC) $(CFLAGS) $(AHRS_CFLAGS) -o $@ $^ $(LDFLAGS)
IVY_CFLAGS=-g -O2 -Wall `pkg-config glib-2.0 --cflags`
IVY_LDFLAGS=`pkg-config glib-2.0 --libs` `pcre-config --libs` -lglibivy
run_ahrs_on_synth_ivy: run_ahrs_on_synth_ivy.c $(RAOS_SRCS) $(AHRS_SRCS)
$(CC) $(CFLAGS) $(AHRS_CFLAGS) $(IVY_CFLAGS) -o $@ $^ $(LDFLAGS) $(IVY_LDFLAGS)
clean:
rm -f *~ run_ahrs_*_on_flight_log
@echo "cleaning ..."
$(Q) rm -f *~ run_ahrs_*_on_flight_log run_ahrs_on_synth_ivy run_ahrs_on_synth
sw/airborne/test/ahrs/ahrs_on_synth.c
+413
View File
@@ -0,0 +1,413 @@
#include "test/ahrs/ahrs_on_synth.h"
#include <stdio.h>
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "../simulator/nps/nps_random.h"
#include "../pprz_algebra_print.h"
char* ahrs_type_str[AHRS_TYPE_NB] = {
"Int Compl Euler",
"Int Compl Quat",
"Float LKF Quat",
"Float Compl Rmat",
"Float Compl Rmat 2",
"Float Compl Quat" };
static void traj_static_static_init(void);
static void traj_static_static_update(void);
static void traj_step_phi_init(void);
static void traj_step_phi_update(void);
static void traj_step_phi_2nd_order_init(void);
static void traj_step_phi_2nd_order_update(void);
static void traj_step_biasp_init(void);
static void traj_step_biasp_update(void);
static void traj_static_sine_init(void);
static void traj_static_sine_update(void);
static void traj_sineX_quad_init(void);
static void traj_sineX_quad_update(void);
static void traj_coordinated_circle_init(void);
static void traj_coordinated_circle_update(void);
static void traj_stop_stop_x_init(void);
static void traj_stop_stop_x_update(void);
struct traj traj[] = {
{.name="static", .desc="blaa",
.init_fun=traj_static_static_init, .update_fun=traj_static_static_update},
{.name="sine", .desc="blaa2",
.init_fun=traj_static_sine_init, .update_fun=traj_static_sine_update},
{.name="sineX", .desc="blaa2",
.init_fun=traj_sineX_quad_init, .update_fun=traj_sineX_quad_update},
{.name="step_phi", .desc="blaa2",
.init_fun=traj_step_phi_init, .update_fun=traj_step_phi_update},
{.name="step_phi2", .desc="blaa2",
.init_fun=traj_step_phi_2nd_order_init, .update_fun=traj_step_phi_2nd_order_update},
{.name="step_bias", .desc="blaa2",
.init_fun=traj_step_biasp_init, .update_fun=traj_step_biasp_update},
{.name="coordinated circle", .desc="blaa2",
.init_fun=traj_coordinated_circle_init, .update_fun=traj_coordinated_circle_update},
{.name="stop stop x", .desc="blaa2",
.init_fun=traj_stop_stop_x_init, .update_fun=traj_stop_stop_x_update}
};
struct AhrsOnSynth aos;
void aos_init(int traj_nb) {
aos.traj = &traj[traj_nb];
aos.time = 0;
aos.dt = 1./AHRS_PROPAGATE_FREQUENCY;
aos.traj->ts = 0;
aos.traj->ts = 1.; // default to one second
/* default state */
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
RATES_ASSIGN(aos.imu_rates, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
FLOAT_VECT3_ZERO(aos.ltp_pos);
FLOAT_VECT3_ZERO(aos.ltp_vel);
FLOAT_VECT3_ZERO(aos.ltp_accel);
FLOAT_VECT3_ZERO(aos.ltp_jerk);
aos.traj->init_fun();
imu_init();
ahrs_init();
ahrs_aligner_init();
#ifdef PERFECT_SENSORS
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(0.), RadOfDeg(0.), RadOfDeg(0.));
VECT3_ASSIGN(aos.accel_noise, 0., 0., 0.);
#else
RATES_ASSIGN(aos.gyro_bias, RadOfDeg(1.), RadOfDeg(2.), RadOfDeg(3.));
RATES_ASSIGN(aos.gyro_noise, RadOfDeg(1.), RadOfDeg(1.), RadOfDeg(1.));
VECT3_ASSIGN(aos.accel_noise, .5, .5, .5);
#endif
#ifdef FORCE_ALIGNEMENT
// DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("# oas quat", aos.ltp_to_imu_quat);
aos_compute_sensors();
// DISPLAY_FLOAT_RATES_DEG("# oas gyro_bias", aos.gyro_bias);
// DISPLAY_FLOAT_RATES_DEG("# oas imu_rates", aos.imu_rates);
VECT3_COPY(ahrs_aligner.lp_accel, imu.accel);
VECT3_COPY(ahrs_aligner.lp_mag, imu.mag);
RATES_COPY(ahrs_aligner.lp_gyro, imu.gyro);
// DISPLAY_INT32_RATES_AS_FLOAT_DEG("# ahrs_aligner.lp_gyro", ahrs_aligner.lp_gyro);
ahrs_align();
// DISPLAY_FLOAT_RATES_DEG("# ahrs_impl.gyro_bias", ahrs_impl.gyro_bias);
#endif
#ifdef DISABLE_ALIGNEMENT
printf("# DISABLE_ALIGNEMENT\n");
#endif
#ifdef DISABLE_PROPAGATE
printf("# DISABLE_PROPAGATE\n");
#endif
#ifdef DISABLE_ACCEL_UPDATE
printf("# DISABLE_ACCEL_UPDATE\n");
#endif
#ifdef DISABLE_MAG_UPDATE
printf("# DISABLE_MAG_UPDATE\n");
#endif
printf("# AHRS_TYPE %s\n", ahrs_type_str[AHRS_TYPE]);
printf("# AHRS_PROPAGATE_FREQUENCY %d\n", AHRS_PROPAGATE_FREQUENCY);
#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
printf("# AHRS_PROPAGATE_LOW_PASS_RATES\n");
#endif
#ifdef AHRS_MAG_UPDATE_YAW_ONLY
printf("# AHRS_MAG_UPDATE_YAW_ONLY\n");
#endif
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
printf("# AHRS_GRAVITY_UPDATE_COORDINATED_TURN\n");
#endif
#ifdef AHRS_GRAVITY_UPDATE_NORM_HEURISTIC
printf("# AHRS_GRAVITY_UPDATE_NORM_HEURISTIC\n");
#endif
#ifdef PERFECT_SENSORS
printf("# PERFECT_SENSORS\n");
#endif
printf("# tajectory : %s\n", aos.traj->name);
};
void aos_compute_sensors(void) {
struct FloatRates gyro;
RATES_SUM(gyro, aos.imu_rates, aos.gyro_bias);
// printf("#aos.gyro_bias %f\n",DegOfRad( aos.gyro_bias.r));
float_rates_add_gaussian_noise(&gyro, &aos.gyro_noise);
RATES_BFP_OF_REAL(imu.gyro, gyro);
RATES_BFP_OF_REAL(imu.gyro_prev, gyro);
struct FloatVect3 g_ltp = {0., 0., 9.81};
struct FloatVect3 accelero_ltp;
VECT3_DIFF(accelero_ltp, aos.ltp_accel, g_ltp);
struct FloatVect3 accelero_imu;
FLOAT_QUAT_VMULT(accelero_imu, aos.ltp_to_imu_quat, accelero_ltp);
float_vect3_add_gaussian_noise(&accelero_imu, &aos.accel_noise);
ACCELS_BFP_OF_REAL(imu.accel, accelero_imu);
struct FloatVect3 h_earth = {AHRS_H_X, AHRS_H_Y, AHRS_H_Z};
struct FloatVect3 h_imu;
FLOAT_QUAT_VMULT(h_imu, aos.ltp_to_imu_quat, h_earth);
MAGS_BFP_OF_REAL(imu.mag, h_imu);
#ifdef AHRS_GRAVITY_UPDATE_COORDINATED_TURN
#if AHRS_TYPE == AHRS_TYPE_FCR2 || AHRS_TYPE == AHRS_TYPE_FCQ || AHRS_TYPE == AHRS_TYPE_FLQ
VECT3_COPY(ahrs_impl.est_ltp_speed, aos.ltp_vel);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
ahrs_impl.ltp_vel_norm = SPEED_BFP_OF_REAL(FLOAT_VECT3_NORM(aos.ltp_vel));
#endif
#endif
}
void aos_compute_state(void) {
aos.time += aos.dt;
aos.traj->update_fun();
}
void aos_run(void) {
aos_compute_state();
aos_compute_sensors();
#ifndef DISABLE_ALIGNEMENT
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
#endif /* DISABLE_ALIGNEMENT */
ahrs_propagate();
ahrs_update_accel();
ahrs_update_mag();
#ifndef DISABLE_ALIGNEMENT
}
#endif
}
static void traj_static_static_init(void) {
aos.traj->te = 120.;
}
static void traj_static_static_update(void) {
// if (aos.time > 3) {
// EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
// FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
// }
// aos.imu_rates.p = 0.;
// aos.imu_rates.q = 0.;
// aos.imu_rates.r = 0.;
}
//
//
//
static void traj_static_sine_init(void) {
aos.traj->te = 10.;
}
static void traj_static_sine_update(void) {
aos.imu_rates.p = RadOfDeg(200)*cos(aos.time);
aos.imu_rates.q = RadOfDeg(200)*cos(0.7*aos.time+2);
aos.imu_rates.r = RadOfDeg(200)*cos(0.8*aos.time+1);
FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);
}
//
// this is a sine trajectory along the x axis
// we pretend a dragless vectoriel trust vehicle banks
// to achieve it
//
static void traj_sineX_quad_init(void) { aos.traj->te = 60.; }
static void traj_sineX_quad_update(void) {
const float om = RadOfDeg(10);
if ( aos.time > (M_PI/om) ) {
const float a = 20;
struct FloatVect3 jerk;
VECT3_ASSIGN(jerk , -a*om*om*om*cos(om*aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_accel , -a*om*om *sin(om*aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_vel , a*om *cos(om*aos.time), 0, 0);
VECT3_ASSIGN(aos.ltp_pos , a *sin(om*aos.time), 0, 0);
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81*jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
0. };
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
//
//
//
static void traj_step_phi_init(void) { aos.traj->te = 40.;}
static void traj_step_phi_update(void) {
if (aos.time > 5) {
EULERS_ASSIGN(aos.ltp_to_imu_euler, RadOfDeg(5), 0, 0);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
}
}
//
//
//
static void traj_step_phi_2nd_order_init(void) {
aos.traj->te = 0.;
aos.traj->te = 40.;
}
static void traj_step_phi_2nd_order_update(void) {
if (aos.time > 15) {
const float omega = RadOfDeg(100);
const float xi = 0.9;
struct FloatRates raccel;
RATES_ASSIGN(raccel, -2.*xi*omega*aos.imu_rates.p - omega*omega*(aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0., 0.);
FLOAT_RATES_INTEGRATE_FI(aos.imu_rates, raccel, aos.dt);
FLOAT_QUAT_INTEGRATE(aos.ltp_to_imu_quat, aos.imu_rates, aos.dt);
FLOAT_EULERS_OF_QUAT(aos.ltp_to_imu_euler, aos.ltp_to_imu_quat);
}
}
static void traj_step_biasp_init(void) { aos.traj->te = 120.; }
static void traj_step_biasp_update(void) { if (aos.time > 5) aos.gyro_bias.p = RadOfDeg(3);}
static void traj_coordinated_circle_init(void) {
aos.traj->te = 120.;
const float speed = 15; // m/s
const float R = 100; // radius in m
// tan phi = v^2/Rg
float phi = atan2(speed*speed, R*9.81);
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, M_PI_2);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
}
static void traj_coordinated_circle_update(void) {
const float speed = 15; // m/s
const float R = 100; // radius in m
float omega = speed / R;
// tan phi = v^2/Rg
float phi = atan2(speed*speed, R*9.81);
if ( aos.time > 2.*M_PI/omega) {
VECT3_ASSIGN(aos.ltp_pos, R*cos(omega*aos.time), R*sin(omega*aos.time), 0.);
VECT3_ASSIGN(aos.ltp_vel, -omega*R*sin(omega*aos.time), omega*R*cos(omega*aos.time), 0.);
VECT3_ASSIGN(aos.ltp_accel, -omega*omega*R*cos(omega*aos.time), -omega*omega*R*sin(omega*aos.time), 0.);
// float psi = atan2(aos.ltp_pos.y, aos.ltp_pos.x);
float psi = M_PI_2 + omega*aos.time; while (psi>M_PI) psi -= 2.*M_PI;
EULERS_ASSIGN(aos.ltp_to_imu_euler, phi, 0, psi);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
struct FloatEulers e_dot;
EULERS_ASSIGN(e_dot, 0., 0., omega);
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
}
//static char** traj_stop_stop_x_desc(void) {
// static const char** desc =
// {"stop top", NULL};
// return desc;
//}
static void traj_stop_stop_x_init(void) { aos.traj->te = 30.;}
static void traj_stop_stop_x_update(void){
const float t0 = 5.;
const float dt_jerk = 0.75;
const float dt_nojerk = 10.;
const float val_jerk = 5.;
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_pos, aos.ltp_vel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_vel, aos.ltp_accel, aos.dt);
FLOAT_VECT3_INTEGRATE_FI(aos.ltp_accel, aos.ltp_jerk, aos.dt);
if (aos.time < t0) return;
else if (aos.time < t0+dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
else if (aos.time < t0 + 2.*dt_jerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
else if (aos.time < t0 + 2.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
else if (aos.time < t0 + 3.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , -val_jerk, 0., 0.); }
else if (aos.time < t0 + 4.*dt_jerk + dt_nojerk) {
VECT3_ASSIGN(aos.ltp_jerk , val_jerk, 0., 0.); }
else {
VECT3_ASSIGN(aos.ltp_jerk , 0. , 0., 0.); }
// this is based on differential flatness of the quad
EULERS_ASSIGN(aos.ltp_to_imu_euler, 0., atan2(aos.ltp_accel.x, 9.81), 0.);
FLOAT_QUAT_OF_EULERS(aos.ltp_to_imu_quat, aos.ltp_to_imu_euler);
const struct FloatEulers e_dot = {
0.,
9.81*aos.ltp_jerk.x / ( (9.81*9.81) + (aos.ltp_accel.x*aos.ltp_accel.x)),
0. };
FLOAT_RATES_OF_EULER_DOT(aos.imu_rates, aos.ltp_to_imu_euler, e_dot);
}
sw/airborne/test/ahrs/ahrs_on_synth.h
+60
View File
@@ -0,0 +1,60 @@
#ifndef AHRS_ON_SYNTH_H
#define AHRS_ON_SYNTH_H
#include "math/pprz_algebra_float.h"
struct traj {
char* name;
char* desc;
void (*init_fun)(void);
void (*update_fun)(void);
double ts;
double te;
};
struct AhrsOnSynth {
struct traj* traj;
double time;
double dt;
/* sensors */
struct FloatRates gyro_bias;
struct FloatRates gyro_noise;
struct FloatVect3 accel_noise;
/* state */
struct FloatEulers ltp_to_imu_euler;
struct FloatQuat ltp_to_imu_quat;
struct FloatRates imu_rates;
struct FloatVect3 ltp_jerk;
struct FloatVect3 ltp_accel;
struct FloatVect3 ltp_vel;
struct FloatVect3 ltp_pos;
};
extern struct AhrsOnSynth aos;
extern void aos_init(int traj_nb);
extern void aos_run(void);
extern void aos_compute_sensors(void);
extern void aos_compute_state(void);
#define AHRS_TYPE_ICE 0
#define AHRS_TYPE_ICQ 1
#define AHRS_TYPE_FLQ 2
#define AHRS_TYPE_FCR 3
#define AHRS_TYPE_FCR2 4
#define AHRS_TYPE_FCQ 5
#define AHRS_TYPE_NB 6
extern char* ahrs_type_str[AHRS_TYPE_NB];
#endif /* AHRS_ON_SYNTH_H */
sw/airborne/test/ahrs/ahrs_utils.py
+158
View File
@@ -0,0 +1,158 @@
#! /usr/bin/env python
# $Id$
# Copyright (C) 2011 Antoine Drouin
#
# 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.
#
#import os
#from optparse import OptionParser
#import scipy
#from scipy import optimize
import shlex, subprocess
from pylab import *
from array import array
import numpy
import matplotlib.pyplot as plt
def run_simulation(ahrs_type, build_opt, traj_nb):
print "\nBuilding ahrs"
args = ["make", "clean", "run_ahrs_on_synth", "AHRS_TYPE=AHRS_TYPE_"+ahrs_type] + build_opt
# print args
p = subprocess.Popen(args=args, stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
outputlines = p.stdout.readlines()
p.wait()
for i in outputlines:
print " # "+i,
print
print "Running simulation"
print " using traj " + str(traj_nb)
p = subprocess.Popen(args=["./run_ahrs_on_synth",str(traj_nb)], stdout=subprocess.PIPE, stderr=subprocess.STDOUT, shell=False)
outputlines = p.stdout.readlines()
p.wait()
# for i in outputlines:
# print " "+i,
# print "\n"
ahrs_data_type = [('time', 'float32'),
('phi_true', 'float32'), ('theta_true', 'float32'), ('psi_true', 'float32'),
('p_true', 'float32'), ('q_true', 'float32'), ('r_true', 'float32'),
('bp_true', 'float32'), ('bq_true', 'float32'), ('br_true', 'float32'),
('phi_ahrs', 'float32'), ('theta_ahrs', 'float32'), ('psi_ahrs', 'float32'),
('p_ahrs', 'float32'), ('q_ahrs', 'float32'), ('r_ahrs', 'float32'),
('bp_ahrs', 'float32'), ('bq_ahrs', 'float32'), ('br_ahrs', 'float32')
]
pos_data_type = [ ('x0_true', 'float32'), ('y0_true', 'float32'), ('z0_true', 'float32'),
('x1_true', 'float32'), ('y1_true', 'float32'), ('z1_true', 'float32'),
('x2_true', 'float32'), ('y2_true', 'float32'), ('z2_true', 'float32'),
('x3_true', 'float32'), ('y3_true', 'float32'), ('z3_true', 'float32'),
]
mydescr = numpy.dtype(ahrs_data_type)
data = [[] for dummy in xrange(len(mydescr))]
# import code; code.interact(local=locals())
for line in outputlines:
if line.startswith("#"):
print " "+line,
else:
fields = line.strip().split(' ');
# print fields
for i, number in enumerate(fields):
data[i].append(number)
print
for i in xrange(len(mydescr)):
data[i] = cast[mydescr[i]](data[i])
return numpy.rec.array(data, dtype=mydescr)
def plot_simulation_results(plot_true_state, lsty, sim_res):
print "Plotting Results"
# f, (ax1, ax2, ax3) = plt.subplots(3, sharex=True, sharey=True)
subplot(3,3,1)
plt.plot(sim_res.time, sim_res.phi_ahrs, lsty)
ylabel('degres')
title('phi')
subplot(3,3,2)
plot(sim_res.time, sim_res.theta_ahrs, lsty)
title('theta')
subplot(3,3,3)
plot(sim_res.time, sim_res.psi_ahrs, lsty)
title('psi')
subplot(3,3,4)
plt.plot(sim_res.time, sim_res.p_ahrs, lsty)
ylabel('degres/s')
title('p')
subplot(3,3,5)
plt.plot(sim_res.time, sim_res.q_ahrs, lsty)
title('q')
subplot(3,3,6)
plt.plot(sim_res.time, sim_res.r_ahrs, lsty)
title('r')
subplot(3,3,7)
plt.plot(sim_res.time, sim_res.bp_ahrs, lsty)
ylabel('degres/s')
xlabel('time in s')
title('bp')
subplot(3,3,8)
plt.plot(sim_res.time, sim_res.bq_ahrs, lsty)
xlabel('time in s')
title('bq')
subplot(3,3,9)
plt.plot(sim_res.time, sim_res.br_ahrs, lsty)
xlabel('time in s')
title('br')
if plot_true_state:
subplot(3,3,1)
plt.plot(sim_res.time, sim_res.phi_true, 'r--')
subplot(3,3,2)
plot(sim_res.time, sim_res.theta_true, 'r--')
subplot(3,3,3)
plot(sim_res.time, sim_res.psi_true, 'r--')
subplot(3,3,4)
plot(sim_res.time, sim_res.p_true, 'r--')
subplot(3,3,5)
plot(sim_res.time, sim_res.q_true, 'r--')
subplot(3,3,6)
plot(sim_res.time, sim_res.r_true, 'r--')
subplot(3,3,7)
plot(sim_res.time, sim_res.bp_true, 'r--')
subplot(3,3,8)
plot(sim_res.time, sim_res.bq_true, 'r--')
subplot(3,3,9)
plot(sim_res.time, sim_res.br_true, 'r--')
def show_plot():
plt.show();
sw/airborne/test/ahrs/compare_ahrs.py
Executable
+67
View File
@@ -0,0 +1,67 @@
#! /usr/bin/env python
# $Id$
# Copyright (C) 2011 Antoine Drouin
#
# 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.
#
import ahrs_utils
def main():
# traj_nb = 0; # static
# traj_nb = 1; # sine orientation
# traj_nb = 2; # sine X quad
traj_nb = 3; # step_phi
# traj_nb = 4; # step_phi_2nd_order
# traj_nb = 5; # step_bias
# traj_nb = 6; # coordinated circle
# traj_nb = 7; # stop stop X quad
build_opt1 = [];
# build_opt1 = ["Q="];
build_opt1 += ["PROPAGATE_LOW_PASS_RATES=1"];
# build_opt1 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
build_opt1 += ["GRAVITY_UPDATE_NORM_HEURISTIC=0"];
# build_opt1 = ["MAG_UPDATE_YAW_ONLY=1"];
ahrs_type1 = "FCQ";
# ahrs_type1 = "FCR2";
# ahrs_type1 = "FLQ";
# ahrs_type1 = "ICQ";
sim_res1 = ahrs_utils.run_simulation(ahrs_type1, build_opt1, traj_nb)
# import code; code.interact(local=locals())
build_opt2 = [];
# build_opt2 = ["GRAVITY_UPDATE_COORDINATED_TURN=1"];
build_opt2 += ["GRAVITY_UPDATE_NORM_HEURISTIC=1"];
# build_opt2 = ["MAG_UPDATE_YAW_ONLY=0"];
build_opt2 += ["PROPAGATE_LOW_PASS_RATES=1"];
# build_opt2 = build_opt1;
# ahrs_type2 = "FLQ";
ahrs_type2 = "FCQ";
# ahrs_type2 = "ICQ";
ahrs_type2 = ahrs_type1;
sim_res2 = ahrs_utils.run_simulation(ahrs_type2, build_opt2, traj_nb)
ahrs_utils.plot_simulation_results(False, 'b', sim_res1)
ahrs_utils.plot_simulation_results(True, 'g', sim_res2)
ahrs_utils.show_plot()
if __name__ == "__main__":
main()
sw/airborne/test/ahrs/run_ahrs_on_synth_ivy.c
+102
View File
@@ -0,0 +1,102 @@
#include <glib.h>
#include <stdio.h>
#include <math.h>
#include <stdlib.h>
#include <string.h>
#include <Ivy/ivy.h>
#include <Ivy/ivyglibloop.h>
#include "test/ahrs/ahrs_on_synth.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "math/pprz_algebra_float.h"
#include "math/pprz_algebra_int.h"
#include "test/pprz_algebra_print.h"
gboolean timeout_callback(gpointer data) {
for (int i=0; i<20; i++) {
aos_compute_state();
aos_compute_sensors();
#ifndef DISABLE_PROPAGATE
ahrs_propagate();
#endif
#ifndef DISABLE_ACCEL_UPDATE
ahrs_update_accel();
#endif
#ifndef DISABLE_MAG_UPDATE
if (!(i%5)) ahrs_update_mag();
#endif
}
#if AHRS_TYPE == AHRS_TYPE_ICE || AHRS_TYPE == AHRS_TYPE_ICQ
EULERS_FLOAT_OF_BFP(ahrs_float.ltp_to_imu_euler, ahrs.ltp_to_imu_euler);
#endif
#if AHRS_TYPE == AHRS_TYPE_ICQ
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
ahrs_impl.gyro_bias.p,
ahrs_impl.gyro_bias.q,
ahrs_impl.gyro_bias.r);
#endif
#if AHRS_TYPE == AHRS_TYPE_FLQ || AHRS_TYPE == AHRS_TYPE_FCR2
struct Int32Rates bias_i;
RATES_BFP_OF_REAL(bias_i, ahrs_impl.gyro_bias);
IvySendMsg("183 BOOZ_AHRS_BIAS %d %d %d",
bias_i.p,
bias_i.q,
bias_i.r);
#endif
IvySendMsg("183 AHRS_EULER %f %f %f",
ahrs_float.ltp_to_imu_euler.phi,
ahrs_float.ltp_to_imu_euler.theta,
ahrs_float.ltp_to_imu_euler.psi);
IvySendMsg("183 BOOZ_SIM_RATE_ATTITUDE %f %f %f %f %f %f",
DegOfRad(aos.imu_rates.p),
DegOfRad(aos.imu_rates.q),
DegOfRad(aos.imu_rates.r),
DegOfRad(aos.ltp_to_imu_euler.phi),
DegOfRad(aos.ltp_to_imu_euler.theta),
DegOfRad(aos.ltp_to_imu_euler.psi));
IvySendMsg("183 BOOZ_SIM_GYRO_BIAS %f %f %f",
DegOfRad(aos.gyro_bias.p),
DegOfRad(aos.gyro_bias.q),
DegOfRad(aos.gyro_bias.r));
return TRUE;
}
int main ( int argc, char** argv) {
printf("hello\n");
g_timeout_add(1000/25, timeout_callback, NULL);
GMainLoop *ml = g_main_loop_new(NULL, FALSE);
IvyInit ("test_ahrs", "test_ahrs READY", NULL, NULL, NULL, NULL);
IvyStart("127.255.255.255");
imu_init();
ahrs_init();
aos_init();
g_main_loop_run(ml);
return 0;
}
sw/airborne/test/pprz_algebra_print.h
+17
View File
@@ -12,6 +12,10 @@
printf("%s %f %f %f\n",text, (_v).p, (_v).q, (_v).r); \
}
#define DISPLAY_FLOAT_RATES_DEG(text, _v) { \
printf("%s %f %f %f\n",text, DegOfRad((_v).p), DegOfRad((_v).q), DegOfRad((_v).r)); \
}
#define DISPLAY_FLOAT_RMAT(text, mat) { \
printf("%s\n %f %f %f\n %f %f %f\n %f %f %f\n",text, \
mat.m[0], mat.m[1], mat.m[2], mat.m[3], mat.m[4], mat.m[5], \
@@ -70,6 +74,19 @@
printf("%s %d %d %d\n",text, (_v).p, (_v).q, (_v).r); \
}
#define DISPLAY_INT32_RATES_AS_FLOAT(text, _r) { \
struct FloatRates _fr; \
RATES_FLOAT_OF_BFP(_fr, (_r)); \
DISPLAY_FLOAT_RATES(text, _fr); \
}
#define DISPLAY_INT32_RATES_AS_FLOAT_DEG(text, _r) { \
struct FloatRates _fr; \
RATES_FLOAT_OF_BFP(_fr, (_r)); \
DISPLAY_FLOAT_RATES_DEG(text, _fr); \
}
#define DISPLAY_INT32_EULERS(text, _e) { \
printf("%s %d %d %d\n",text, (_e).phi, (_e).theta, (_e).psi); \
sw/airborne/test/subsystems/test_ahrs.c
+191
View File
@@ -0,0 +1,191 @@
/*
* $Id$
*
* Copyright (C) 2011 The Paparazzi Team
*
* 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.
*/
#include <inttypes.h>
#include "std.h"
#include "mcu.h"
#include "sys_time.h"
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs.h"
#include "subsystems/ahrs/ahrs_aligner.h"
#include "my_debug_servo.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
static inline void main_report(void);
static inline void on_gyro_event(void);
static inline void on_accel_event(void);
static inline void on_mag_event(void);
int main( void ) {
main_init();
while(1) {
if (sys_time_periodic())
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_init();
imu_init();
ahrs_aligner_init();
ahrs_init();
DEBUG_SERVO1_INIT();
// DEBUG_SERVO2_INIT();
mcu_int_enable();
}
static inline void main_periodic_task( void ) {
if (cpu_time_sec > 1) imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();});
main_report();
}
static inline void main_event_task( void ) {
ImuEvent(on_gyro_event, on_accel_event, on_mag_event);
}
static inline void on_gyro_event(void) {
ImuScaleGyro(imu);
if (ahrs.status == AHRS_UNINIT) {
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
DEBUG_S1_ON();
ahrs_propagate();
DEBUG_S1_OFF();
}
}
static inline void on_accel_event(void) {
ImuScaleAccel(imu);
if (ahrs.status != AHRS_UNINIT) {
DEBUG_S2_ON();
ahrs_update_accel();
DEBUG_S2_OFF();
}
}
static inline void on_mag_event(void) {
ImuScaleMag(imu);
if (ahrs.status == AHRS_RUNNING) {
ahrs_update_mag();
}
}
static inline void main_report(void) {
PeriodicPrescaleBy10(
{
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
&imu.accel_unscaled.x,
&imu.accel_unscaled.y,
&imu.accel_unscaled.z);
},
{
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
&imu.gyro_unscaled.p,
&imu.gyro_unscaled.q,
&imu.gyro_unscaled.r);
},
{
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
&imu.mag_unscaled.x,
&imu.mag_unscaled.y,
&imu.mag_unscaled.z);
},
{
DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
&imu.accel.x,
&imu.accel.y,
&imu.accel.z);
},
{
DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r);
},
{
DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
&imu.mag.x,
&imu.mag.y,
&imu.mag.z);
},
{
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
},
{
#ifdef USE_I2C2
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
&i2c2_errors.ack_fail_cnt,
&i2c2_errors.miss_start_stop_cnt,
&i2c2_errors.arb_lost_cnt,
&i2c2_errors.over_under_cnt,
&i2c2_errors.pec_recep_cnt,
&i2c2_errors.timeout_tlow_cnt,
&i2c2_errors.smbus_alert_cnt,
&i2c2_errors.unexpected_event_cnt,
&i2c2_errors.last_unexpected_event);
#endif
},
{
DOWNLINK_SEND_BOOZ2_AHRS_EULER(DefaultChannel,
&ahrs.ltp_to_imu_euler.phi,
&ahrs.ltp_to_imu_euler.theta,
&ahrs.ltp_to_imu_euler.psi,
&ahrs.ltp_to_body_euler.phi,
&ahrs.ltp_to_body_euler.theta,
&ahrs.ltp_to_body_euler.psi);
},
{
DOWNLINK_SEND_BOOZ_AHRS_BIAS(DefaultChannel,
&ahrs_impl.gyro_bias.p,
&ahrs_impl.gyro_bias.q,
&ahrs_impl.gyro_bias.r);
});
}
sw/airborne/test/subsystems/test_imu.c
+159
View File
@@ -0,0 +1,159 @@
/*
* $Id$
*
* Copyright (C) 2008-2011 The Paparazzi Team
*
* 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.
*/
#include <inttypes.h>
#include "std.h"
#include "mcu.h"
#include "sys_time.h"
#include "led.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "downlink.h"
#include "subsystems/imu.h"
#include "my_debug_servo.h"
static inline void main_init( void );
static inline void main_periodic_task( void );
static inline void main_event_task( void );
static inline void on_gyro_accel_event(void);
static inline void on_accel_event(void);
static inline void on_mag_event(void);
int main( void ) {
main_init();
while(1) {
if (sys_time_periodic())
main_periodic_task();
main_event_task();
}
return 0;
}
static inline void main_init( void ) {
mcu_init();
sys_time_init();
imu_init();
// DEBUG_SERVO1_INIT();
// DEBUG_SERVO2_INIT();
mcu_int_enable();
}
static inline void main_periodic_task( void ) {
RunOnceEvery(100, {
// LED_TOGGLE(3);
DOWNLINK_SEND_ALIVE(DefaultChannel, 16, MD5SUM);
});
#ifdef USE_I2C2
RunOnceEvery(111, {
DOWNLINK_SEND_I2C_ERRORS(DefaultChannel,
&i2c2_errors.ack_fail_cnt,
&i2c2_errors.miss_start_stop_cnt,
&i2c2_errors.arb_lost_cnt,
&i2c2_errors.over_under_cnt,
&i2c2_errors.pec_recep_cnt,
&i2c2_errors.timeout_tlow_cnt,
&i2c2_errors.smbus_alert_cnt,
&i2c2_errors.unexpected_event_cnt,
&i2c2_errors.last_unexpected_event);
});
#endif
if (cpu_time_sec > 1) imu_periodic();
RunOnceEvery(10, { LED_PERIODIC();});
}
static inline void main_event_task( void ) {
ImuEvent(on_gyro_accel_event, on_accel_event, on_mag_event);
}
static inline void on_accel_event(void) {
ImuScaleAccel(imu);
static uint8_t cnt;
cnt++;
if (cnt > 15) cnt = 0;
if (cnt == 0) {
DOWNLINK_SEND_IMU_ACCEL_RAW(DefaultChannel,
&imu.accel_unscaled.x,
&imu.accel_unscaled.y,
&imu.accel_unscaled.z);
}
else if (cnt == 7) {
DOWNLINK_SEND_BOOZ2_ACCEL(DefaultChannel,
&imu.accel.x,
&imu.accel.y,
&imu.accel.z);
}
}
static inline void on_gyro_accel_event(void) {
ImuScaleGyro(imu);
static uint8_t cnt;
cnt++;
if (cnt > 15) cnt = 0;
if (cnt == 0) {
DOWNLINK_SEND_IMU_GYRO_RAW(DefaultChannel,
&imu.gyro_unscaled.p,
&imu.gyro_unscaled.q,
&imu.gyro_unscaled.r);
}
else if (cnt == 7) {
DOWNLINK_SEND_BOOZ2_GYRO(DefaultChannel,
&imu.gyro.p,
&imu.gyro.q,
&imu.gyro.r);
}
}
static inline void on_mag_event(void) {
ImuScaleMag(imu);
static uint8_t cnt;
cnt++;
if (cnt > 10) cnt = 0;
if (cnt == 0) {
DOWNLINK_SEND_BOOZ2_MAG(DefaultChannel,
&imu.mag.x,
&imu.mag.y,
&imu.mag.z);
}
else if (cnt == 5) {
DOWNLINK_SEND_IMU_MAG_RAW(DefaultChannel,
&imu.mag_unscaled.x,
&imu.mag_unscaled.y,
&imu.mag_unscaled.z);
}
}
sw/simulator/nps/nps_random.c
+35 -5
View File
@@ -4,6 +4,8 @@
#include <math.h>
#include <limits.h>
#if 0
/*
* R250
* Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
@@ -26,7 +28,7 @@ static double dr250( void );
static long set_seed(long);
//static long get_seed(void);
static unsigned long int randlcg(void);
#endif
@@ -37,6 +39,20 @@ void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect
vect->z += get_gaussian_noise() * std_dev->z;
}
void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev) {
vect->x += get_gaussian_noise() * std_dev->x;
vect->y += get_gaussian_noise() * std_dev->y;
vect->z += get_gaussian_noise() * std_dev->z;
}
void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev) {
vect->p += get_gaussian_noise() * std_dev->p;
vect->q += get_gaussian_noise() * std_dev->q;
vect->r += get_gaussian_noise() * std_dev->r;
}
void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev) {
vect->x = get_gaussian_noise() * std_dev->x;
vect->y = get_gaussian_noise() * std_dev->y;
@@ -54,11 +70,14 @@ void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3*
VECT3_ADD(*rw, drw);
}
#if 0
/*
http://www.taygeta.com/random/gaussian.html
*/
double get_gaussian_noise(void) {
double x1;
@@ -79,10 +98,20 @@ double get_gaussian_noise(void) {
else
return x2 * w;
}
#else
#include <gsl/gsl_rng.h>
#include <gsl/gsl_randist.h>
#include <stdlib.h>
double get_gaussian_noise(void) {
static gsl_rng * r = NULL;
// select random number generator
if (!r) r = gsl_rng_alloc (gsl_rng_mt19937);
return gsl_ran_gaussian(r, 1.);
}
#endif
#if 0
/*
* R250
* Kirkpatrick, S., and E. Stoll, 1981; "A Very Fast
@@ -242,3 +271,4 @@ unsigned long int randlcg() {
return seed_val;
}
#endif /* 0*/
sw/simulator/nps/nps_random.h
+5
View File
@@ -8,5 +8,10 @@ extern void double_vect3_add_gaussian_noise(struct DoubleVect3* vect, struct Dou
extern void double_vect3_get_gaussian_noise(struct DoubleVect3* vect, struct DoubleVect3* std_dev);
extern void double_vect3_update_random_walk(struct DoubleVect3* rw, struct DoubleVect3* std_dev, double dt, double thau);
extern void float_vect3_add_gaussian_noise(struct FloatVect3* vect, struct FloatVect3* std_dev);
extern void float_rates_add_gaussian_noise(struct FloatRates* vect, struct FloatRates* std_dev);
#endif /* NPS_RANDOM_H */