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

changed variable names

Browse Source
This commit is contained in:
coppolam
2017-10-30 12:39:04 +01:00
parent e25d177469
commit 4184ce3500
1 changed files with 21 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/subsystems/ins/hf_float.c
+21 -21
View File
@@ -65,16 +65,16 @@
/** horizontal filter propagation frequency */ /** horizontal filter propagation frequency */
#define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER) #define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER)
#define DT_HFILTER (1./HFF_FREQ) #define HFF_DT (1./HFF_FREQ)
/** initial covariance diagonal */ /** initial covariance diagonal */
#define INIT_PXX 1. #define HFF_INIT_PXX 1.
/** process noise (is the same for x and y)*/ /** process noise (is the same for x and y)*/
#ifndef HFF_ACCEL_NOISE #ifndef HFF_ACCEL_NOISE
#define HFF_ACCEL_NOISE 0.5 #define HFF_ACCEL_NOISE 0.5
#endif #endif
#define Q HFF_ACCEL_NOISE #define HFF_Q HFF_ACCEL_NOISE
#define Qdotdot HFF_ACCEL_NOISE #define HFF_Qdotdot HFF_ACCEL_NOISE
//TODO: proper measurement noise //TODO: proper measurement noise
#ifndef HFF_R_POS #ifndef HFF_R_POS
@@ -135,7 +135,7 @@ static float hff_y_meas = 0;
static int hff_ps_counter; static int hff_ps_counter;
/* default parameters */ /* default parameters */
#define Qbiasbias 1e-7 #define HFF_Qbiasbias 1e-7
/* /*
* For GPS lag compensation * For GPS lag compensation
@@ -288,7 +288,7 @@ void hff_init(float init_x, float init_xdot, float init_y, float init_ydot)
printf("GPS_LAG: %f\n", GPS_LAG); printf("GPS_LAG: %f\n", GPS_LAG);
printf("GPS_LAG_N: %d\n", GPS_LAG_N); printf("GPS_LAG_N: %d\n", GPS_LAG_N);
printf("GPS_DT_N: %d\n", GPS_DT_N); printf("GPS_DT_N: %d\n", GPS_DT_N);
printf("DT_HFILTER: %f\n", DT_HFILTER); printf("HFF_DT: %f\n", HFF_DT);
printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N); printf("GPS_LAG_TOL_N: %i\n", GPS_LAG_TOL_N);
#endif #endif
#else #else
@@ -328,7 +328,7 @@ static void hff_init_x(float init_x, float init_xdot, float init_xbias)
for (j = 0; j < HFF_STATE_SIZE; j++) { for (j = 0; j < HFF_STATE_SIZE; j++) {
hff.xP[i][j] = 0.; hff.xP[i][j] = 0.;
} }
hff.xP[i][i] = INIT_PXX; hff.xP[i][i] = HFF_INIT_PXX;
} }
} }
@@ -342,7 +342,7 @@ static void hff_init_y(float init_y, float init_ydot, float init_ybias)
for (j = 0; j < HFF_STATE_SIZE; j++) { for (j = 0; j < HFF_STATE_SIZE; j++) {
hff.yP[i][j] = 0.; hff.yP[i][j] = 0.;
} }
hff.yP[i][i] = INIT_PXX; hff.yP[i][i] = HFF_INIT_PXX;
} }
} }
@@ -439,8 +439,8 @@ static void hff_propagate_past(struct HfilterFloat *filt_past)
if (hff_past->lag_counter > 0) { if (hff_past->lag_counter > 0) {
hff_get_past_accel(hff_past->lag_counter); hff_get_past_accel(hff_past->lag_counter);
PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter)); PRINT_DBG(2, ("propagate past: %d\n", hff_past->lag_counter));
hff_propagate_x(hff_past, DT_HFILTER); hff_propagate_x(hff_past, HFF_DT);
hff_propagate_y(hff_past, DT_HFILTER); hff_propagate_y(hff_past, HFF_DT);
hff_past->lag_counter--; hff_past->lag_counter--;
if (past_save_counter > 0) { if (past_save_counter > 0) {
@@ -512,8 +512,8 @@ void hff_propagate(void)
/* /*
* propagate current state * propagate current state
*/ */
hff_propagate_x(&hff, DT_HFILTER); hff_propagate_x(&hff, HFF_DT);
hff_propagate_y(&hff, DT_HFILTER); hff_propagate_y(&hff, HFF_DT);
#ifdef GPS_LAG #ifdef GPS_LAG
/* increase lag counter on last saved state */ /* increase lag counter on last saved state */
@@ -623,9 +623,9 @@ void hff_realign(struct FloatVect2 pos, struct FloatVect2 vel)
* *
* B = [ dt^2/2 dt 0]'; * B = [ dt^2/2 dt 0]';
* *
* Q = [ Q 0 0 * Q = [ HFF_Q 0 0
* 0 Qdotdot 0 * 0 HFF_Qdotdot 0
* 0 0 Qbiasbias ]; * 0 0 HFF_Qbiasbias ];
* *
* Xk1 = F * Xk0 + B * accel; * Xk1 = F * Xk0 + B * accel;
* *
@@ -649,15 +649,15 @@ static void hff_propagate_x(struct HfilterFloat *filt, float dt)
const float FPF21 = filt->xP[2][1] + dt * (-filt->xP[2][2]); const float FPF21 = filt->xP[2][1] + dt * (-filt->xP[2][2]);
const float FPF22 = filt->xP[2][2]; const float FPF22 = filt->xP[2][2];
filt->xP[0][0] = FPF00 + Q * dt * dt / 2.; filt->xP[0][0] = FPF00 + HFF_Q * dt * dt / 2.;
filt->xP[0][1] = FPF01; filt->xP[0][1] = FPF01;
filt->xP[0][2] = FPF02; filt->xP[0][2] = FPF02;
filt->xP[1][0] = FPF10; filt->xP[1][0] = FPF10;
filt->xP[1][1] = FPF11 + Qdotdot * dt; filt->xP[1][1] = FPF11 + HFF_Qdotdot * dt;
filt->xP[1][2] = FPF12; filt->xP[1][2] = FPF12;
filt->xP[2][0] = FPF20; filt->xP[2][0] = FPF20;
filt->xP[2][1] = FPF21; filt->xP[2][1] = FPF21;
filt->xP[2][2] = FPF22 + Qbiasbias; filt->xP[2][2] = FPF22 + HFF_Qbiasbias;
} }
static void hff_propagate_y(struct HfilterFloat *filt, float dt) static void hff_propagate_y(struct HfilterFloat *filt, float dt)
@@ -677,15 +677,15 @@ static void hff_propagate_y(struct HfilterFloat *filt, float dt)
const float FPF21 = filt->yP[2][1] + dt * (-filt->yP[2][2]); const float FPF21 = filt->yP[2][1] + dt * (-filt->yP[2][2]);
const float FPF22 = filt->yP[2][2]; const float FPF22 = filt->yP[2][2];
filt->yP[0][0] = FPF00 + Q * dt * dt / 2.; filt->yP[0][0] = FPF00 + HFF_Q * dt * dt / 2.;
filt->yP[0][1] = FPF01; filt->yP[0][1] = FPF01;
filt->yP[0][2] = FPF02; filt->yP[0][2] = FPF02;
filt->yP[1][0] = FPF10; filt->yP[1][0] = FPF10;
filt->yP[1][1] = FPF11 + Qdotdot * dt; filt->yP[1][1] = FPF11 + HFF_Qdotdot * dt;
filt->yP[1][2] = FPF12; filt->yP[1][2] = FPF12;
filt->yP[2][0] = FPF20; filt->yP[2][0] = FPF20;
filt->yP[2][1] = FPF21; filt->yP[2][1] = FPF21;
filt->yP[2][2] = FPF22 + Qbiasbias; filt->yP[2][2] = FPF22 + HFF_Qbiasbias;
} }