#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:

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	HFF_DT (1./HFF_FREQ)

#define	HFF_INIT_PXX 1.
	initial covariance diagonal More...

#define	HFF_ACCEL_NOISE 0.5
	process noise (is the same for x and y) More...

#define	HFF_Q HFF_ACCEL_NOISE

#define	HFF_Qdotdot HFF_ACCEL_NOISE

#define	HFF_R_POS 8.

#define	HFF_R_POS_MIN 3.

#define	HFF_R_GPS_SPEED 2.

#define	HFF_R_GPS_SPEED_MIN 0.25

#define	HFF_UPDATE_GPS_SPEED TRUE

#define	HFF_LOWPASS_CUTOFF_FREQUENCY 14

#define	HFF_Qbiasbias 1e-7

#define	SAVE_NOW 0

#define	SAVING -1

#define	SAVE_DONE -2

#define	HFF_LOST_LIMIT 1000

Functions
static void	hff_init_x (float init_x, float init_xdot, float init_xbias)

static void	hff_init_y (float init_y, float init_ydot, float init_ybias)

static void	hff_propagate_x (struct HfilterFloat *filt, float dt)
	Propagate the filter in time. More...

static void	hff_propagate_y (struct HfilterFloat *filt, float dt)

static void	hff_update_x (struct HfilterFloat *filt, float x_meas, float Rpos)

static void	hff_update_y (struct HfilterFloat *filt, float y_meas, float Rpos)

static void	hff_update_xdot (struct HfilterFloat *filt, float vel, float Rvel)

static void	hff_update_ydot (struct HfilterFloat *filt, 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	hff_init (float init_x, float init_xdot, float init_y, float init_ydot)

void	hff_propagate (void)

void	hff_update_gps (struct FloatVect2 pos_ned, struct FloatVect2 speed_ned)

void	hff_realign (struct FloatVect2 pos, struct FloatVect2 vel)

void	hff_update_pos (struct FloatVect2 pos, struct FloatVect2 Rpos)
	Update position. More...

void	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	hff

static float	hff_xdd_meas = 0

static float	hff_ydd_meas = 0

static float	hff_xd_meas = 0

static float	hff_yd_meas = 0

static float	hff_x_meas = 0

static float	hff_y_meas = 0

static int	hff_ps_counter
	counter for hff propagation More...

struct HfilterFloat *	hff_rb_last
	ringbuffer read pointer More...

static int	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	hff_lost_limit

static uint16_t	hff_lost_counter

static uint16_t	hff_speed_lost_counter

Detailed Description

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

Definition in file hf_float.c.

Macro Definition Documentation

◆ AHRS_PROPAGATE_FREQUENCY

#define AHRS_PROPAGATE_FREQUENCY PERIODIC_FREQUENCY

Definition at line 55 of file hf_float.c.

◆ HFF_ACCEL_NOISE

#define HFF_ACCEL_NOISE 0.5

process noise (is the same for x and y)

Definition at line 78 of file hf_float.c.

◆ HFF_DT

#define HFF_DT (1./HFF_FREQ)

Definition at line 72 of file hf_float.c.

◆ HFF_FREQ

#define HFF_FREQ (AHRS_PROPAGATE_FREQUENCY / HFF_PRESCALER)

horizontal filter propagation frequency

Definition at line 71 of file hf_float.c.

◆ HFF_INIT_PXX

#define HFF_INIT_PXX 1.

initial covariance diagonal

Definition at line 75 of file hf_float.c.

◆ HFF_LOST_LIMIT

#define HFF_LOST_LIMIT 1000

Definition at line 209 of file hf_float.c.

◆ HFF_LOWPASS_CUTOFF_FREQUENCY

#define HFF_LOWPASS_CUTOFF_FREQUENCY 14

Definition at line 103 of file hf_float.c.

◆ HFF_Q

#define HFF_Q HFF_ACCEL_NOISE

Definition at line 80 of file hf_float.c.

◆ HFF_Qbiasbias

#define HFF_Qbiasbias 1e-7

Definition at line 142 of file hf_float.c.

◆ HFF_Qdotdot

#define HFF_Qdotdot HFF_ACCEL_NOISE

Definition at line 81 of file hf_float.c.

◆ HFF_R_GPS_SPEED

#define HFF_R_GPS_SPEED 2.

Definition at line 92 of file hf_float.c.

◆ HFF_R_GPS_SPEED_MIN

#define HFF_R_GPS_SPEED_MIN 0.25

Definition at line 95 of file hf_float.c.

◆ HFF_R_POS

#define HFF_R_POS 8.

Definition at line 85 of file hf_float.c.

◆ HFF_R_POS_MIN

#define HFF_R_POS_MIN 3.

Definition at line 88 of file hf_float.c.

◆ HFF_UPDATE_GPS_SPEED

#define HFF_UPDATE_GPS_SPEED TRUE

Definition at line 99 of file hf_float.c.

◆ PRINT_DBG

#define PRINT_DBG	(	_l,
		_p
	)	{}

Definition at line 50 of file hf_float.c.

◆ SAVE_DONE

#define SAVE_DONE -2

Definition at line 207 of file hf_float.c.

◆ SAVE_NOW

#define SAVE_NOW 0

Definition at line 205 of file hf_float.c.

◆ SAVING

#define SAVING -1

Definition at line 206 of file hf_float.c.

Function Documentation

◆ hff_init()

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

Definition at line 274 of file hf_float.c.

References AHRS_PROPAGATE_FREQUENCY, DefaultPeriodic, f, filter_x, filter_y, filter_z, hff, HFF_DT, hff_init_x(), hff_init_y(), hff_lost_counter, HFF_LOST_LIMIT, hff_lost_limit, HFF_LOWPASS_CUTOFF_FREQUENCY, hff_ps_counter, HFF_R_GPS_SPEED, HFF_R_POS, hff_rb_last, hff_rb_n, hff_speed_lost_counter, 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:

◆ hff_init_x()

static void hff_init_x	(	float	init_x,
		float	init_xdot,
		float	init_xbias
	)

static

Definition at line 324 of file hf_float.c.

References hff, HFF_INIT_PXX, HFF_STATE_SIZE, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, and HfilterFloat::xP.

Referenced by hff_init().

Here is the caller graph for this function:

◆ hff_init_y()

static void hff_init_y	(	float	init_y,
		float	init_ydot,
		float	init_ybias
	)

static

Definition at line 338 of file hf_float.c.

References hff, HFF_INIT_PXX, HFF_STATE_SIZE, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, and HfilterFloat::yP.

Referenced by hff_init().

Here is the caller graph for this function:

◆ hff_propagate()

void hff_propagate ( void )

Definition at line 479 of file hf_float.c.

References ACCEL_FLOAT_OF_BFP, filter_x, filter_y, filter_z, hff, HFF_DT, hff_lost_counter, hff_lost_limit, hff_propagate_x(), hff_propagate_y(), hff_ps_counter, hff_rb_last, hff_rb_n, hff_speed_lost_counter, hff_xdd_meas, hff_ydd_meas, int32_rmat_transp_vmult(), HfilterFloat::lag_counter, PRINT_DBG, HfilterFloat::rollback, save_counter, stateGetAccelBody_i(), 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:

◆ hff_propagate_x()

static void hff_propagate_x	(	struct HfilterFloat *	filt,
		float	dt
	)

static

Propagate the filter in time.

F = [ 1 dt -dt^2/2 0 1 -dt 0 0 1 ];

B = [ dt^2/2 dt 0]';

Q = [ HFF_Q 0 0 0 HFF_Qdotdot 0 0 0 HFF_Qbiasbias ];

Xk1 = F * Xk0 + B * accel;

Pk1 = F * Pk0 * F' + Q;

Definition at line 638 of file hf_float.c.

References HFF_Q, HFF_Qbiasbias, HFF_Qdotdot, hff_xdd_meas, HfilterFloat::x, HfilterFloat::xbias, HfilterFloat::xdot, HfilterFloat::xdotdot, and HfilterFloat::xP.

Referenced by hff_propagate().

Here is the caller graph for this function:

◆ hff_propagate_y()

static void hff_propagate_y	(	struct HfilterFloat *	filt,
		float	dt
	)

static

Definition at line 666 of file hf_float.c.

References HFF_Q, HFF_Qbiasbias, HFF_Qdotdot, hff_ydd_meas, HfilterFloat::y, HfilterFloat::ybias, HfilterFloat::ydot, HfilterFloat::ydotdot, and HfilterFloat::yP.

Referenced by hff_propagate().

Here is the caller graph for this function:

◆ hff_realign()

void hff_realign	(	struct FloatVect2	pos,
		struct FloatVect2	vel
	)

Definition at line 602 of file hf_float.c.

References hff, hff_rb_n, past_save_counter, save_counter, SAVE_DONE, HfilterFloat::x, FloatVect2::x, HfilterFloat::xbias, HfilterFloat::xdot, HfilterFloat::y, FloatVect2::y, HfilterFloat::ybias, and HfilterFloat::ydot.

Referenced by ins_int_update_gps().

Here is the caller graph for this function:

◆ hff_update_gps()

void hff_update_gps	(	struct FloatVect2 *	pos_ned,
		struct FloatVect2 *	speed_ned
	)

Definition at line 541 of file hf_float.c.

References gps, hff, hff_lost_counter, HFF_R_GPS_SPEED_MIN, HFF_R_POS_MIN, hff_rb_last, hff_rb_n, hff_update_gps(), hff_update_x(), hff_update_xdot(), hff_update_y(), hff_update_ydot(), if(), HfilterFloat::lag_counter, lag_counter_err, GpsState::pacc, past_save_counter, PRINT_DBG, Rgps_pos, Rgps_vel, HfilterFloat::rollback, GpsState::sacc, save_counter, HfilterFloat::x, FloatVect2::x, HfilterFloat::y, and FloatVect2::y.

Referenced by hff_update_gps(), and ins_int_update_gps().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ hff_update_pos()

void hff_update_pos	(	struct FloatVect2	pos,
		struct FloatVect2	Rpos
	)

Update position.

H = [1 0 0]; R = 0.1; // state residual y = rangemeter - H * Xm; // covariance residual S = H*Pm*H' + R; // kalman gain K = Pm*H'*inv(S); // update state Xp = Xm + K*y; // update covariance Pp = Pm - K*H*Pm;

Definition at line 711 of file hf_float.c.

References hff, hff_lost_counter, hff_update_x(), hff_update_y(), FloatVect2::x, and FloatVect2::y.

Referenced by pos_est_cb().

Here is the call graph for this function: