ins_flow.h File Reference

#include "modules/ahrs/ahrs.h"
#include "modules/ahrs/ahrs_int_cmpl_quat.h"
#include "modules/ahrs/ahrs_aligner.h"
#include "modules/ins/ins.h"

Include dependency graph for ins_flow.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Macros
#define	CONSTANT_ALT_FILTER   1
#define	OF_DRAG   1
#define	OF_TWO_DIM   1
#define	OF_THRUST_BIAS   0
#define	OF_USE_GYROS   1
#define	PREDICT_GYROS   0
#define	N_STATES_OF_KF   6
#define	OF_V_IND   0
#define	OF_ANGLE_IND   1
#define	OF_Z_IND   2
#define	OF_THETA_IND   3
#define	OF_VX_IND   4
#define	OF_ANGLE_DOT_IND   5
#define	OF_THRUST_BIAS_IND   -1
#define	N_MEAS_OF_KF   4
#define	OF_Z_DOT_IND   -1
#define	OF_TB_Q   0.02
#define	OF_TB_P   0.5
#define	OF_LAT_FLOW_IND   0
#define	OF_DIV_FLOW_IND   1
#define	OF_RATE_IND   2
#define	OF_LAT_FLOW_X_IND   3
#define	USE_ANGLE   1
#define	USE_VELOCITY   2
#define	USE_HEIGHT   3

Functions
void	ins_flow_init (void)
void	ins_flow_update (void)

Variables
float	OF_X [N_STATES_OF_KF]
bool	reset_filter
bool	run_filter
int	use_filter
float	thrust_factor
float	GT_phi
float	GT_theta

Macro Definition Documentation

CONSTANT_ALT_FILTER

#define CONSTANT_ALT_FILTER   1

Definition at line 35 of file ins_flow.h.

N_MEAS_OF_KF

#define N_MEAS_OF_KF   4

Definition at line 90 of file ins_flow.h.

N_STATES_OF_KF

#define N_STATES_OF_KF   6

Definition at line 75 of file ins_flow.h.

OF_ANGLE_DOT_IND

#define OF_ANGLE_DOT_IND   5

Definition at line 81 of file ins_flow.h.

OF_ANGLE_IND

#define OF_ANGLE_IND   1

Definition at line 77 of file ins_flow.h.

OF_DIV_FLOW_IND

#define OF_DIV_FLOW_IND   1

Definition at line 133 of file ins_flow.h.

OF_DRAG

#define OF_DRAG   1

Definition at line 36 of file ins_flow.h.

OF_LAT_FLOW_IND

#define OF_LAT_FLOW_IND   0

Definition at line 132 of file ins_flow.h.

OF_LAT_FLOW_X_IND

#define OF_LAT_FLOW_X_IND   3

Definition at line 136 of file ins_flow.h.

OF_RATE_IND

#define OF_RATE_IND   2

Definition at line 134 of file ins_flow.h.

OF_TB_P

#define OF_TB_P   0.5

Definition at line 129 of file ins_flow.h.

OF_TB_Q

#define OF_TB_Q   0.02

Definition at line 128 of file ins_flow.h.

OF_THETA_IND

#define OF_THETA_IND   3

Definition at line 79 of file ins_flow.h.

OF_THRUST_BIAS

#define OF_THRUST_BIAS   0

Definition at line 40 of file ins_flow.h.

OF_THRUST_BIAS_IND

#define OF_THRUST_BIAS_IND   -1

Definition at line 85 of file ins_flow.h.

OF_TWO_DIM

#define OF_TWO_DIM   1

Definition at line 38 of file ins_flow.h.

OF_USE_GYROS

#define OF_USE_GYROS   1

Definition at line 42 of file ins_flow.h.

OF_V_IND

#define OF_V_IND   0

Definition at line 76 of file ins_flow.h.

OF_VX_IND

#define OF_VX_IND   4

Definition at line 80 of file ins_flow.h.

OF_Z_DOT_IND

#define OF_Z_DOT_IND   -1

Definition at line 97 of file ins_flow.h.

OF_Z_IND

#define OF_Z_IND   2

Definition at line 78 of file ins_flow.h.

PREDICT_GYROS

#define PREDICT_GYROS   0

Definition at line 44 of file ins_flow.h.

USE_ANGLE

#define USE_ANGLE   1

Definition at line 143 of file ins_flow.h.

USE_HEIGHT

#define USE_HEIGHT   3

Definition at line 145 of file ins_flow.h.

USE_VELOCITY

#define USE_VELOCITY   2

Definition at line 144 of file ins_flow.h.

Function Documentation

ins_flow_init()

void ins_flow_init

(

void

)

Definition at line 532 of file ins_flow.c.

References ABI_BROADCAST, accel_cb(), accel_ev, ahrs_aligner_init(), ahrs_icq_init(), aligner_cb(), aligner_ev, LlaCoor_i::alt, CONSTANT_ALT_FILTER, DefaultPeriodic, E, ecef_of_lla_i(), get_sys_time_float(), gps_cb(), gps_ev, GT_phi, GT_theta, gyro_cb(), gyro_ev, LtpDef_i::hmsl, ins_act_feedback_cb(), ins_act_feedback_ev, ins_flow, INS_FLOW_ACCEL_ID, INS_FLOW_GPS_ID, INS_FLOW_GYRO_ID, INS_FLOW_IMU_ID, ins_optical_flow_cb(), ins_optical_flow_ev, INS_OPTICAL_FLOW_ID, ins_reset_filter(), INS_RPM_ID, LlaCoor_i::lat, LlaCoor_i::lon, lp_factor, lp_factor_strong, InsFlow::lp_gyro_bias_pitch, InsFlow::lp_gyro_bias_roll, InsFlow::lp_gyro_pitch, InsFlow::lp_gyro_roll, InsFlow::lp_roll_command, InsFlow::lp_thrust, ltp_def, InsFlow::ltp_def, ltp_def_from_ecef_i(), InsFlow::ltp_initialized, moment_ind, N_MEAS_OF_KF, InsFlow::new_flow_measurement, OF_ANGLE_DOT_IND, OF_ANGLE_IND, OF_DIV_FLOW_IND, OF_LAT_FLOW_IND, OF_LAT_FLOW_X_IND, of_prev_time, OF_Q, OF_R, OF_RATE_IND, OF_THETA_IND, OF_THRUST_BIAS, OF_THRUST_BIAS_IND, of_time, OF_TWO_DIM, OF_V_IND, OF_VX_IND, OF_Z_DOT_IND, OF_Z_IND, PAR_K0, PAR_K1, PAR_K2, PAR_K3, PAR_Q0, PAR_Q1, PAR_Q2, PAR_Q3, PAR_Q4, PAR_Q_TB, PAR_R0, PAR_R1, parameters, register_periodic_telemetry(), reset_cb(), reset_ev, reset_filter, RPM_FACTORS, run_filter, send_bias(), send_euler(), send_filter_status(), send_geo_mag(), send_ins(), send_ins_flow(), send_ins_ref(), send_quat(), set_body_state_from_quat(), stateSetLocalOrigin_i(), InsFlow::thrust_factor, and use_filter.

Here is the call graph for this function:

ins_flow_update()

void ins_flow_update

(

void

)

Definition at line 725 of file ins_flow.c.

References autopilot_in_flight(), Stabilization::cmd, CONSTANT_ALT_FILTER, DEBUG_INS_FLOW, DEBUG_MAT_PRINT, DEBUG_PRINT, InsFlow::divergence, E, float_mat_diagonal_scal(), float_mat_diff(), float_mat_invert(), float_mat_mul(), float_mat_sum(), float_mat_transpose(), g, get_sys_time_float(), H, if(), ins_flow, ins_reset_filter(), K, lp_factor_strong, InsFlow::lp_gyro_bias_pitch, InsFlow::lp_gyro_bias_roll, InsFlow::lp_gyro_pitch, InsFlow::lp_gyro_roll, InsFlow::lp_roll_command, InsFlow::lp_thrust, MAKE_MATRIX_PTR, N_MEAS_OF_KF, N_STATES_OF_KF, InsFlow::new_flow_measurement, OF_ANGLE_DOT_IND, OF_ANGLE_IND, OF_DIV_FLOW_IND, OF_DRAG, OF_LAT_FLOW_IND, OF_LAT_FLOW_X_IND, OF_N_ROTORS, OF_P, of_prev_time, OF_Q, OF_R, OF_RATE_IND, OF_THETA_IND, OF_THRUST_BIAS, OF_THRUST_BIAS_IND, of_time, OF_TWO_DIM, OF_USE_GYROS, OF_V_IND, OF_VX_IND, OF_X, OF_Z_DOT_IND, OF_Z_IND, InsFlow::optical_flow_x, InsFlow::optical_flow_y, P, PAR_IX, PAR_KD, PAR_MASS, PAR_PRED_ROLL_3, parameters, PREDICT_GYROS, print_ins_flow_state(), print_true_state(), reset_filter, InsFlow::RPM, RPM_FACTORS, run_filter, stabilization, InsFlow::thrust_factor, and thrust_factor.

Here is the call graph for this function:

Variable Documentation

GT_phi

float GT_phi

extern

Definition at line 190 of file ins_flow.c.

Referenced by ins_flow_init(), send_ins_flow(), and set_body_state_from_quat().

GT_theta

float GT_theta

extern

Definition at line 191 of file ins_flow.c.

Referenced by ins_flow_init(), send_ins_flow(), and set_body_state_from_quat().

OF_X

float OF_X[N_STATES_OF_KF]

extern

Definition at line 172 of file ins_flow.c.

Referenced by gps_cb(), ins_flow_update(), ins_reset_filter(), print_ins_flow_state(), send_ins_flow(), and set_body_state_from_quat().

reset_filter

bool reset_filter

extern

Definition at line 184 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

run_filter

bool run_filter

extern

Definition at line 186 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

thrust_factor

float thrust_factor

extern

Definition at line 188 of file ins_flow.c.

Referenced by ins_flow_update(), and send_ins_flow().

use_filter

int use_filter

extern

Definition at line 185 of file ins_flow.c.

Referenced by gps_cb(), ins_flow_init(), send_ins_flow(), and set_body_state_from_quat().