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 29 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.theta, \
&ahrs.ltp_to_imu_euler.psi, \
&face_measure.phi, \
&face_measure.theta, \
&face_measure.psi, \
&face_corrected.phi, \
&face_corrected.theta, \
&face_corrected.psi, \
&face_residual.phi, \
&face_residual.theta, \
&face_residual.psi, \
&face_gyro_bias.p, \
&face_gyro_bias.q, \
&face_gyro_bias.r); \
&ahrs_impl.measure.phi, \
&ahrs_impl.measure.theta, \
&ahrs_impl.measure.psi, \
&ahrs_impl.corrected.phi, \
&ahrs_impl.corrected.theta, \
&ahrs_impl.corrected.psi, \
&ahrs_impl.residual.phi, \
&ahrs_impl.residual.theta, \
&ahrs_impl.residual.psi, \
&ahrs_impl.gyro_bias.p, \
&ahrs_impl.gyro_bias.q, \
&ahrs_impl.gyro_bias.r); \
}
#else
#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
*
@@ -31,15 +31,7 @@
#include "math/pprz_algebra_int.h"
struct Int32Rates face_gyro_bias;
struct Int32Eulers face_measure;
struct Int32Eulers face_residual;
struct Int32Eulers face_uncorrected;
struct Int32Eulers face_corrected;
struct Int32Eulers measurement;
int32_t face_reinj_1;
struct AhrsIntCmplEuler ahrs_impl;
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
@@ -49,10 +41,10 @@ void ahrs_init(void) {
INT32_QUAT_ZERO(ahrs.ltp_to_imu_quat);
INT_RATES_ZERO(ahrs.body_rate);
INT_RATES_ZERO(ahrs.imu_rate);
INT_RATES_ZERO(face_gyro_bias);
face_reinj_1 = FACE_REINJ_1;
INT_RATES_ZERO(ahrs_impl.gyro_bias);
ahrs_impl.reinj_1 = FACE_REINJ_1;
INT_EULERS_ZERO(face_uncorrected);
INT_EULERS_ZERO(ahrs_impl.uncorrected);
#ifdef IMU_MAG_OFFSET
ahrs_mag_offset = IMU_MAG_OFFSET;
@@ -63,7 +55,7 @@ void ahrs_init(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;
}
@@ -97,7 +89,7 @@ void ahrs_propagate(void) {
/* unbias gyro */
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 */
RATES_ADD(ahrs.imu_rate, uf_rate);
RATES_SDIV(ahrs.imu_rate, ahrs.imu_rate, 2);
@@ -105,25 +97,25 @@ void ahrs_propagate(void) {
/* integrate eulers */
struct Int32Eulers euler_dot;
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 */
EULERS_ADD(face_measure, measurement);
EULERS_SDIV(face_measure, face_measure, 2);
EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);
/* compute residual */
EULERS_DIFF(face_residual, face_measure, face_corrected);
INTEG_EULER_NORMALIZE(face_residual.psi);
EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.corrected);
INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, face_residual, face_reinj_1);
EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
/* correct estimation */
EULERS_ADD(face_corrected, correction);
INTEG_EULER_NORMALIZE(face_corrected.psi);
EULERS_ADD(ahrs_impl.corrected, correction);
INTEG_EULER_NORMALIZE(ahrs_impl.corrected.psi);
/* 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 */
INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
/* Compute LTP to IMU rotation matrix */
@@ -143,13 +135,13 @@ void ahrs_propagate(void) {
void ahrs_update_accel(void) {
/* 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;
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_ATAN2(measurement.theta, -cphi_ax, -imu.accel.z);
measurement.phi *= F_UPDATE;
measurement.theta *= F_UPDATE;
INT32_ATAN2(ahrs_impl.measurement.theta, -cphi_ax, -imu.accel.z);
ahrs_impl.measurement.phi *= F_UPDATE;
ahrs_impl.measurement.theta *= F_UPDATE;
}
@@ -183,6 +175,6 @@ void ahrs_update_mag(void) {
// sphi_ctheta * imu.mag.y +
// cphi_ctheta * imu.mag.z;
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$
*
* Copyright (C) 2008-2009 Antoine Drouin <poinix@gmail.com>
* Copyright (C) 2008-2010 The Paparazzi Team
*
* This file is part of paparazzi.
*
@@ -21,20 +21,23 @@
* Boston, MA 02111-1307, USA.
*/
#ifndef AHRS_CMPL_EULER_H
#define AHRS_CMPL_EULER_H
#ifndef AHRS_INT_CMPL_EULER_H
#define AHRS_INT_CMPL_EULER_H
#include "subsystems/ahrs.h"
#include "std.h"
#include "math/pprz_algebra_int.h"
extern struct Int32Rates face_gyro_bias;
extern struct Int32Eulers face_measure;
extern struct Int32Eulers face_residual;
extern struct Int32Eulers face_uncorrected;
extern struct Int32Eulers face_corrected;
struct AhrsIntCmplEuler {
struct Int32Rates gyro_bias;
struct Int32Eulers measure;
struct Int32Eulers residual;
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_CMPL_EULER_H */
#endif /* AHRS_INT_CMPL_EULER_H */