[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6297] grouped the ahrs variables into one structure
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [6297] grouped the ahrs variables into one structure |
|
Date: |
Thu, 28 Oct 2010 11:38:14 +0000 |
Revision: 6297
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6297
Author: poine
Date: 2010-10-28 11:38:14 +0000 (Thu, 28 Oct 2010)
Log Message:
-----------
grouped the ahrs variables into one structure
Modified Paths:
--------------
paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
Modified: paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-10-28 10:44:34 UTC (rev 6296)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-10-28 11:38:14 UTC (rev 6297)
@@ -331,18 +331,18 @@
&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) {}
Modified: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
2010-10-28 10:44:34 UTC (rev 6296)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
2010-10-28 11:38:14 UTC (rev 6297)
@@ -1,5 +1,5 @@
/*
- * $Id: $
+ * $Id$
*
* Copyright (C) 2008-2010 The Paparazzi Team
*
@@ -31,16 +31,8 @@
#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 AhrsIntCmplEuler ahrs_impl;
-struct Int32Eulers measurement;
-
-int32_t face_reinj_1;
-
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
@@ -49,10 +41,10 @@
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_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 @@
/* 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 @@
/* 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_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 @@
// 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);
}
Modified: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
2010-10-28 10:44:34 UTC (rev 6296)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
2010-10-28 11:38:14 UTC (rev 6297)
@@ -1,7 +1,7 @@
/*
* $Id$
*
- * Copyright (C) 2008-2009 Antoine Drouin <address@hidden>
+ * 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 */
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6297] grouped the ahrs variables into one structure,
antoine drouin <=