Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
hf_float.c File Reference

Horizontal filter (x,y) to estimate position and velocity. More...

#include "subsystems/ins/hf_float.h"
#include "subsystems/ins.h"
#include "subsystems/imu.h"
#include "state.h"
#include "subsystems/gps.h"
#include <stdlib.h>
#include "generated/airframe.h"
+ Include dependency graph for hf_float.c:

Go to the source code of this file.

Data Structures

struct  AccBuf
 

Macros

#define PRINT_DBG(_l, _p)   {}
 
#define INIT_PXX   1.
 
#define HFF_ACCEL_NOISE   0.5
 
#define Q   HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.
 
#define Qdotdot   HFF_ACCEL_NOISE*DT_HFILTER
 
#define HFF_R_POS   8.
 
#define HFF_R_POS_MIN   3.
 
#define HFF_R_SPEED   2.
 
#define HFF_R_SPEED_MIN   1.
 
#define ACC_RB_MAXN   64
 
#define SAVE_NOW   0
 
#define SAVING   -1
 
#define SAVE_DONE   -2
 

Functions

void b2_hff_store_accel_body (void)
 
static void b2_hff_compute_accel_body_mean (uint8_t n)
 
static void b2_hff_init_x (float init_x, float init_xdot)
 
static void b2_hff_init_y (float init_y, float init_ydot)
 
static void b2_hff_propagate_x (struct HfilterFloat *hff_work)
 
static void b2_hff_propagate_y (struct HfilterFloat *hff_work)
 
static void b2_hff_update_x (struct HfilterFloat *hff_work, float x_meas, float Rpos)
 
static void b2_hff_update_y (struct HfilterFloat *hff_work, float y_meas, float Rpos)
 
static void b2_hff_update_xdot (struct HfilterFloat *hff_work, float vel, float Rvel)
 
static void b2_hff_update_ydot (struct HfilterFloat *hff_work, float vel, float Rvel)
 
void b2_hff_init (float init_x, float init_xdot, float init_y, float init_ydot)
 
void b2_hff_propagate (void)
 
void b2_hff_update_gps (void)
 
void b2_hff_realign (struct FloatVect2 pos, struct FloatVect2 vel)
 
void b2_hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos)
 
void b2_hff_update_vel (struct FloatVect2 vel, struct FloatVect2 Rvel)
 

Variables

float Rgps_pos
 
float Rgps_vel
 
struct HfilterFloat b2_hff_state
 
float b2_hff_xdd_meas
 
float b2_hff_ydd_meas
 
float b2_hff_xd_meas
 
float b2_hff_yd_meas
 
float b2_hff_x_meas
 
float b2_hff_y_meas
 
int b2_hff_ps_counter
 
struct AccBuf acc_body
 
struct Int32Vect3 acc_body_mean
 
struct HfilterFloatb2_hff_rb_last
 
int b2_hff_rb_n
 
int lag_counter_err
 
int save_counter
 
int past_save_counter
 
uint16_t b2_hff_lost_limit
 
uint16_t b2_hff_lost_counter
 

Detailed Description

Horizontal filter (x,y) to estimate position and velocity.

Definition in file hf_float.c.

Macro Definition Documentation

#define ACC_RB_MAXN   64

Definition at line 108 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_ACCEL_NOISE   0.5

Definition at line 55 of file hf_float.c.

#define HFF_R_POS   8.

Definition at line 62 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_POS_MIN   3.

Definition at line 65 of file hf_float.c.

Referenced by b2_hff_update_gps().

#define HFF_R_SPEED   2.

Definition at line 69 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_SPEED_MIN   1.

Definition at line 72 of file hf_float.c.

Referenced by b2_hff_update_gps().

#define INIT_PXX   1.

Definition at line 52 of file hf_float.c.

Referenced by b2_hff_init_x(), and b2_hff_init_y().

#define PRINT_DBG (   _l,
  _p 
)    {}

Definition at line 47 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_update_gps().

Definition at line 57 of file hf_float.c.

Referenced by b2_hff_propagate_x(), b2_hff_propagate_y(), lla_of_ecef_d(), and lla_of_ecef_f().

#define Qdotdot   HFF_ACCEL_NOISE*DT_HFILTER

Definition at line 58 of file hf_float.c.

Referenced by b2_hff_propagate_x(), and b2_hff_propagate_y().

#define SAVE_DONE   -2

Definition at line 214 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_realign().

#define SAVE_NOW   0

Definition at line 212 of file hf_float.c.

#define SAVING   -1

Definition at line 213 of file hf_float.c.

Function Documentation

static void b2_hff_compute_accel_body_mean ( uint8_t  n)
inlinestatic

Definition at line 132 of file hf_float.c.

References acc_body, acc_body_mean, AccBuf::buf, INT_VECT3_ZERO, AccBuf::n, AccBuf::size, VECT3_ADD, VECT3_COPY, VECT3_SDIV, and AccBuf::w.

Referenced by b2_hff_propagate().

+ Here is the caller graph for this function:

void b2_hff_init ( float  init_x,
float  init_xdot,
float  init_y,
float  init_ydot 
)
static void b2_hff_init_x ( float  init_x,
float  init_xdot 
)
inlinestatic

Definition at line 283 of file hf_float.c.

References b2_hff_state, HFF_STATE_SIZE, INIT_PXX, HfilterFloat::x, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by b2_hff_init().

+ Here is the caller graph for this function:

static void b2_hff_init_y ( float  init_y,
float  init_ydot 
)
inlinestatic

Definition at line 295 of file hf_float.c.

References b2_hff_state, HFF_STATE_SIZE, INIT_PXX, HfilterFloat::y, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by b2_hff_init().

+ Here is the caller graph for this function:

static void b2_hff_propagate_x ( struct HfilterFloat hff_work)
inlinestatic

Definition at line 592 of file hf_float.c.

References b2_hff_xdd_meas, DT_HFILTER, Q, Qdotdot, HfilterFloat::x, HfilterFloat::xdot, HfilterFloat::xdotdot, and HfilterFloat::xP.

Referenced by b2_hff_propagate().

+ Here is the caller graph for this function:

static void b2_hff_propagate_y ( struct HfilterFloat hff_work)
inlinestatic

Definition at line 609 of file hf_float.c.

References b2_hff_ydd_meas, DT_HFILTER, Q, Qdotdot, HfilterFloat::y, HfilterFloat::ydot, HfilterFloat::ydotdot, and HfilterFloat::yP.

Referenced by b2_hff_propagate().

+ Here is the caller graph for this function:

void b2_hff_realign ( struct FloatVect2  pos,
struct FloatVect2  vel 
)

Definition at line 558 of file hf_float.c.

References b2_hff_rb_n, b2_hff_state, past_save_counter, save_counter, SAVE_DONE, FloatVect2::x, HfilterFloat::x, HfilterFloat::xdot, FloatVect2::y, HfilterFloat::y, and HfilterFloat::ydot.

Referenced by ins_realign_h(), and ins_update_gps().

+ Here is the caller graph for this function:

void b2_hff_store_accel_body ( void  )

Definition at line 119 of file hf_float.c.

References acc_body, Imu::accel, Imu::body_to_imu_rmat, AccBuf::buf, imu, INT32_RMAT_TRANSP_VMULT, AccBuf::n, AccBuf::r, AccBuf::size, and AccBuf::w.

Referenced by b2_hff_propagate().

+ Here is the caller graph for this function:

void b2_hff_update_pos ( struct FloatVect2  pos,
struct FloatVect2  Rpos 
)

Definition at line 646 of file hf_float.c.

References b2_hff_state, b2_hff_update_x(), b2_hff_update_y(), FloatVect2::x, and FloatVect2::y.

Referenced by track_event().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void b2_hff_update_vel ( struct FloatVect2  vel,
struct FloatVect2  Rvel 
)

Definition at line 717 of file hf_float.c.

References b2_hff_state, b2_hff_update_xdot(), b2_hff_update_ydot(), FloatVect2::x, and FloatVect2::y.

+ Here is the call graph for this function:

static void b2_hff_update_x ( struct HfilterFloat hff_work,
float  x_meas,
float  Rpos 
)
inlinestatic

Definition at line 651 of file hf_float.c.

References b2_hff_x_meas, HfilterFloat::x, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by b2_hff_update_gps(), and b2_hff_update_pos().

+ Here is the caller graph for this function:

static void b2_hff_update_xdot ( struct HfilterFloat hff_work,
float  vel,
float  Rvel 
)
inlinestatic

Definition at line 722 of file hf_float.c.

References b2_hff_xd_meas, HfilterFloat::x, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by b2_hff_update_gps(), and b2_hff_update_vel().

+ Here is the caller graph for this function:

static void b2_hff_update_y ( struct HfilterFloat hff_work,
float  y_meas,
float  Rpos 
)
inlinestatic

Definition at line 673 of file hf_float.c.

References b2_hff_y_meas, HfilterFloat::y, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by b2_hff_update_gps(), and b2_hff_update_pos().

+ Here is the caller graph for this function:

static void b2_hff_update_ydot ( struct HfilterFloat hff_work,
float  vel,
float  Rvel 
)
inlinestatic

Definition at line 744 of file hf_float.c.

References b2_hff_yd_meas, HfilterFloat::y, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by b2_hff_update_gps(), and b2_hff_update_vel().

+ Here is the caller graph for this function:

Variable Documentation

struct AccBuf acc_body
struct Int32Vect3 acc_body_mean

Definition at line 117 of file hf_float.c.

Referenced by b2_hff_compute_accel_body_mean(), and b2_hff_propagate().

uint16_t b2_hff_lost_counter

Definition at line 217 of file hf_float.c.

Referenced by b2_hff_init(), b2_hff_propagate(), b2_hff_update_gps(), and track_event().

uint16_t b2_hff_lost_limit

Definition at line 216 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

int b2_hff_ps_counter

Definition at line 102 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

struct HfilterFloat* b2_hff_rb_last

Definition at line 202 of file hf_float.c.

int b2_hff_rb_n

Definition at line 203 of file hf_float.c.

Referenced by b2_hff_init(), b2_hff_propagate(), b2_hff_realign(), and b2_hff_update_gps().

float b2_hff_x_meas

Definition at line 98 of file hf_float.c.

Referenced by b2_hff_update_x().

float b2_hff_xd_meas

Definition at line 94 of file hf_float.c.

Referenced by b2_hff_update_xdot().

float b2_hff_xdd_meas

Definition at line 90 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_propagate_x().

float b2_hff_y_meas

Definition at line 99 of file hf_float.c.

Referenced by b2_hff_update_y().

float b2_hff_yd_meas

Definition at line 95 of file hf_float.c.

Referenced by b2_hff_update_ydot().

float b2_hff_ydd_meas

Definition at line 91 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_propagate_y().

int lag_counter_err

Definition at line 207 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

int past_save_counter

Definition at line 211 of file hf_float.c.

Referenced by b2_hff_init(), b2_hff_realign(), and b2_hff_update_gps().

float Rgps_pos

Definition at line 76 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

float Rgps_vel

Definition at line 76 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

int save_counter

Definition at line 210 of file hf_float.c.

Referenced by b2_hff_init(), b2_hff_propagate(), b2_hff_realign(), and b2_hff_update_gps().