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
vf_extended_float.c File Reference

Extended vertical filter (in float). More...

#include "subsystems/ins/vf_extended_float.h"
#include "mcu_periph/uart.h"
#include "messages.h"
#include "subsystems/datalink/downlink.h"
+ Include dependency graph for vf_extended_float.c:

Go to the source code of this file.

Macros

#define DEBUG_VFF_EXTENDED   1
 
#define INIT_PXX   1.
 
#define ACCEL_NOISE   0.5
 
#define Qzz   ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.
 
#define Qzdotzdot   ACCEL_NOISE * DT_VFILTER
 
#define Qbiasbias   1e-7
 
#define Qoffoff   1e-4
 
#define R_BARO   1.
 
#define R_ALT   0.1
 
#define R_OFFSET   1.
 

Functions

void vff_init (float init_z, float init_zdot, float init_accel_bias, float init_baro_offset)
 
void vff_propagate (float accel)
 
static void update_baro_conf (float z_meas, float conf)
 
void vff_update_baro (float z_meas)
 
void vff_update_baro_conf (float z_meas, float conf)
 
static void update_alt_conf (float z_meas, float conf)
 
void vff_update_alt (float z_meas)
 
void vff_update_alt_conf (float z_meas, float conf)
 
static void update_offset_conf (float offset, float conf)
 
void vff_update_offset (float offset)
 
void vff_realign (float z_meas)
 

Variables

float vff_z
 
float vff_zdot
 
float vff_bias
 
float vff_offset
 
float vff_zdotdot
 
float vff_P [VFF_STATE_SIZE][VFF_STATE_SIZE]
 
float vff_z_meas
 
float vff_z_meas_baro
 

Detailed Description

Extended vertical filter (in float).

Estimates altitude, vertical speed, accelerometer bias and barometer offset.

Definition in file vf_extended_float.c.

Macro Definition Documentation

#define ACCEL_NOISE   0.5

Definition at line 54 of file vf_extended_float.c.

#define DEBUG_VFF_EXTENDED   1

Definition at line 34 of file vf_extended_float.c.

#define INIT_PXX   1.

Definition at line 52 of file vf_extended_float.c.

Referenced by vff_init().

#define Qbiasbias   1e-7

Definition at line 57 of file vf_extended_float.c.

Referenced by vff_propagate().

#define Qoffoff   1e-4

Definition at line 58 of file vf_extended_float.c.

Referenced by vff_propagate().

#define Qzdotzdot   ACCEL_NOISE * DT_VFILTER

Definition at line 56 of file vf_extended_float.c.

Referenced by vff_propagate().

#define Qzz   ACCEL_NOISE * DT_VFILTER * DT_VFILTER / 2.

Definition at line 55 of file vf_extended_float.c.

Referenced by vff_propagate().

#define R_ALT   0.1

Definition at line 60 of file vf_extended_float.c.

Referenced by vff_update_alt().

#define R_BARO   1.

Definition at line 59 of file vf_extended_float.c.

Referenced by vff_update_baro().

#define R_OFFSET   1.

Definition at line 61 of file vf_extended_float.c.

Referenced by vff_update_offset().

Function Documentation

static void update_alt_conf ( float  z_meas,
float  conf 
)
inlinestatic

Definition at line 216 of file vf_extended_float.c.

References K0, vff_bias, vff_offset, vff_P, vff_z, vff_z_meas, vff_zdot, and FloatVect2::y.

Referenced by vff_update_alt(), and vff_update_alt_conf().

+ Here is the caller graph for this function:

static void update_baro_conf ( float  z_meas,
float  conf 
)
inlinestatic

Definition at line 155 of file vf_extended_float.c.

References K0, vff_bias, vff_offset, vff_P, vff_z, vff_z_meas_baro, vff_zdot, and FloatVect2::y.

Referenced by vff_update_baro(), and vff_update_baro_conf().

+ Here is the caller graph for this function:

static void update_offset_conf ( float  offset,
float  conf 
)
inlinestatic

Definition at line 276 of file vf_extended_float.c.

References K0, vff_bias, vff_offset, vff_P, vff_z, vff_zdot, and FloatVect2::y.

Referenced by vff_update_offset().

+ Here is the caller graph for this function:

void vff_init ( float  init_z,
float  init_zdot,
float  init_accel_bias,
float  init_baro_offset 
)

Definition at line 74 of file vf_extended_float.c.

References INIT_PXX, vff_bias, vff_offset, vff_P, VFF_STATE_SIZE, vff_z, and vff_zdot.

Referenced by ins_init(), and vff_realign().

+ Here is the caller graph for this function:

void vff_propagate ( float  accel)

Definition at line 108 of file vf_extended_float.c.

Referenced by ins_propagate().

+ Here is the caller graph for this function:

void vff_realign ( float  z_meas)

Definition at line 318 of file vf_extended_float.c.

Referenced by ins_realign_v(), and ins_update_baro().

+ Here is the caller graph for this function:

void vff_update_alt ( float  z_meas)

Definition at line 254 of file vf_extended_float.c.

References R_ALT, and update_alt_conf().

+ Here is the call graph for this function:

void vff_update_alt_conf ( float  z_meas,
float  conf 
)

Definition at line 258 of file vf_extended_float.c.

References update_alt_conf().

Referenced by ins_update_sonar().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void vff_update_baro ( float  z_meas)

Definition at line 194 of file vf_extended_float.c.

References R_BARO, and update_baro_conf().

Referenced by ins_update_baro().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void vff_update_baro_conf ( float  z_meas,
float  conf 
)

Definition at line 198 of file vf_extended_float.c.

References update_baro_conf().

+ Here is the call graph for this function:

void vff_update_offset ( float  offset)

Definition at line 313 of file vf_extended_float.c.

References R_OFFSET, and update_offset_conf().

Referenced by ins_update_sonar().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

float vff_bias
float vff_z_meas

Definition at line 71 of file vf_extended_float.c.

Referenced by update_alt_conf(), and vff_propagate().

float vff_z_meas_baro

Definition at line 72 of file vf_extended_float.c.

Referenced by update_baro_conf(), and vff_propagate().

float vff_zdotdot

Definition at line 67 of file vf_extended_float.c.

Referenced by ins_propagate(), ins_update_baro(), and vff_propagate().