Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-05-24 13:55:51 +08:00
Code Issues Actions 28 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 "estimator.h" // for wind FIXME use state interface
float heading;
void ahrs_init(void) {
ahrs.status = AHRS_UNINIT;
/* set ltp_to_body to zero */
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);
heading = 0.;
}
void ahrs_align(void) {
@@ -54,15 +44,17 @@ void ahrs_align(void) {
}
void ahrs_propagate(void) {
struct FloatRates body_rate = { 0., 0., 0. };
#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
#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
#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
stateSetBodyRates_f(&body_rate);
}
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
float w_vn = cosf(course_f) * hspeed_mod_f - wind_north;
float w_ve = sinf(course_f) * hspeed_mod_f - wind_east;
ahrs_float.ltp_to_body_euler.psi = atan2f(w_ve, w_vn);
if (ahrs_float.ltp_to_body_euler.psi < 0.)
ahrs_float.ltp_to_body_euler.psi += 2 * M_PI;
heading = atan2f(w_ve, w_vn);
if (heading < 0.)
heading += 2 * M_PI;
ahrs_update_fw_estimator();
}
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)
ahrs_float.ltp_to_body_euler.theta += M_PI;
else if (ahrs_float.ltp_to_body_euler.theta > M_PI_2)
ahrs_float.ltp_to_body_euler.theta -= M_PI;
if (phi >= 0) phi *= infrared.correction_right;
else phi *= infrared.correction_left;
if (ahrs_float.ltp_to_body_euler.phi >= 0)
ahrs_float.ltp_to_body_euler.phi *= infrared.correction_right;
else
ahrs_float.ltp_to_body_euler.phi *= infrared.correction_left;
if (theta >= 0) theta *= infrared.correction_up;
else theta *= infrared.correction_down;
if (ahrs_float.ltp_to_body_euler.theta >= 0)
ahrs_float.ltp_to_body_euler.theta *= infrared.correction_up;
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);
struct FloatEulers att = { phi, theta, heading };
stateSetNedToBodyEulers_f(&att);
}
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);
// TODO copy ahrs to state instead of estimator
extern void ahrs_update_fw_estimator(void);
#endif /* AHRS_INFRARED_H */