ins_alt_float.c File Reference

#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

GPS_R

#define GPS_R   2.

Definition at line 266 of file ins_alt_float.c.

GPS_SIGMA2

#define GPS_SIGMA2   1.

Definition at line 265 of file ins_alt_float.c.

INS_ALT_BARO_ID

#define INS_ALT_BARO_ID   ABI_BROADCAST

Definition at line 66 of file ins_alt_float.c.

INS_ALT_GPS_ID

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

INS_ALT_IMU_ID

#define INS_ALT_IMU_ID   ABI_BROADCAST

Definition at line 86 of file ins_alt_float.c.

USE_INS_NAV_INIT

#define USE_INS_NAV_INIT   TRUE

Definition at line 49 of file ins_alt_float.c.

Function Documentation

accel_cb()

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:

alt_kalman()

static void alt_kalman ( float  z_meas, float  dt  )

static

Definition at line 283 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, ins_altf, and p.

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:

alt_kalman_init()

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:

alt_kalman_reset()

static void alt_kalman_reset ( void  )

static

Definition at line 270 of file ins_alt_float.c.

References p.

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

Here is the caller graph for this function:

baro_cb()

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:

body_to_imu_cb()

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:

gps_cb()

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:

ins_alt_float_init()

void ins_alt_float_init

(

void

)

Definition at line 101 of file ins_alt_float.c.

References accel_cb(), accel_ev, alt_kalman(), alt_kalman_init(), InsAltFloat::baro_alt, baro_cb(), baro_ev, InsAltFloat::baro_initialized, body_to_imu, body_to_imu_cb(), body_to_imu_ev, f, gps_cb(), gps_ev, ground_alt, INS_ALT_BARO_ID, INS_ALT_GPS_ID, INS_ALT_IMU_ID, ins_altf, nav_utm_east0, nav_utm_north0, nav_utm_zone0, orientationSetEulers_i(), InsAltFloat::origin_initialized, InsAltFloat::qfe, InsAltFloat::reset_alt_ref, stateSetLocalUtmOrigin_f(), and stateSetPositionUtm_f().

Here is the call graph for this function:

ins_alt_float_update_baro()

void ins_alt_float_update_baro

(

float

pressure

)

Definition at line 165 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, get_sys_time_usec(), ground_alt, ins_altf, last_ts, pprz_isa_height_of_pressure(), InsAltFloat::qfe, InsAltFloat::reset_alt_ref, stateGetPositionUtm_f(), stateGetSpeedNed_f(), stateSetPositionUtm_f(), stateSetSpeedNed_f(), and UTM_COPY.

Referenced by baro_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_alt_float_update_gps()

void ins_alt_float_update_gps

(

struct GpsState *

gps_s

)

Definition at line 210 of file ins_alt_float.c.

References InsAltFloat::alt, UtmCoor_f::alt, InsAltFloat::alt_dot, alt_kalman(), alt_kalman_reset(), get_sys_time_usec(), GPS_FIX_3D, ins_altf, ins_reset_local_origin(), last_ts, nav_utm_zone0, InsAltFloat::origin_initialized, InsAltFloat::reset_alt_ref, stateSetPositionUtm_f(), stateSetSpeedNed_f(), utm_float_from_gps(), and NedCoor_f::x.

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_reset_altitude_ref()

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:

ins_reset_local_origin()

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

accel_ev

abi_event accel_ev

static

Definition at line 88 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

baro_ev

abi_event baro_ev

Definition at line 71 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

body_to_imu

struct OrientationReps body_to_imu

static

Definition at line 93 of file ins_alt_float.c.

Referenced by accel_cb(), ahrs_dcm_set_body_to_imu(), ahrs_fc_set_body_to_imu(), ahrs_ice_set_body_to_imu(), ahrs_icq_set_body_to_imu(), ahrs_mlkf_set_body_to_imu(), body_to_imu_cb(), ins_alt_float_init(), nps_sensor_accel_run_step(), nps_sensor_gyro_run_step(), and nps_sensor_mag_run_step().

body_to_imu_ev

abi_event body_to_imu_ev

static

Definition at line 91 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

gps_ev

abi_event gps_ev

static

Definition at line 82 of file ins_alt_float.c.

Referenced by ins_alt_float_init().

ins_altf

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

p

float p[2][2]

static

Definition at line 268 of file ins_alt_float.c.

Referenced by _chvsnprintf(), ads1114_read(), alt_kalman(), alt_kalman_reset(), baro_apply_calibration(), baro_bmp280_event(), baro_bmp3_event(), bef(), bluegiga_broadcast_msg(), bluegiga_ch_available(), bluegiga_init(), bluegiga_load_tx(), bluegiga_transmit(), bmp085_compensated_pressure(), collective_tracking_control(), compensate_pressure(), ctc_target_send_info_to_nei(), cv_blob_locator_func(), dev_char_available(), dev_check_free_space(), dev_get_byte(), dev_put_buffer(), dev_put_byte(), dev_send_message(), distributed_circular(), dqrdc(), fast9_detect(), fast9_detect_pixel(), fit_linear_flow_field(), float_mat_exp(), frsky_x_serial_char_available(), frsky_x_serial_check_free_space(), frsky_x_serial_get_byte(), frsky_x_serial_put_buffer(), frsky_x_serial_put_byte(), frsky_x_serial_send_message(), get_dist2_to_point(), get_tas_factor(), gps_i2c_put_buffer(), grayscale_opencv_to_yuv422(), gvf_ellipse_info(), gvf_intercept_two_lines(), gvf_line_info(), gvf_segment_XY1_XY2(), gvf_sin_info(), handle_spi_thd(), handle_uart_rx(), handle_uart_tx(), htond(), htonf(), i2c_arch_init(), i2c_blocking_receive(), i2c_blocking_transceive(), i2c_blocking_transmit(), i2c_chibios_submit(), i2c_idle(), i2c_init(), i2c_linux_submit(), i2c_lpc21_idle(), i2c_lpc21_setbitrate(), i2c_lpc21_submit(), i2c_receive(), i2c_setbitrate(), i2c_submit(), i2c_thread(), i2c_transceive(), i2c_transmit(), I2cAutomaton(), I2cEndOfTransaction(), I2cFail(), I2cSendStart(), I2cSendStop(), image_labeling(), ins_mekf_wind_get_pos_ned(), ins_mekf_wind_set_pos_ned(), intercept_two_lines(), jpeg_create_svs_header(), logger_uart_periodic(), long_to_string_with_divisor(), ltoa(), mag_calc(), mavlink_send_attitude(), mavlink_send_attitude_quaternion(), mavlink_send_highres_imu(), mf_daq_send_state(), ms5607_calc(), ms5611_calc(), navdata_update(), operator delete(), opticFlowLK(), opticFlowLK_flat(), out_of_segment_area(), parse_mf_daq_msg(), pipe_arch_periph_init(), pipe_char_available(), pipe_check_free_space(), pipe_getch(), pipe_periph_init(), pipe_put_buffer(), pipe_put_byte(), pipe_receive(), pipe_send_message(), pipe_send_raw(), pprz_polyfit_float(), provide_rates(), px4flash_event(), rc_mode_switch(), RotateAndTranslateToWorld(), rtcm3_find_callback(), rtcm3_register_callback(), sdlog_send(), sdlogger_spi_direct_char_available(), sdlogger_spi_direct_check_free_space(), sdlogger_spi_direct_get_byte(), sdlogger_spi_direct_put_buffer(), sdlogger_spi_direct_put_byte(), sdlogger_spi_direct_send_message(), send_tune_roll(), sirf_parse_2(), sirf_parse_41(), softi2c_device_event(), softi2c_idle(), softi2c_spin(), softi2c_submit(), spi_blocking_transceive(), spi_init(), spi_lock(), spi_resume(), spi_slave_hs_transmit_buffer(), spi_slave_init(), spi_slave_register(), spi_slave_wait(), spi_submit(), SpiAutomaton(), SpiClearCPHA(), SpiClearCPOL(), SpiClearRti(), SpiDisable(), SpiDisableRti(), SpiDisableRxi(), SpiDisableTxi(), SpiEnableRti(), SpiEnableRxi(), SpiEnableTxi(), SpiEndOfTransaction(), SpiInitBuf(), SpiRead(), SpiReceive(), SpiSend(), SpiSetCPHA(), SpiSetDataSize(), SpiSlaveAutomaton(), SpiSlaveStart(), SpiStart(), SpiTransmit(), stabilization_attitude_init(), superbitrf_check_free_space(), superbitrf_transmit(), superbitrf_transmit_buffer(), tas_from_eas(), telemetry_intermcu_check_free_space(), telemetry_intermcu_put_buffer(), telemetry_intermcu_put_byte(), telemetry_intermcu_send_message(), teraranger_crc8(), THD_FUNCTION(), TranslateAndRotateFromWorld(), uart_char_available(), uart_check_free_space(), uart_disable_interrupts(), uart_enable_interrupts(), uart_getch(), uart_ISR(), uart_periph_init(), uart_periph_set_baudrate(), uart_periph_set_bits_stop_parity(), uart_periph_set_mode(), uart_put_buffer(), uart_put_byte(), uart_send_message(), uart_set_baudrate(), udp_arch_periph_init(), udp_char_available(), udp_check_free_space(), udp_getch(), udp_periph_init(), udp_put_buffer(), udp_put_byte(), udp_receive(), udp_send_message(), udp_send_raw(), UKF_Wind_Estimator_step(), usage(), usart_isr(), and wls_alloc().