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	INS_BARO_ID   ABI_BROADCAST
#define	INS_ALT_IMU_ID   ABI_BROADCAST
#define	GPS_SIGMA2   1.
#define	GPS_R   2.

Functions
static void	baro_cb (uint8_t sender_id, float pressure)
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)
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)
void	ins_altf_register (void)

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 247 of file ins_alt_float.c.

Referenced by alt_kalman().

#define GPS_SIGMA2   1.

Definition at line 246 of file ins_alt_float.c.

Referenced by alt_kalman().

#define INS_ALT_IMU_ID   ABI_BROADCAST

Definition at line 79 of file ins_alt_float.c.

Referenced by ins_altf_register().

#define INS_BARO_ID   ABI_BROADCAST

Definition at line 66 of file ins_alt_float.c.

Referenced by ins_altf_register().

Function Documentation

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

static

Definition at line 352 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_altf_register().

Here is the call graph for this function:

Here is the caller graph for this function:

static void alt_kalman ( float  z_meas, float  dt  )

static

Definition at line 264 of file ins_alt_float.c.

References InsAltFloat::alt, alt_baro_enabled, InsAltFloat::alt_dot, baro_amsys_enabled, baro_amsys_r, baro_amsys_sigma2, baro_bmp_enabled, baro_bmp_r, baro_bmp_sigma2, baro_ets_enabled, baro_ets_r, baro_ets_sigma2, baro_MS5534A_r, baro_MS5534A_sigma2, baro_ms5611_enabled, baro_ms5611_r, baro_ms5611_sigma2, DefaultChannel, DefaultDevice, GPS_R, GPS_SIGMA2, and ins_altf.

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

Here is the caller graph for this function:

static void alt_kalman_init ( void  )

static

Definition at line 259 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 251 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, float  pressure  )

static

Definition at line 339 of file ins_alt_float.c.

References ins_alt_float_update_baro().

Referenced by ins_altf_register().

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 366 of file ins_alt_float.c.

References body_to_imu, and orientationSetQuat_f().

Referenced by ins_altf_register().

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 345 of file ins_alt_float.c.

References ins_alt_float_update_gps().

Referenced by ins_altf_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_alt_float_init

(

void

)

Definition at line 88 of file ins_alt_float.c.

References alt_kalman(), alt_kalman_init(), InsAltFloat::baro_alt, InsAltFloat::baro_initialized, body_to_imu, FALSE, ground_alt, ins_altf, nav_utm_east0, nav_utm_north0, nav_utm_zone0, orientationSetEulers_i(), InsAltFloat::qfe, InsAltFloat::reset_alt_ref, stateSetLocalUtmOrigin_f(), and stateSetPositionUtm_f().

Referenced by ins_altf_register().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_alt_float_update_baro

(

float

pressure

)

Definition at line 150 of file ins_alt_float.c.

References InsAltFloat::alt, UtmCoor_f::alt, InsAltFloat::alt_dot, alt_kalman(), alt_kalman_reset(), InsAltFloat::baro_alt, InsAltFloat::baro_initialized, FALSE, get_sys_time_usec(), ground_alt, ins_altf, pprz_isa_height_of_pressure(), InsAltFloat::qfe, InsAltFloat::reset_alt_ref, stateGetPositionUtm_f(), stateGetSpeedNed_f(), stateSetPositionUtm_f(), stateSetSpeedNed_f(), TRUE, and UTM_COPY.

Referenced by baro_cb().

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

)

Definition at line 195 of file ins_alt_float.c.

References InsAltFloat::alt, UtmCoor_f::alt, InsAltFloat::alt_dot, alt_kalman(), alt_kalman_reset(), UtmCoor_f::east, UtmCoor_i::east, FALSE, get_sys_time_usec(), GpsState::hmsl, ins_altf, nav_utm_zone0, GpsState::ned_vel, UtmCoor_f::north, UtmCoor_i::north, InsAltFloat::reset_alt_ref, stateSetPositionUtm_f(), stateSetSpeedNed_f(), GpsState::utm_pos, NedCoor_i::x, NedCoor_i::y, NedCoor_i::z, and UtmCoor_f::zone.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

void ins_altf_register

(

void

)

Definition at line 372 of file ins_alt_float.c.

References ABI_BROADCAST, accel_cb(), baro_cb(), body_to_imu_cb(), gps_cb(), ins_alt_float_init(), INS_ALT_IMU_ID, INS_BARO_ID, and ins_register_impl().

Here is the call graph for this function:

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 138 of file ins_alt_float.c.

Referenced by nav_reset_alt().

Here is the caller 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 114 of file ins_alt_float.c.

Referenced by nav_reset_reference().

Here is the caller graph for this function:

Variable Documentation

abi_event accel_ev

static

Definition at line 75 of file ins_alt_float.c.

abi_event baro_ev

Definition at line 70 of file ins_alt_float.c.

struct OrientationReps body_to_imu

static

Definition at line 77 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 76 of file ins_alt_float.c.

abi_event gps_ev

static

Definition at line 74 of file ins_alt_float.c.

struct InsAltFloat ins_altf

Definition at line 53 of file ins_alt_float.c.

Referenced by alt_kalman(), ins_alt_float_init(), ins_alt_float_update_baro(), ins_alt_float_update_gps(), ins_reset_altitude_ref(), and ins_reset_local_origin().

float p[2][2]

static

Definition at line 249 of file ins_alt_float.c.

Referenced by baro_apply_calibration(), bef(), bmp085_compensated_pressure(), cv_blob_locator_func(), fast9_detect(), fit_linear_flow_field(), image_labeling(), jpeg_create_svs_header(), logger_uart_periodic(), mag_calc(), mavlink_send_attitude(), mavlink_send_attitude_quaternion(), mf_daq_send_state(), ms5607_calc(), ms5611_calc(), navdata_update(), opticFlowLK(), parse_mf_daq_msg(), sirf_parse_2(), sirf_parse_41(), stabilization_attitude_init(), and video_thread_periodic().