Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-01 21:07:40 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

grouped the ahrs variables into one structure

Browse Source
This commit is contained in:
Antoine Drouin
2010-10-28 11:38:14 +00:00
parent 354e5d89fb
commit ed8397780d
3 changed files with 48 additions and 53 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/firmwares/rotorcraft/telemetry.h
+12 -12
View File
@@ -331,18 +331,18 @@ extern uint8_t telemetry_mode_Main_DefaultChannel;
&ahrs.ltp_to_imu_euler.phi, \ &ahrs.ltp_to_imu_euler.phi, \
&ahrs.ltp_to_imu_euler.theta, \ &ahrs.ltp_to_imu_euler.theta, \
&ahrs.ltp_to_imu_euler.psi, \ &ahrs.ltp_to_imu_euler.psi, \
&face_measure.phi, \ &ahrs_impl.measure.phi, \
&face_measure.theta, \ &ahrs_impl.measure.theta, \
&face_measure.psi, \ &ahrs_impl.measure.psi, \
&face_corrected.phi, \ &ahrs_impl.corrected.phi, \
&face_corrected.theta, \ &ahrs_impl.corrected.theta, \
&face_corrected.psi, \ &ahrs_impl.corrected.psi, \
&face_residual.phi, \ &ahrs_impl.residual.phi, \
&face_residual.theta, \ &ahrs_impl.residual.theta, \
&face_residual.psi, \ &ahrs_impl.residual.psi, \
&face_gyro_bias.p, \ &ahrs_impl.gyro_bias.p, \
&face_gyro_bias.q, \ &ahrs_impl.gyro_bias.q, \
&face_gyro_bias.r); \ &ahrs_impl.gyro_bias.r); \
} }
#else #else
#define PERIODIC_SEND_FILTER(_chan) {} #define PERIODIC_SEND_FILTER(_chan) {}
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
+22 -30
View File
@@ -1,5 +1,5 @@
/* /*
* $Id: $ * $Id$
* *
* Copyright (C) 2008-2010 The Paparazzi Team * Copyright (C) 2008-2010 The Paparazzi Team
* *
@@ -31,15 +31,7 @@
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
struct Int32Rates face_gyro_bias; struct AhrsIntCmplEuler ahrs_impl;
struct Int32Eulers face_measure;
struct Int32Eulers face_residual;
struct Int32Eulers face_uncorrected;
struct Int32Eulers face_corrected;
struct Int32Eulers measurement;
int32_t face_reinj_1;
void ahrs_init(void) { void ahrs_init(void) {
ahrs.status = AHRS_UNINIT; ahrs.status = AHRS_UNINIT;
@@ -49,10 +41,10 @@ void ahrs_init(void) {
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat); INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate); INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate); INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(face_gyro_bias); INT_RATES_ZERO(ahrs_impl.gyro_bias);
face_reinj_1 = FACE_REINJ_1; ahrs_impl.reinj_1 = FACE_REINJ_1;
INT_EULERS_ZERO(face_uncorrected); INT_EULERS_ZERO(ahrs_impl.uncorrected);
#ifdef IMU_MAG_OFFSET #ifdef IMU_MAG_OFFSET
ahrs_mag_offset = IMU_MAG_OFFSET; ahrs_mag_offset = IMU_MAG_OFFSET;
@@ -63,7 +55,7 @@ void ahrs_init(void) {
void ahrs_align(void) { void ahrs_align(void) {
RATES_COPY( face_gyro_bias, ahrs_aligner.lp_gyro); RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
ahrs.status = AHRS_RUNNING; ahrs.status = AHRS_RUNNING;
} }
@@ -97,7 +89,7 @@ void ahrs_propagate(void) {
/* unbias gyro */ /* unbias gyro */
struct Int32Rates uf_rate; struct Int32Rates uf_rate;
RATES_DIFF(uf_rate, imu.gyro, face_gyro_bias); RATES_DIFF(uf_rate, imu.gyro, ahrs_impl.gyro_bias);
/* low pass rate */ /* low pass rate */
RATES_ADD(ahrs.imu_rate, uf_rate); RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2); RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
@@ -105,25 +97,25 @@ void ahrs_propagate(void) {
/* integrate eulers */ /* integrate eulers */
struct Int32Eulers euler_dot; struct Int32Eulers euler_dot;
INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate); INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
EULERS_ADD(face_corrected, euler_dot); EULERS_ADD(ahrs_impl.corrected, euler_dot);
/* low pass measurement */ /* low pass measurement */
EULERS_ADD(face_measure, measurement); EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
EULERS_SDIV(face_measure, face_measure, 2); EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);
/* compute residual */ /* compute residual */
EULERS_DIFF(face_residual, face_measure, face_corrected); EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.corrected);
INTEG_EULER_NORMALIZE(face_residual.psi); INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);
struct Int32Eulers correction; struct Int32Eulers correction;
/* compute a correction */ /* compute a correction */
EULERS_SDIV(correction, face_residual, face_reinj_1); EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
/* correct estimation */ /* correct estimation */
EULERS_ADD(face_corrected, correction); EULERS_ADD(ahrs_impl.corrected, correction);
INTEG_EULER_NORMALIZE(face_corrected.psi); INTEG_EULER_NORMALIZE(ahrs_impl.corrected.psi);
/* Compute LTP to IMU eulers */ /* Compute LTP to IMU eulers */
EULERS_SDIV(ahrs.ltp_to_imu_euler, face_corrected, F_UPDATE); EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.corrected, F_UPDATE);
/* Compute LTP to IMU quaternion */ /* Compute LTP to IMU quaternion */
INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler); INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
/* Compute LTP to IMU rotation matrix */ /* Compute LTP to IMU rotation matrix */
@@ -143,13 +135,13 @@ void ahrs_propagate(void) {
void ahrs_update_accel(void) { void ahrs_update_accel(void) {
/* build a measurement assuming constant acceleration ?!! */ /* build a measurement assuming constant acceleration ?!! */
INT32_ATAN2(measurement.phi, -imu.accel.y, -imu.accel.z); INT32_ATAN2(ahrs_impl.measurement.phi, -imu.accel.y, -imu.accel.z);
int32_t cphi; int32_t cphi;
PPRZ_ITRIG_COS(cphi, measurement.phi); PPRZ_ITRIG_COS(cphi, ahrs_impl.measurement.phi);
int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, imu.accel.x, INT32_TRIG_FRAC); int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, imu.accel.x, INT32_TRIG_FRAC);
INT32_ATAN2(measurement.theta, -cphi_ax, -imu.accel.z); INT32_ATAN2(ahrs_impl.measurement.theta, -cphi_ax, -imu.accel.z);
measurement.phi *= F_UPDATE; ahrs_impl.measurement.phi *= F_UPDATE;
measurement.theta *= F_UPDATE; ahrs_impl.measurement.theta *= F_UPDATE;
} }
@@ -183,6 +175,6 @@ void ahrs_update_mag(void) {
// sphi_ctheta * imu.mag.y + // sphi_ctheta * imu.mag.y +
// cphi_ctheta * imu.mag.z; // cphi_ctheta * imu.mag.z;
float m_psi = -atan2(me, mn); float m_psi = -atan2(me, mn);
measurement.psi = ((m_psi - RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE); ahrs_impl.measurement.psi = ((m_psi - RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
} }
sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
+14 -11
View File
@@ -1,7 +1,7 @@
/* /*
* $Id$ * $Id$
* *
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com> * Copyright (C) 2008-2010 The Paparazzi Team
* *
* This file is part of paparazzi. * This file is part of paparazzi.
* *
@@ -21,20 +21,23 @@
* Boston, MA 02111-1307, USA. * Boston, MA 02111-1307, USA.
*/ */
#ifndef AHRS_CMPL_EULER_H #ifndef AHRS_INT_CMPL_EULER_H
#define AHRS_CMPL_EULER_H #define AHRS_INT_CMPL_EULER_H
#include "subsystems/ahrs.h" #include "subsystems/ahrs.h"
#include "std.h" #include "std.h"
#include "math/pprz_algebra_int.h" #include "math/pprz_algebra_int.h"
extern struct Int32Rates face_gyro_bias; struct AhrsIntCmplEuler {
extern struct Int32Eulers face_measure; struct Int32Rates gyro_bias;
extern struct Int32Eulers face_residual; struct Int32Eulers measure;
extern struct Int32Eulers face_uncorrected; struct Int32Eulers residual;
extern struct Int32Eulers face_corrected; struct Int32Eulers uncorrected;
struct Int32Eulers corrected;
struct Int32Eulers measurement;
int32_t reinj_1;
};
extern int32_t face_reinj_1; extern struct AhrsIntCmplEuler ahrs_impl;
#endif /* AHRS_INT_CMPL_EULER_H */
#endif /* AHRS_CMPL_EULER_H */