Paparazzi UAS  v4.0.4_stable-3-gf39211a
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
hf_float.c File Reference
#include "subsystems/ins/hf_float.h"
#include "subsystems/ins.h"
#include "subsystems/imu.h"
#include "subsystems/ahrs.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
 

Macro Definition Documentation

#define ACC_RB_MAXN   64

Definition at line 103 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_ACCEL_NOISE   0.5

Definition at line 50 of file hf_float.c.

#define HFF_R_POS   8.

Definition at line 57 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_POS_MIN   3.

Definition at line 60 of file hf_float.c.

Referenced by b2_hff_update_gps().

#define HFF_R_SPEED   2.

Definition at line 64 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_SPEED_MIN   1.

Definition at line 67 of file hf_float.c.

Referenced by b2_hff_update_gps().

#define INIT_PXX   1.

Definition at line 47 of file hf_float.c.

Referenced by b2_hff_init_x(), and b2_hff_init_y().

#define PRINT_DBG (   _l,
  _p 
)    {}

Definition at line 42 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_update_gps().

Definition at line 52 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 53 of file hf_float.c.

Referenced by b2_hff_propagate_x(), and b2_hff_propagate_y().

#define SAVE_DONE   -2

Definition at line 209 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_realign().

#define SAVE_NOW   0

Definition at line 207 of file hf_float.c.

#define SAVING   -1

Definition at line 208 of file hf_float.c.

Function Documentation

static void b2_hff_compute_accel_body_mean ( uint8_t  n)
inlinestatic

Definition at line 127 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 278 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 290 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 586 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 603 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 552 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_update_gps().

+ Here is the caller graph for this function:

void b2_hff_store_accel_body ( void  )

Definition at line 114 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 640 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 711 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 645 of file hf_float.c.

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

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 716 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 667 of file hf_float.c.

References b2_hff_y_meas, HfilterFloat::y, Int32Vect3::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 738 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 112 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 212 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 211 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

int b2_hff_ps_counter

Definition at line 97 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

struct HfilterFloat* b2_hff_rb_last

Definition at line 197 of file hf_float.c.

int b2_hff_rb_n

Definition at line 198 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 93 of file hf_float.c.

Referenced by b2_hff_update_x().

float b2_hff_xd_meas

Definition at line 89 of file hf_float.c.

Referenced by b2_hff_update_xdot().

float b2_hff_xdd_meas

Definition at line 85 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_propagate_x().

float b2_hff_y_meas

Definition at line 94 of file hf_float.c.

Referenced by b2_hff_update_y().

float b2_hff_yd_meas

Definition at line 90 of file hf_float.c.

Referenced by b2_hff_update_ydot().

float b2_hff_ydd_meas

Definition at line 86 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_propagate_y().

int lag_counter_err

Definition at line 202 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

int past_save_counter

Definition at line 206 of file hf_float.c.

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

float Rgps_pos

Definition at line 71 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

float Rgps_vel

Definition at line 71 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

int save_counter

Definition at line 205 of file hf_float.c.

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