Paparazzi UAS  v5.15_devel-99-g2ff7410
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
ins_alt_float.c File Reference

Filters altitude and climb rate for fixedwings. More...

#include "subsystems/ins/ins_alt_float.h"
#include "subsystems/abi.h"
#include "state.h"
#include <inttypes.h>
#include <math.h>
#include "mcu_periph/sys_time.h"
#include "subsystems/gps.h"
#include "firmwares/fixedwing/nav.h"
#include "generated/airframe.h"
#include "generated/modules.h"
#include "subsystems/sensors/baro.h"
#include "math/pprz_isa.h"
+ Include dependency graph for ins_alt_float.c:

Go to the source code of this file.

Macros

#define USE_INS_NAV_INIT   TRUE
 
#define INS_ALT_BARO_ID   ABI_BROADCAST
 
#define INS_ALT_GPS_ID   GPS_MULTI_ID
 ABI binding for gps data. More...
 
#define INS_ALT_IMU_ID   ABI_BROADCAST
 
#define GPS_SIGMA2   1.
 
#define GPS_R   2.
 

Functions

static void baro_cb (uint8_t sender_id, uint32_t stamp, float pressure)
 
static void gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
 
static void accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
 
static void body_to_imu_cb (uint8_t sender_id, struct FloatQuat *q_b2i_f)
 
static void alt_kalman_reset (void)
 
static void alt_kalman_init (void)
 
static void alt_kalman (float z_meas, float dt)
 
void ins_alt_float_update_gps (struct GpsState *gps_s)
 
void ins_alt_float_init (void)
 
void ins_reset_local_origin (void)
 Reset the geographic reference to the current GPS fix. More...
 
void ins_reset_altitude_ref (void)
 INS altitude reference reset. More...
 
void ins_alt_float_update_baro (float pressure)
 

Variables

struct InsAltFloat ins_altf
 
abi_event baro_ev
 
static abi_event gps_ev
 
static abi_event accel_ev
 
static abi_event body_to_imu_ev
 
static struct OrientationReps body_to_imu
 
static float p [2][2]
 

Detailed Description

Filters altitude and climb rate for fixedwings.

Definition in file ins_alt_float.c.

Macro Definition Documentation

#define GPS_R   2.

Definition at line 266 of file ins_alt_float.c.

Referenced by alt_kalman().

#define GPS_SIGMA2   1.

Definition at line 265 of file ins_alt_float.c.

Referenced by alt_kalman().

#define INS_ALT_BARO_ID   ABI_BROADCAST

Definition at line 66 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

#define INS_ALT_GPS_ID   GPS_MULTI_ID

ABI binding for gps data.

Used for GPS ABI messages.

Definition at line 79 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

#define INS_ALT_IMU_ID   ABI_BROADCAST

Definition at line 86 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

#define USE_INS_NAV_INIT   TRUE

Definition at line 49 of file ins_alt_float.c.

Function Documentation

static void accel_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct Int32Vect3 accel 
)
static

Definition at line 371 of file ins_alt_float.c.

References ACCEL_BFP_OF_REAL, body_to_imu, int32_rmat_transp_vmult(), orientationGetRMat_i(), stateGetNedToBodyRMat_i(), stateSetAccelNed_i(), and Int32Vect3::z.

Referenced by ins_alt_float_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void alt_kalman_init ( void  )
static

Definition at line 278 of file ins_alt_float.c.

References alt_kalman_reset().

Referenced by ins_alt_float_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void alt_kalman_reset ( void  )
static

Definition at line 270 of file ins_alt_float.c.

Referenced by alt_kalman_init(), ins_alt_float_update_baro(), and ins_alt_float_update_gps().

+ Here is the caller graph for this function:

static void baro_cb ( uint8_t  sender_id,
uint32_t  stamp,
float  pressure 
)
static

Definition at line 358 of file ins_alt_float.c.

References ins_alt_float_update_baro().

Referenced by ins_alt_float_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void body_to_imu_cb ( uint8_t  sender_id,
struct FloatQuat q_b2i_f 
)
static

Definition at line 385 of file ins_alt_float.c.

References body_to_imu, and orientationSetQuat_f().

Referenced by ins_alt_float_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

static void gps_cb ( uint8_t  sender_id,
uint32_t  stamp,
struct GpsState gps_s 
)
static

Definition at line 364 of file ins_alt_float.c.

References ins_alt_float_update_gps().

Referenced by ins_alt_float_init().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

void ins_alt_float_update_gps ( struct GpsState gps_s)
void ins_reset_altitude_ref ( void  )

INS altitude reference reset.

Reset only vertical reference to the current altitude. Does nothing if not implemented by specific INS algorithm.

Definition at line 153 of file ins_alt_float.c.

References UtmCoor_f::alt, gps, GpsState::hmsl, ins_altf, InsAltFloat::reset_alt_ref, state, stateSetLocalUtmOrigin_f(), and State::utm_origin_f.

+ Here is the call graph for this function:

void ins_reset_local_origin ( void  )

Reset the geographic reference to the current GPS fix.

INS local origin reset.

Definition at line 139 of file ins_alt_float.c.

References gps, ins_altf, InsAltFloat::origin_initialized, InsAltFloat::reset_alt_ref, stateSetLocalUtmOrigin_f(), and utm_float_from_gps().

Referenced by ins_alt_float_update_gps().

+ Here is the call graph for this function:

+ Here is the caller graph for this function:

Variable Documentation

abi_event accel_ev
static

Definition at line 88 of file ins_alt_float.c.

abi_event baro_ev

Definition at line 71 of file ins_alt_float.c.

struct OrientationReps body_to_imu
static

Definition at line 93 of file ins_alt_float.c.

Referenced by accel_cb(), body_to_imu_cb(), and ins_alt_float_init().

abi_event body_to_imu_ev
static

Definition at line 91 of file ins_alt_float.c.

abi_event gps_ev
static

Definition at line 82 of file ins_alt_float.c.