Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-31 20:38:27 +08:00
Code Issues Actions 30 Packages Projects Releases Wiki Activity

[ahrs] infrared ahrs not using old ahrs structure

Browse Source
This commit is contained in:
Gautier Hattenberger
2012-08-21 15:05:12 +02:00
parent d379af9647
commit 85fc5319c5
2 changed files with 21 additions and 47 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_infrared.c
+21 -44
View File
@@ -28,22 +28,12 @@
#include "state.h" #include "state.h"
#include "estimator.h" // for wind FIXME use state interface #include "estimator.h" // for wind FIXME use state interface
float heading;
void ahrs_init(void) { void ahrs_init(void) {
ahrs.status = AHRS_UNINIT; ahrs.status = AHRS_UNINIT;
/* set ltp_to_body to zero */ heading = 0.;
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_body to zero */
FLOAT_QUAT_ZERO(ahrs_float.ltp_to_imu_quat);
FLOAT_EULERS_ZERO(ahrs_float.ltp_to_imu_euler);
FLOAT_RMAT_ZERO(ahrs_float.ltp_to_imu_rmat);
FLOAT_RATES_ZERO(ahrs_float.imu_rate);
} }
void ahrs_align(void) { void ahrs_align(void) {
@@ -54,15 +44,17 @@ void ahrs_align(void) {
} }
void ahrs_propagate(void) { void ahrs_propagate(void) {
struct FloatRates body_rate = { 0., 0., 0. };
#ifdef ADC_CHANNEL_GYRO_P #ifdef ADC_CHANNEL_GYRO_P
ahrs_float.body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p); body_rate.p = RATE_FLOAT_OF_BFP(imu.gyro.p);
#endif #endif
#ifdef ADC_CHANNEL_GYRO_Q #ifdef ADC_CHANNEL_GYRO_Q
ahrs_float.body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q); body_rate.q = RATE_FLOAT_OF_BFP(imu.gyro.q);
#endif #endif
#ifdef ADC_CHANNEL_GYRO_R #ifdef ADC_CHANNEL_GYRO_R
ahrs_float.body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r); body_rate.r = RATE_FLOAT_OF_BFP(imu.gyro.r);
#endif #endif
stateSetBodyRates_f(&body_rate);
} }
void ahrs_update_accel(void) { void ahrs_update_accel(void) {
@@ -80,42 +72,27 @@ void ahrs_update_gps(void) {
// wind_north and wind_east initialized to 0, so still correct if not updated // wind_north and wind_east initialized to 0, so still correct if not updated
float w_vn = cosf(course_f) * hspeed_mod_f - wind_north; float w_vn = cosf(course_f) * hspeed_mod_f - wind_north;
float w_ve = sinf(course_f) * hspeed_mod_f - wind_east; float w_ve = sinf(course_f) * hspeed_mod_f - wind_east;
ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn); heading = atan2f(w_ve, w_vn);
if (ahrs_float.ltp_to_body_euler.psi < 0.) if (heading < 0.)
ahrs_float.ltp_to_body_euler.psi += 2 * M_PI; heading += 2 * M_PI;
ahrs_update_fw_estimator();
} }
void ahrs_update_infrared(void) { void ahrs_update_infrared(void) {
ahrs_float.ltp_to_body_euler.phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral; float phi = atan2(infrared.roll, infrared.top) - infrared.roll_neutral;
float theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral;
ahrs_float.ltp_to_body_euler.theta = atan2(infrared.pitch, infrared.top) - infrared.pitch_neutral; if (theta < -M_PI_2) theta += M_PI;
else if (theta > M_PI_2) theta -= M_PI;
if (ahrs_float.ltp_to_body_euler.theta < -M_PI_2) if (phi >= 0) phi *= infrared.correction_right;
ahrs_float.ltp_to_body_euler.theta += M_PI; else phi *= infrared.correction_left;
else if (ahrs_float.ltp_to_body_euler.theta > M_PI_2)
ahrs_float.ltp_to_body_euler.theta -= M_PI;
if (ahrs_float.ltp_to_body_euler.phi >= 0) if (theta >= 0) theta *= infrared.correction_up;
ahrs_float.ltp_to_body_euler.phi *= infrared.correction_right; else theta *= infrared.correction_down;
else
ahrs_float.ltp_to_body_euler.phi *= infrared.correction_left;
if (ahrs_float.ltp_to_body_euler.theta >= 0) struct FloatEulers att = { phi, theta, heading };
ahrs_float.ltp_to_body_euler.theta *= infrared.correction_up; stateSetNedToBodyEulers_f(&att);
else
ahrs_float.ltp_to_body_euler.theta *= infrared.correction_down;
ahrs_update_fw_estimator();
}
// TODO use ahrs result directly
void ahrs_update_fw_estimator(void)
{
// export results to estimator
stateSetNedToBodyEulers_f(&ahrs_float.ltp_to_body_euler);
stateSetBodyRates_f(&ahrs_float.body_rate);
} }
sw/airborne/subsystems/ahrs/ahrs_infrared.h
-3
View File
@@ -33,7 +33,4 @@ extern float ins_pitch_neutral;
extern void ahrs_update_infrared(void); extern void ahrs_update_infrared(void);
// TODO copy ahrs to state instead of estimator
extern void ahrs_update_fw_estimator(void);
#endif /* AHRS_INFRARED_H */ #endif /* AHRS_INFRARED_H */