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 HfilterFloat *	b2_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().

#define Q   HFF_ACCEL_NOISE*DT_HFILTER*DT_HFILTER/2.

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

void b2_hff_init	(	float	init_x,
		float	init_xdot,
		float	init_y,
		float	init_ydot
	)

Definition at line 268 of file hf_float.c.

References AHRS_PROPAGATE_FREQUENCY, b2_hff_init_x(), b2_hff_init_y(), b2_hff_lost_counter, b2_hff_lost_limit, b2_hff_ps_counter, b2_hff_rb_n, b2_hff_state, DefaultPeriodic, DT_HFILTER, HFF_LOST_LIMIT, HFF_LOWPASS_CUTOFF_FREQUENCY, HFF_R_POS, HFF_R_SPEED, init_butterworth_2_low_pass_int(), HfilterFloat::lag_counter, lag_counter_err, past_save_counter, register_periodic_telemetry(), Rgps_pos, Rgps_vel, HfilterFloat::rollback, save_counter, SAVE_DONE, send_hff(), and send_hff_debug().

Referenced by ins_int_init().

Here is the call graph for this function:

Here is the caller graph for this function:

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:

void b2_hff_propagate

(

void

)

Definition at line 470 of file hf_float.c.

References Imu::accel, ACCEL_FLOAT_OF_BFP, b2_hff_lost_counter, b2_hff_lost_limit, b2_hff_propagate_x(), b2_hff_propagate_y(), b2_hff_ps_counter, b2_hff_rb_n, b2_hff_state, b2_hff_xdd_meas, b2_hff_ydd_meas, Imu::body_to_imu, DT_HFILTER, imu, int32_rmat_transp_vmult(), HfilterFloat::lag_counter, orientationGetRMat_i(), PRINT_DBG, HfilterFloat::rollback, save_counter, stateGetNedToBodyRMat_i(), update_butterworth_2_low_pass_int(), Int32Vect3::x, Int32Vect3::y, and Int32Vect3::z.

Referenced by ins_int_propagate().

Here is the call graph for this function:

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_gps	(	struct FloatVect2 *	pos_ned,
		struct FloatVect2 *	speed_ned
	)

Definition at line 532 of file hf_float.c.

References b2_hff_lost_counter, b2_hff_rb_n, b2_hff_state, b2_hff_update_gps(), b2_hff_update_x(), b2_hff_update_xdot(), b2_hff_update_y(), b2_hff_update_ydot(), gps, HFF_R_POS_MIN, HFF_R_SPEED_MIN, if(), HfilterFloat::lag_counter, lag_counter_err, GpsState::pacc, past_save_counter, PRINT_DBG, Rgps_pos, Rgps_vel, HfilterFloat::rollback, GpsState::sacc, save_counter, FloatVect2::x, and FloatVect2::y.

Referenced by b2_hff_update_gps(), and ins_int_update_gps().

Here is the call graph for this function:

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().

struct HfilterFloat b2_hff_state

Definition at line 120 of file hf_float.c.

Referenced by b2_hff_init(), b2_hff_init_x(), b2_hff_init_y(), b2_hff_propagate(), b2_hff_realign(), b2_hff_update_gps(), b2_hff_update_pos(), b2_hff_update_vel(), send_hff(), and send_hff_debug().

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().

Butterworth2LowPass_int filter_x

Definition at line 105 of file hf_float.c.

Butterworth2LowPass_int filter_y

Definition at line 106 of file hf_float.c.

Butterworth2LowPass_int filter_z

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().