Paparazzi UAS
v5.12_stable-4-g9b43e9b
Paparazzi is a free software Unmanned Aircraft System.
|
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"
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, 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] |
Filters altitude and climb rate for fixedwings.
Definition in file ins_alt_float.c.
#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.
|
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().
|
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, and ins_altf.
Referenced by ins_alt_float_init(), ins_alt_float_update_baro(), and ins_alt_float_update_gps().
|
static |
Definition at line 278 of file ins_alt_float.c.
References alt_kalman_reset().
Referenced by ins_alt_float_init().
|
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().
|
static |
Definition at line 358 of file ins_alt_float.c.
References ins_alt_float_update_baro().
Referenced by ins_alt_float_init().
Definition at line 385 of file ins_alt_float.c.
References body_to_imu, and orientationSetQuat_f().
Referenced by ins_alt_float_init().
Definition at line 364 of file ins_alt_float.c.
References ins_alt_float_update_gps().
Referenced by ins_alt_float_init().
void ins_alt_float_init | ( | void | ) |
Definition at line 101 of file ins_alt_float.c.
References accel_cb(), alt_kalman(), alt_kalman_init(), InsAltFloat::baro_alt, baro_cb(), InsAltFloat::baro_initialized, body_to_imu, body_to_imu_cb(), gps_cb(), 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().
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().
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().
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.
Referenced by nav_reset_alt().
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().
|
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.
|
static |
Definition at line 93 of file ins_alt_float.c.
Referenced by accel_cb(), body_to_imu_cb(), and ins_alt_float_init().
|
static |
Definition at line 91 of file ins_alt_float.c.
|
static |
Definition at line 82 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().
|
static |
Definition at line 268 of file ins_alt_float.c.
Referenced by _chvsnprintf(), baro_apply_calibration(), bef(), bmp085_compensated_pressure(), coloryuv_opencv_to_yuv422(), crc_packet(), cv_blob_locator_func(), dqrdc(), fast9_detect(), fit_linear_flow_field(), grayscale_opencv_to_yuv422(), gvf_ellipse_info(), gvf_line_info(), gvf_segment_XY1_XY2(), gvf_sin_info(), htond(), htonf(), image_labeling(), jpeg_create_svs_header(), logger_uart_periodic(), long_to_string_with_divisor(), mag_calc(), mavlink_send_attitude(), mavlink_send_attitude_quaternion(), mf_daq_send_state(), ms5607_calc(), ms5611_calc(), navdata_update(), opticFlowLK(), opticFlowLK_flat(), out_of_segment_area(), parse_mf_daq_msg(), px4flash_event(), rc_mode_switch(), rtcm3_find_callback(), rtcm3_register_callback(), send_tune_roll(), sirf_parse_2(), sirf_parse_41(), spi_slave_hs_transmit_buffer(), stabilization_attitude_init(), THD_FUNCTION(), UKF_Wind_Estimator_step(), and wls_alloc().