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

Faster Aligner

Browse Source
This commit is contained in:
Christophe De Wagter
2010-12-22 12:09:38 +01:00
parent 85220cee54
commit 2dd622d1d5
4 changed files with 34 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
conf/settings/tuning_ins.xml
+3 -1
View File
@@ -32,7 +32,9 @@
<dl_settings NAME="control">
<dl_settings NAME="ins">
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="imu_overrun" module="subsystems/imu" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_health" shortname="health" module="subsystems/ahrs" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="renorm_sqrt_count" shortname="err_norm" module="subsystems/ahrs" />
<dl_setting MAX="100" MIN="0" STEP="1" VAR="imu_overrun_error" shortname="err_adc" module="subsystems/imu" />
<dl_setting MAX="0.3" MIN="-0.3" STEP="0.01" VAR="ins_roll_neutral" shortname="roll_neutral" module="subsystems/ahrs" param="INS_ROLL_NEUTRAL_DEFAULT" unit="rad"/>
<dl_setting MAX="0.5" MIN="-0.3" STEP="0.01" VAR="ins_pitch_neutral" shortname="pitch_neutral" param="INS_PITCH_NEUTRAL_DEFAULT" unit="rad"/>
</dl_settings>
sw/airborne/firmwares/fixedwing/main_ap.c
+9 -11
View File
@@ -669,6 +669,15 @@ static inline void on_gyro_accel_event( void ) {
LED_ON(AHRS_CPU_LED);
#endif
// Run aligner on raw data as it also makes averages.
if (ahrs.status == AHRS_UNINIT) {
ImuScaleGyro(imu);
ImuScaleAccel(imu);
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
return;
}
gyr_avg.x += imu.gyro_unscaled.p;
@@ -692,15 +701,6 @@ static inline void on_gyro_accel_event( void ) {
ImuScaleGyro(imu);
if (ahrs.status == AHRS_UNINIT) {
ImuScaleAccel(imu);
ahrs_aligner_run();
if (ahrs_aligner.status == AHRS_ALIGNER_LOCKED)
ahrs_align();
}
else {
ahrs_propagate();
_reduced_correction_rate++;
@@ -714,8 +714,6 @@ static inline void on_gyro_accel_event( void ) {
ahrs_update_accel();
ahrs_update_fw_estimator();
}
}
}
#ifdef AHRS_CPU_LED
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+10 -1
View File
@@ -50,7 +50,7 @@ float ins_pitch_neutral = INS_PITCH_NEUTRAL_DEFAULT;
// Positive yaw : clockwise
// DCM Working variables
float G_Dt=1. / ((float) AHRS_PROPAGATE_FREQUENCY );
const float G_Dt = 1. / ((float) AHRS_PROPAGATE_FREQUENCY );
struct FloatVect3 accel_float = {0,0,0};
@@ -72,6 +72,13 @@ void Normalize(void);
void Drift_correction(void);
void Matrix_update(void);
#if PERFORMANCE_REPORTING == 1
int renorm_sqrt_count = 0;
int renorm_blowup_count = 0;
float imu_health = 0.;
#endif
/**************************************************/
void ahrs_update_fw_estimator( void )
@@ -96,6 +103,8 @@ void ahrs_update_fw_estimator( void )
estimator_theta = ahrs_float.ltp_to_imu_euler.theta - ins_pitch_neutral;
estimator_psi = ahrs_float.ltp_to_imu_euler.psi;
estimator_p = Omega_Vector[0];
// estimator_p =
}
sw/airborne/subsystems/ahrs/ahrs_float_dcm.h
+1 -1
View File
@@ -59,7 +59,7 @@ void ahrs_update_fw_estimator(void);
// Mode 2 = direct accelerometer -> euler
#define PERFORMANCE_REPORTING 0
#define PERFORMANCE_REPORTING 1
#if PERFORMANCE_REPORTING == 1
extern int renorm_sqrt_count;
extern int renorm_blowup_count;