Paparazzi UAS  v5.10_stable-5-g83a0da5-dirty
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces 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/imu.h"
#include "state.h"
#include "subsystems/gps.h"
#include <stdlib.h>
#include "filters/low_pass_filter.h"
#include "generated/airframe.h"
#include "subsystems/datalink/telemetry.h"
+ Include dependency graph for hf_float.c:

Go to the source code of this file.

Macros

#define PRINT_DBG(_l, _p)   {}
 
#define AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY
 
#define HFF_FREQ   ((AHRS_PROPAGATE_FREQUENCY)/HFF_PRESCALER)
 horizontal filter propagation frequency More...
 
#define DT_HFILTER   (1./HFF_FREQ)
 
#define INIT_PXX   1.
 initial covariance diagonal More...
 
#define HFF_ACCEL_NOISE   0.5
 process noise (is the same for x and y) More...
 
#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 HFF_UPDATE_SPEED   TRUE
 
#define HFF_LOWPASS_CUTOFF_FREQUENCY   14
 
#define SAVE_NOW   0
 
#define SAVING   -1
 
#define SAVE_DONE   -2
 
#define HFF_LOST_LIMIT   1000
 

Functions

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, float dt)
 
static void b2_hff_propagate_y (struct HfilterFloat *hff_work, float dt)
 
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)
 
static void send_hff (struct transport_tx *trans, struct link_device *dev)
 
static void send_hff_debug (struct transport_tx *trans, struct link_device *dev)
 
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 (struct FloatVect2 *pos_ned, struct FloatVect2 *speed_ned)
 
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

Butterworth2LowPass_int filter_x
 
Butterworth2LowPass_int filter_y
 
Butterworth2LowPass_int filter_z
 
float Rgps_pos
 
float Rgps_vel
 
struct HfilterFloat b2_hff_state
 
static float b2_hff_xdd_meas
 
static float b2_hff_ydd_meas
 
static float b2_hff_xd_meas
 
static float b2_hff_yd_meas
 
static float b2_hff_x_meas
 
static float b2_hff_y_meas
 
static int b2_hff_ps_counter
 counter for hff propagation More...
 
struct HfilterFloatb2_hff_rb_last
 ringbuffer read pointer More...
 
static int b2_hff_rb_n
 ringbuffer fill count More...
 
static int16_t lag_counter_err
 by how many steps the estimated GPS validity point in time differed from GPS_LAG_N More...
 
static int16_t save_counter
 counts down the propagation steps until the filter state is saved again More...
 
static int past_save_counter
 
static uint16_t b2_hff_lost_limit
 
static 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 AHRS_PROPAGATE_FREQUENCY   PERIODIC_FREQUENCY

Definition at line 51 of file hf_float.c.

Referenced by b2_hff_init(), and gyro_cb().

#define DT_HFILTER   (1./HFF_FREQ)

Definition at line 66 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

#define HFF_ACCEL_NOISE   0.5

process noise (is the same for x and y)

Definition at line 72 of file hf_float.c.

#define HFF_FREQ   ((AHRS_PROPAGATE_FREQUENCY)/HFF_PRESCALER)

horizontal filter propagation frequency

Definition at line 65 of file hf_float.c.

#define HFF_LOST_LIMIT   1000

Definition at line 205 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_LOWPASS_CUTOFF_FREQUENCY   14

Definition at line 97 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_POS   8.

Definition at line 79 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_POS_MIN   3.

Definition at line 82 of file hf_float.c.

Referenced by b2_hff_update_gps().

#define HFF_R_SPEED   2.

Definition at line 86 of file hf_float.c.

Referenced by b2_hff_init().

#define HFF_R_SPEED_MIN   1.

Definition at line 89 of file hf_float.c.

Referenced by b2_hff_update_gps().

#define HFF_UPDATE_SPEED   TRUE

Definition at line 93 of file hf_float.c.

#define INIT_PXX   1.

initial covariance diagonal

Definition at line 69 of file hf_float.c.

Referenced by b2_hff_init_x(), and b2_hff_init_y().

#define PRINT_DBG (   _l,
  _p 
)    {}

Definition at line 46 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_update_gps().

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

Referenced by b2_hff_propagate_x(), and b2_hff_propagate_y().

#define SAVE_DONE   -2

Definition at line 203 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_realign().

#define SAVE_NOW   0

Definition at line 201 of file hf_float.c.

#define SAVING   -1

Definition at line 202 of file hf_float.c.

Function Documentation

static void b2_hff_init_x ( float  init_x,
float  init_xdot 
)
static

Definition at line 317 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 
)
static

Definition at line 330 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,
float  dt 
)
static

Definition at line 629 of file hf_float.c.

References b2_hff_xdd_meas, 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,
float  dt 
)
static

Definition at line 647 of file hf_float.c.

References b2_hff_ydd_meas, 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 594 of file hf_float.c.

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

Referenced by ins_int_update_gps().

+ Here is the caller graph for this function:

void b2_hff_update_pos ( struct FloatVect2  pos,
struct FloatVect2  Rpos 
)

Definition at line 685 of file hf_float.c.

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

+ Here is the call graph for this function:

void b2_hff_update_vel ( struct FloatVect2  vel,
struct FloatVect2  Rvel 
)

Definition at line 757 of file hf_float.c.

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

Referenced by vel_est_cb().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

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

Definition at line 691 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 
)
static

Definition at line 763 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 
)
static

Definition at line 714 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 
)
static

Definition at line 786 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:

static void send_hff ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 232 of file hf_float.c.

References b2_hff_state, HfilterFloat::x, HfilterFloat::xdot, HfilterFloat::xdotdot, HfilterFloat::y, HfilterFloat::ydot, and HfilterFloat::ydotdot.

Referenced by b2_hff_init().

+ Here is the caller graph for this function:

static void send_hff_debug ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Definition at line 243 of file hf_float.c.

References b2_hff_state, b2_hff_x_meas, b2_hff_xd_meas, b2_hff_y_meas, b2_hff_yd_meas, HfilterFloat::xP, and HfilterFloat::yP.

Referenced by b2_hff_init().

+ Here is the caller graph for this function:

Variable Documentation

uint16_t b2_hff_lost_counter
static

Definition at line 207 of file hf_float.c.

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

uint16_t b2_hff_lost_limit
static

Definition at line 206 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

int b2_hff_ps_counter
static

counter for hff propagation

Definition at line 136 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_propagate().

struct HfilterFloat* b2_hff_rb_last

ringbuffer read pointer

Definition at line 191 of file hf_float.c.

int b2_hff_rb_n
static

ringbuffer fill count

Definition at line 192 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
static

Definition at line 132 of file hf_float.c.

Referenced by b2_hff_update_x(), and send_hff_debug().

float b2_hff_xd_meas
static

Definition at line 128 of file hf_float.c.

Referenced by b2_hff_update_xdot(), and send_hff_debug().

float b2_hff_xdd_meas
static

Definition at line 124 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_propagate_x().

float b2_hff_y_meas
static

Definition at line 133 of file hf_float.c.

Referenced by b2_hff_update_y(), and send_hff_debug().

float b2_hff_yd_meas
static

Definition at line 129 of file hf_float.c.

Referenced by b2_hff_update_ydot(), and send_hff_debug().

float b2_hff_ydd_meas
static

Definition at line 125 of file hf_float.c.

Referenced by b2_hff_propagate(), and b2_hff_propagate_y().

Definition at line 105 of file hf_float.c.

Definition at line 106 of file hf_float.c.

Definition at line 107 of file hf_float.c.

int16_t lag_counter_err
static

by how many steps the estimated GPS validity point in time differed from GPS_LAG_N

Definition at line 196 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

int past_save_counter
static

Definition at line 200 of file hf_float.c.

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

float Rgps_pos

Definition at line 110 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

float Rgps_vel

Definition at line 110 of file hf_float.c.

Referenced by b2_hff_init(), and b2_hff_update_gps().

int16_t save_counter
static

counts down the propagation steps until the filter state is saved again

Definition at line 199 of file hf_float.c.

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