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

add HIGH_ACCEL flag to pprz dcm filter

Browse Source
This commit is contained in:
Gautier Hattenberger
2011-09-05 11:06:30 +02:00
parent 23a1297ea1
commit d61acb3c1a
1 changed files with 36 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ahrs/ahrs_float_dcm.c
+36
View File
@@ -91,6 +91,18 @@ int renorm_blowup_count = 0;
float imu_health = 0.;
#endif
#ifdef USE_HIGH_ACCEL_FLAG
// High Accel Flag
#define HIGH_ACCEL_LOW_SPEED 15.0
#define HIGH_ACCEL_LOW_SPEED_RESUME 4.0 // Hysteresis
#define HIGH_ACCEL_HIGH_THRUST (0.8*MAX_PPRZ)
#define HIGH_ACCEL_HIGH_THRUST_RESUME (0.1*MAX_PPRZ) // Hysteresis
bool_t high_accel_done;
bool_t high_accel_flag;
// Command vector for thrust (fixed_wing)
#include "inter_mcu.h"
#endif
static inline void set_dcm_matrix_from_rmat(struct FloatRMat *rmat)
{
@@ -175,6 +187,11 @@ void ahrs_init(void) {
/* set inital filter dcm */
set_dcm_matrix_from_rmat(&ahrs_float.ltp_to_imu_rmat);
#ifdef USE_HIGH_ACCEL_FLAG
high_accel_done = FALSE;
high_accel_flag = FALSE;
#endif
}
void ahrs_align(void)
@@ -411,6 +428,25 @@ void Drift_correction(void)
// Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
Accel_weight = Chop(1 - 2*fabs(1 - Accel_magnitude),0,1); //
#ifdef USE_HIGH_ACCEL_FLAG
// Test for high acceleration:
// - low speed
// - high thrust
if (estimator_hspeed_mod < HIGH_ACCEL_LOW_SPEED && ap_state->commands[COMMAND_THROTTLE] > HIGH_ACCEL_HIGH_THRUST && !high_accel_done) {
high_accel_flag = TRUE;
} else {
high_accel_flag = FALSE;
if (estimator_hspeed_mod > HIGH_ACCEL_LOW_SPEED && !high_accel_done) {
high_accel_done = TRUE; // After takeoff, don't use high accel before landing (GS small, Throttle small)
}
if (estimator_hspeed_mod < HIGH_ACCEL_HIGH_THRUST_RESUME && ap_state->commands[COMMAND_THROTTLE] < HIGH_ACCEL_HIGH_THRUST_RESUME) {
high_accel_done = FALSE; // Activate high accel after landing
}
}
if (high_accel_flag) { Accel_weight = 0.; }
#endif
#if PERFORMANCE_REPORTING == 1
{