[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6341] changed names, removed unused variables
|
From: |
antoine drouin |
|
Subject: |
[paparazzi-commits] [6341] changed names, removed unused variables |
|
Date: |
Thu, 04 Nov 2010 15:53:52 +0000 |
Revision: 6341
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6341
Author: poine
Date: 2010-11-04 15:53:52 +0000 (Thu, 04 Nov 2010)
Log Message:
-----------
changed names, removed unused variables
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-11-04 15:50:58 UTC (rev 6340)
+++ paparazzi3/trunk/sw/airborne/firmwares/rotorcraft/telemetry.h
2010-11-04 15:53:52 UTC (rev 6341)
@@ -334,9 +334,9 @@
&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.hi_res_euler.phi, \
+ &ahrs_impl.hi_res_euler.theta, \
+ &ahrs_impl.hi_res_euler.psi, \
&ahrs_impl.residual.phi, \
&ahrs_impl.residual.theta, \
&ahrs_impl.residual.psi, \
Modified: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
2010-11-04 15:50:58 UTC (rev 6340)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.c
2010-11-04 15:53:52 UTC (rev 6341)
@@ -25,14 +25,27 @@
#include "subsystems/imu.h"
#include "subsystems/ahrs/ahrs_aligner.h"
-
-#include "airframe.h"
#include "math/pprz_trig_int.h"
#include "math/pprz_algebra_int.h"
+#include "airframe.h"
struct AhrsIntCmplEuler ahrs_impl;
+static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas,
int32_t* theta_meas, struct Int32Vect3 accel);
+static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t
phi_est, int32_t theta_est, struct Int32Vect3 mag);
+static inline void compute_imu_quat_and_rmat_from_euler(void);
+static inline void compute_body_orientation(void);
+
+#define F_UPDATE 512
+
+#define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
+#define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
+#define INTEG_EULER_NORMALIZE(_a) { \
+ while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \
+ while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
+ }
+
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
INT_EULERS_ZERO(ahrs.ltp_to_body_euler);
@@ -44,8 +57,6 @@
INT_RATES_ZERO(ahrs_impl.gyro_bias);
ahrs_impl.reinj_1 = FACE_REINJ_1;
- INT_EULERS_ZERO(ahrs_impl.uncorrected);
-
#ifdef IMU_MAG_OFFSET
ahrs_mag_offset = IMU_MAG_OFFSET;
#else
@@ -54,23 +65,28 @@
}
void ahrs_align(void) {
+
+ get_phi_theta_measurement_fom_accel(&ahrs_impl.hi_res_euler.phi,
&ahrs_impl.hi_res_euler.theta, ahrs_aligner.lp_accel);
+ get_psi_measurement_from_mag(&ahrs_impl.hi_res_euler.psi,
+ ahrs_impl.hi_res_euler.phi/F_UPDATE,
ahrs_impl.hi_res_euler.theta/F_UPDATE, ahrs_aligner.lp_mag);
+ EULERS_COPY(ahrs_impl.measure, ahrs_impl.hi_res_euler);
+ EULERS_COPY(ahrs_impl.measurement, ahrs_impl.hi_res_euler);
+
+ /* Compute LTP to IMU eulers */
+ EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
+
+ compute_imu_quat_and_rmat_from_euler();
+
+ compute_body_orientation();
+
RATES_COPY( ahrs_impl.gyro_bias, ahrs_aligner.lp_gyro);
ahrs.status = AHRS_RUNNING;
}
-#define F_UPDATE 512
-#define PI_INTEG_EULER (INT32_ANGLE_PI * F_UPDATE)
-#define TWO_PI_INTEG_EULER (INT32_ANGLE_2_PI * F_UPDATE)
-#define INTEG_EULER_NORMALIZE(_a) { \
- while (_a > PI_INTEG_EULER) _a -= TWO_PI_INTEG_EULER; \
- while (_a < -PI_INTEG_EULER) _a += TWO_PI_INTEG_EULER; \
- }
-
-
/*
*
* fc = 1/(2*pi*tau)
@@ -97,84 +113,107 @@
/* integrate eulers */
struct Int32Eulers euler_dot;
INT32_EULERS_DOT_OF_RATES(euler_dot, ahrs.ltp_to_imu_euler, ahrs.imu_rate);
- EULERS_ADD(ahrs_impl.corrected, euler_dot);
+ EULERS_ADD(ahrs_impl.hi_res_euler, euler_dot);
/* low pass measurement */
EULERS_ADD(ahrs_impl.measure, ahrs_impl.measurement);
EULERS_SDIV(ahrs_impl.measure, ahrs_impl.measure, 2);
+
/* compute residual */
- EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.corrected);
+ EULERS_DIFF(ahrs_impl.residual, ahrs_impl.measure, ahrs_impl.hi_res_euler);
INTEG_EULER_NORMALIZE(ahrs_impl.residual.psi);
struct Int32Eulers correction;
/* compute a correction */
EULERS_SDIV(correction, ahrs_impl.residual, ahrs_impl.reinj_1);
/* correct estimation */
- EULERS_ADD(ahrs_impl.corrected, correction);
- INTEG_EULER_NORMALIZE(ahrs_impl.corrected.psi);
+ EULERS_ADD(ahrs_impl.hi_res_euler, correction);
+ INTEG_EULER_NORMALIZE(ahrs_impl.hi_res_euler.psi);
/* Compute LTP to IMU eulers */
- 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 */
- INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
+ EULERS_SDIV(ahrs.ltp_to_imu_euler, ahrs_impl.hi_res_euler, F_UPDATE);
+
+ compute_imu_quat_and_rmat_from_euler();
- /* 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);
+ compute_body_orientation();
}
void ahrs_update_accel(void) {
- /* build a measurement assuming constant acceleration ?!! */
- INT32_ATAN2(ahrs_impl.measurement.phi, -imu.accel.y, -imu.accel.z);
- int32_t cphi;
- PPRZ_ITRIG_COS(cphi, ahrs_impl.measurement.phi);
- int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, imu.accel.x, INT32_TRIG_FRAC);
- INT32_ATAN2(ahrs_impl.measurement.theta, -cphi_ax, -imu.accel.z);
- ahrs_impl.measurement.phi *= F_UPDATE;
- ahrs_impl.measurement.theta *= F_UPDATE;
+ get_phi_theta_measurement_fom_accel(&ahrs_impl.measurement.phi,
&ahrs_impl.measurement.theta, imu.accel);
}
-/* measure psi assuming magnetic vector is in earth plan (md = 0) */
+
void ahrs_update_mag(void) {
+ get_psi_measurement_from_mag(&ahrs_impl.measurement.psi,
ahrs.ltp_to_imu_euler.phi, ahrs.ltp_to_imu_euler.theta, imu.mag);
+
+}
+
+/* measures phi and theta assuming no dynamic acceleration ?!! */
+static inline void get_phi_theta_measurement_fom_accel(int32_t* phi_meas,
int32_t* theta_meas, struct Int32Vect3 accel) {
+
+ INT32_ATAN2(*phi_meas, -accel.y, -accel.z);
+ int32_t cphi;
+ PPRZ_ITRIG_COS(cphi, *phi_meas);
+ int32_t cphi_ax = -INT_MULT_RSHIFT(cphi, accel.x, INT32_TRIG_FRAC);
+ INT32_ATAN2(*theta_meas, -cphi_ax, -accel.z);
+ *phi_meas *= F_UPDATE;
+ *theta_meas *= F_UPDATE;
+
+}
+
+/* measure psi by projecting magnetic vector in local tangeant plan */
+static inline void get_psi_measurement_from_mag(int32_t* psi_meas, int32_t
phi_est, int32_t theta_est, struct Int32Vect3 mag) {
+
int32_t sphi;
- PPRZ_ITRIG_SIN(sphi, ahrs.ltp_to_imu_euler.phi);
+ PPRZ_ITRIG_SIN(sphi, phi_est);
int32_t cphi;
- PPRZ_ITRIG_COS(cphi, ahrs.ltp_to_imu_euler.phi);
+ PPRZ_ITRIG_COS(cphi, phi_est);
int32_t stheta;
- PPRZ_ITRIG_SIN(stheta, ahrs.ltp_to_imu_euler.theta);
+ PPRZ_ITRIG_SIN(stheta, theta_est);
int32_t ctheta;
- PPRZ_ITRIG_COS(ctheta, ahrs.ltp_to_imu_euler.theta);
+ PPRZ_ITRIG_COS(ctheta, theta_est);
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 * imu.mag.x +
- sphi_stheta * imu.mag.y +
- cphi_stheta * imu.mag.z;
- const int32_t me =
- 0 * imu.mag.x +
- cphi * imu.mag.y +
- -sphi * imu.mag.z;
+ 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);
- ahrs_impl.measurement.psi = ((m_psi -
RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
+ *psi_meas = ((m_psi -
RadOfDeg(ahrs_mag_offset))*(float)(1<<(INT32_ANGLE_FRAC))*F_UPDATE);
}
+
+/* Compute ltp to imu rotation in quaternion and rotation matrice
representation
+ from the euler angle representation */
+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);
+
+}
+
+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);
+
+}
Modified: paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
===================================================================
--- paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
2010-11-04 15:50:58 UTC (rev 6340)
+++ paparazzi3/trunk/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_euler.h
2010-11-04 15:53:52 UTC (rev 6341)
@@ -30,10 +30,9 @@
struct AhrsIntCmplEuler {
struct Int32Rates gyro_bias;
+ struct Int32Eulers hi_res_euler;
struct Int32Eulers measure;
struct Int32Eulers residual;
- struct Int32Eulers uncorrected;
- struct Int32Eulers corrected;
struct Int32Eulers measurement;
int32_t reinj_1;
};
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6341] changed names, removed unused variables,
antoine drouin <=