ins_flow.c File Reference

#include "ins_flow.h"
#include "modules/core/abi.h"
#include "generated/airframe.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "math/pprz_algebra_float.h"
#include "generated/flight_plan.h"
#include "modules/actuators/motor_mixing.h"
#include "stabilization.h"
#include <stdio.h>
#include "modules/datalink/telemetry.h"
#include "state.h"

Include dependency graph for ins_flow.c:

Go to the source code of this file.

Data Structures
struct	InsFlow

Macros
#define	DEBUG_INS_FLOW   0
#define	DEBUG_PRINT(...)
#define	DEBUG_MAT_PRINT(...)
#define	AHRS_ICQ_OUTPUT_ENABLED   TRUE
#define	INS_FLOW_GYRO_ID   ABI_BROADCAST
#define	INS_FLOW_ACCEL_ID   ABI_BROADCAST
#define	INS_FLOW_IMU_ID   ABI_BROADCAST
#define	INS_FLOW_GPS_ID   GPS_MULTI_ID
#define	INS_OPTICAL_FLOW_ID   ABI_BROADCAST
#define	INS_RPM_ID   ABI_BROADCAST
#define	MOMENT_DELAY   20
#define	OF_N_ROTORS   4
#define	USE_STANDARD_PARAMS   0
#define	PAR_IX   0
#define	PAR_MASS   1
#define	PAR_BASE   2
#define	PAR_K0   3
#define	PAR_K1   4
#define	PAR_K2   5
#define	PAR_K3   6
#define	PAR_R0   7
#define	PAR_R1   8
#define	PAR_Q0   9
#define	PAR_Q1   10
#define	PAR_Q2   11
#define	PAR_Q3   12
#define	PAR_Q4   13
#define	PAR_P0   14
#define	PAR_P1   15
#define	PAR_P2   16
#define	PAR_P3   17
#define	PAR_P4   18
#define	PAR_KD   19
#define	PAR_Q_TB   20
#define	PAR_P_TB   21
#define	PAR_PRED_ROLL_1   22
#define	PAR_PRED_ROLL_2   23
#define	PAR_PRED_ROLL_3   24

Functions
static void	gyro_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *gyro)
static void	accel_cb (uint8_t sender_id, uint32_t stamp, struct Int32Vect3 *accel)
static void	gps_cb (uint8_t sender_id, uint32_t stamp, struct GpsState *gps_s)
void	ins_optical_flow_cb (uint8_t sender_id, uint32_t stamp, int32_t flow_x, int32_t flow_y, int32_t flow_der_x, int32_t flow_der_y, float quality, float size_divergence)
static void	ins_act_feedback_cb (uint8_t sender_id, struct act_feedback_t *feedback, uint8_t num_act)
static void	aligner_cb (uint8_t sender_id, uint32_t stamp, struct Int32Rates *lp_gyro, struct Int32Vect3 *lp_accel, struct Int32Vect3 *lp_mag)
static void	reset_cb (uint8_t sender_id UNUSED, uint8_t flag)
static void	print_ins_flow_state (void)
static void	print_true_state (void)
static void	set_body_state_from_quat (void)
	Rotate angles and rates from imu to body frame and set state. More...
static void	ins_reset_filter (void)
static void	send_quat (struct transport_tx *trans, struct link_device *dev)
static void	send_euler (struct transport_tx *trans, struct link_device *dev)
static void	send_bias (struct transport_tx *trans, struct link_device *dev)
static void	send_geo_mag (struct transport_tx *trans, struct link_device *dev)
static void	send_filter_status (struct transport_tx *trans, struct link_device *dev)
static void	send_ins (struct transport_tx *trans, struct link_device *dev)
static void	send_ins_ref (struct transport_tx *trans, struct link_device *dev)
static void	send_ins_flow (struct transport_tx *trans, struct link_device *dev)
void	ins_flow_init (void)
void	ins_flow_update (void)

Variables
static abi_event	gyro_ev
static abi_event	accel_ev
static abi_event	gps_ev
static abi_event	ins_optical_flow_ev
static abi_event	ins_act_feedback_ev
static abi_event	aligner_ev
static abi_event	reset_ev
static uint32_t	ahrs_icq_last_stamp
static uint8_t	ahrs_flow_id = AHRS_COMP_ID_FLOW
	Component ID for FLOW. More...
struct InsFlow	ins_flow
float	moments [MOMENT_DELAY] = {0.}
int	moment_ind
float	OF_X [N_STATES_OF_KF] = {0.}
float	OF_Q [N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}
float	OF_P [N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}
float	OF_R [N_MEAS_OF_KF][N_MEAS_OF_KF] = {{0.}}
float	RPM_FACTORS [OF_N_ROTORS]
float	of_time
float	of_prev_time
float	lp_factor
float	lp_factor_strong
bool	reset_filter
int	use_filter
bool	run_filter
uint32_t	counter
float	thrust_factor
float	GT_phi
float	GT_theta
float	parameters [20] = {8.407886e-03, 4.056093e-01, 1.555919e-01, 1.291584e-01, 2.594766e-01, 1.927331e-01, 9.599609e-02, 1.688265e-01, 5.589618e-02, 1.605665e-01, 1.195912e-01, 1.809532e+00, 4.268251e-02, 3.003060e+00, 1.098473e+00, 1.944433e-01, 2.363352e-01, 1.110390e+00, 1.190994e+00, 6.211962e-01}

---

Data Structure Documentation

InsFlow

struct InsFlow

Definition at line 133 of file ins_flow.c.

Collaboration diagram for InsFlow:

Data Fields
float	divergence
float	lp_gyro_bias_pitch
float	lp_gyro_bias_roll
float	lp_gyro_pitch
float	lp_gyro_roll
float	lp_roll_command
float	lp_thrust
struct NedCoor_i	ltp_accel
struct LtpDef_i	ltp_def
bool	ltp_initialized
struct NedCoor_i	ltp_pos
struct NedCoor_i	ltp_speed
bool	new_flow_measurement
float	optical_flow_x
float	optical_flow_y
uint16_t	RPM[8]
uint8_t	RPM_num_act
float	thrust_factor
float	vision_time

Macro Definition Documentation

AHRS_ICQ_OUTPUT_ENABLED

#define AHRS_ICQ_OUTPUT_ENABLED   TRUE

Definition at line 62 of file ins_flow.c.

DEBUG_INS_FLOW

#define DEBUG_INS_FLOW   0

Definition at line 49 of file ins_flow.c.

DEBUG_MAT_PRINT

#define DEBUG_MAT_PRINT

(

...

)

Definition at line 58 of file ins_flow.c.

DEBUG_PRINT

#define DEBUG_PRINT

(

...

)

Definition at line 57 of file ins_flow.c.

INS_FLOW_ACCEL_ID

#define INS_FLOW_ACCEL_ID   ABI_BROADCAST

Definition at line 74 of file ins_flow.c.

INS_FLOW_GPS_ID

#define INS_FLOW_GPS_ID   GPS_MULTI_ID

Definition at line 87 of file ins_flow.c.

INS_FLOW_GYRO_ID

#define INS_FLOW_GYRO_ID   ABI_BROADCAST

Definition at line 68 of file ins_flow.c.

INS_FLOW_IMU_ID

#define INS_FLOW_IMU_ID   ABI_BROADCAST

Definition at line 80 of file ins_flow.c.

INS_OPTICAL_FLOW_ID

#define INS_OPTICAL_FLOW_ID   ABI_BROADCAST

Definition at line 93 of file ins_flow.c.

INS_RPM_ID

#define INS_RPM_ID   ABI_BROADCAST

Definition at line 99 of file ins_flow.c.

MOMENT_DELAY

#define MOMENT_DELAY   20

Definition at line 168 of file ins_flow.c.

OF_N_ROTORS

#define OF_N_ROTORS   4

Definition at line 177 of file ins_flow.c.

PAR_BASE

#define PAR_BASE   2

Definition at line 257 of file ins_flow.c.

PAR_IX

#define PAR_IX   0

Definition at line 255 of file ins_flow.c.

PAR_K0

#define PAR_K0   3

Definition at line 258 of file ins_flow.c.

PAR_K1

#define PAR_K1   4

Definition at line 259 of file ins_flow.c.

PAR_K2

#define PAR_K2   5

Definition at line 260 of file ins_flow.c.

PAR_K3

#define PAR_K3   6

Definition at line 261 of file ins_flow.c.

PAR_KD

#define PAR_KD   19

Definition at line 274 of file ins_flow.c.

PAR_MASS

#define PAR_MASS   1

Definition at line 256 of file ins_flow.c.

PAR_P0

#define PAR_P0   14

Definition at line 269 of file ins_flow.c.

PAR_P1

#define PAR_P1   15

Definition at line 270 of file ins_flow.c.

PAR_P2

#define PAR_P2   16

Definition at line 271 of file ins_flow.c.

PAR_P3

#define PAR_P3   17

Definition at line 272 of file ins_flow.c.

PAR_P4

#define PAR_P4   18

Definition at line 273 of file ins_flow.c.

PAR_P_TB

#define PAR_P_TB   21

Definition at line 276 of file ins_flow.c.

PAR_PRED_ROLL_1

#define PAR_PRED_ROLL_1   22

Definition at line 277 of file ins_flow.c.

PAR_PRED_ROLL_2

#define PAR_PRED_ROLL_2   23

Definition at line 278 of file ins_flow.c.

PAR_PRED_ROLL_3

#define PAR_PRED_ROLL_3   24

Definition at line 279 of file ins_flow.c.

PAR_Q0

#define PAR_Q0   9

Definition at line 264 of file ins_flow.c.

PAR_Q1

#define PAR_Q1   10

Definition at line 265 of file ins_flow.c.

PAR_Q2

#define PAR_Q2   11

Definition at line 266 of file ins_flow.c.

PAR_Q3

#define PAR_Q3   12

Definition at line 267 of file ins_flow.c.

PAR_Q4

#define PAR_Q4   13

Definition at line 268 of file ins_flow.c.

PAR_Q_TB

#define PAR_Q_TB   20

Definition at line 275 of file ins_flow.c.

PAR_R0

#define PAR_R0   7

Definition at line 262 of file ins_flow.c.

PAR_R1

#define PAR_R1   8

Definition at line 263 of file ins_flow.c.

USE_STANDARD_PARAMS

#define USE_STANDARD_PARAMS   0

Definition at line 194 of file ins_flow.c.

Function Documentation

accel_cb()

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

static

Definition at line 1372 of file ins_flow.c.

References ahrs_icq, ahrs_icq_update_accel(), AhrsIntCmplQuat::is_aligned, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), and set_body_state_from_quat().

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

aligner_cb()

static void aligner_cb ( uint8_t  sender_id, uint32_t  stamp, struct Int32Rates *  lp_gyro, struct Int32Vect3 *  lp_accel, struct Int32Vect3 *  lp_mag  )

static

Definition at line 1527 of file ins_flow.c.

Referenced by ins_flow_init().

Here is the caller graph for this function:

gps_cb()

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

static

Definition at line 1455 of file ins_flow.c.

References ahrs_icq_update_gps(), CONSTANT_ALT_FILTER, GpsState::ecef_pos, GpsState::ecef_vel, GpsState::fix, float_rmat_inv(), float_rmat_vmult(), GPS_FIX_3D, ins_flow, INS_RESET_REF, INT32_POS_OF_CM_DEN, INT32_POS_OF_CM_NUM, INT32_SPEED_OF_CM_S_DEN, INT32_SPEED_OF_CM_S_NUM, INT32_VECT3_SCALE_2, InsFlow::ltp_def, InsFlow::ltp_initialized, InsFlow::ltp_pos, InsFlow::ltp_speed, ned_of_ecef_point_i(), ned_of_ecef_vect_i(), OF_V_IND, OF_X, OF_Z_IND, reset_cb(), stateGetNedToBodyRMat_f(), stateSetPositionNed_i(), stateSetSpeedNed_i(), use_filter, USE_HEIGHT, USE_VELOCITY, FloatVect3::x, FloatVect3::y, NedCoor_i::y, FloatVect3::z, and NedCoor_i::z.

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

gyro_cb()

static void gyro_cb ( uint8_t  sender_id, uint32_t  stamp, struct Int32Rates *  gyro  )

static

Definition at line 1339 of file ins_flow.c.

References ahrs_icq, ahrs_icq_last_stamp, ahrs_icq_propagate(), AHRS_ICQ_RUNNING, AHRS_PROPAGATE_FREQUENCY, ins_flow, AhrsIntCmplQuat::is_aligned, lp_factor, InsFlow::lp_gyro_pitch, InsFlow::lp_gyro_roll, Int32Rates::p, PRINT_CONFIG_MSG(), PRINT_CONFIG_VAR(), Int32Rates::q, set_body_state_from_quat(), and AhrsIntCmplQuat::status.

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

ins_act_feedback_cb()

static void ins_act_feedback_cb ( uint8_t sender_id  UNUSED, struct act_feedback_t *  feedback, uint8_t  num_act  )

static

Definition at line 1443 of file ins_flow.c.

References ins_flow, act_feedback_t::rpm, InsFlow::RPM, and InsFlow::RPM_num_act.

Referenced by ins_flow_init().

Here is the caller graph for this function:

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:

ins_optical_flow_cb()

void ins_optical_flow_cb	(	uint8_t sender_id	UNUSED,
		uint32_t	stamp,
		int32_t flow_x	UNUSED,
		int32_t flow_y	UNUSED,
		int32_t flow_der_x	UNUSED,
		int32_t flow_der_y	UNUSED,
		float quality	UNUSED,
		float	size_divergence
	)

Definition at line 668 of file ins_flow.c.

References InsFlow::divergence, ins_flow, InsFlow::new_flow_measurement, InsFlow::optical_flow_x, InsFlow::optical_flow_y, and InsFlow::vision_time.

Referenced by ins_flow_init().

Here is the caller graph for this function:

ins_reset_filter()

void ins_reset_filter ( void  )

static

Definition at line 489 of file ins_flow.c.

References CONSTANT_ALT_FILTER, counter, N_STATES_OF_KF, OF_ANGLE_DOT_IND, OF_ANGLE_IND, OF_P, OF_THETA_IND, OF_THRUST_BIAS, OF_THRUST_BIAS_IND, OF_TWO_DIM, OF_V_IND, OF_VX_IND, OF_X, OF_Z_DOT_IND, OF_Z_IND, PAR_P0, PAR_P1, PAR_P2, PAR_P3, PAR_P4, PAR_P_TB, and parameters.

Referenced by ins_flow_init(), and ins_flow_update().

Here is the caller graph for this function:

print_ins_flow_state()

void print_ins_flow_state ( void  )

static

Definition at line 687 of file ins_flow.c.

References CONSTANT_ALT_FILTER, OF_ANGLE_DOT_IND, OF_ANGLE_IND, OF_THETA_IND, OF_THRUST_BIAS, OF_THRUST_BIAS_IND, OF_TWO_DIM, OF_V_IND, OF_VX_IND, OF_X, OF_Z_DOT_IND, and OF_Z_IND.

Referenced by ins_flow_update().

Here is the caller graph for this function:

print_true_state()

void print_true_state ( void  )

static

Definition at line 712 of file ins_flow.c.

References FloatRates::p, FloatEulers::phi, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), NedCoor_f::y, and NedCoor_f::z.

Referenced by ins_flow_update().

Here is the call graph for this function:

Here is the caller graph for this function:

reset_cb()

static void reset_cb ( uint8_t sender_id  UNUSED, uint8_t  flag  )

static

Definition at line 657 of file ins_flow.c.

References LlaCoor_i::alt, GpsState::ecef_pos, gps, LtpDef_i::hmsl, GpsState::hmsl, ins_flow, INS_RESET_REF, LtpDef_i::lla, GpsState::lla_pos, InsFlow::ltp_def, ltp_def_from_ecef_i(), InsFlow::ltp_initialized, and stateSetLocalOrigin_i().

Referenced by gps_cb(), and ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_bias()

static void send_bias ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 331 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, dev, AhrsIntCmplQuat::gyro_bias, Int32Rates::p, Int32Rates::q, and Int32Rates::r.

Referenced by ins_flow_init().

Here is the caller graph for this function:

send_euler()

static void send_euler ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 315 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, dev, int32_eulers_of_quat(), AhrsIntCmplQuat::ltp_to_body_quat, Int32Eulers::phi, Int32Eulers::psi, stateGetNedToBodyEulers_i(), and Int32Eulers::theta.

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_filter_status()

static void send_filter_status ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 348 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, ahrs_icq_last_stamp, dev, get_sys_time_usec(), AhrsIntCmplQuat::is_aligned, and val.

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_geo_mag()

static void send_geo_mag ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 338 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, dev, MAG_FLOAT_OF_BFP, AhrsIntCmplQuat::mag_h, FloatVect3::x, Int32Vect3::x, FloatVect3::y, Int32Vect3::y, FloatVect3::z, and Int32Vect3::z.

Referenced by ins_flow_init().

Here is the caller graph for this function:

send_ins()

static void send_ins ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 360 of file ins_flow.c.

References dev, ins_flow, InsFlow::ltp_accel, InsFlow::ltp_pos, InsFlow::ltp_speed, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_flow_init().

Here is the caller graph for this function:

send_ins_flow()

static void send_ins_flow ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 390 of file ins_flow.c.

References CONSTANT_ALT_FILTER, dev, float_rmat_vmult(), g, GT_phi, GT_theta, ins_flow, InsFlow::lp_gyro_bias_pitch, InsFlow::lp_gyro_bias_roll, InsFlow::lp_gyro_pitch, InsFlow::lp_gyro_roll, OF_ANGLE_DOT_IND, OF_ANGLE_IND, OF_N_ROTORS, OF_THETA_IND, OF_THRUST_BIAS, OF_THRUST_BIAS_IND, OF_TWO_DIM, OF_V_IND, OF_VX_IND, OF_X, OF_Z_DOT_IND, OF_Z_IND, FloatRates::p, p, PAR_MASS, parameters, FloatEulers::phi, FloatRates::q, InsFlow::RPM, RPM_FACTORS, stateGetBodyRates_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyRMat_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), FloatEulers::theta, thrust_factor, USE_ANGLE, use_filter, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

send_ins_ref()

static void send_ins_ref ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 376 of file ins_flow.c.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, LtpDef_i::hmsl, ins_flow, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, InsFlow::ltp_def, InsFlow::ltp_initialized, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_flow_init().

Here is the caller graph for this function:

send_quat()

static void send_quat ( struct transport_tx *  trans, struct link_device *  dev  )

static

Definition at line 299 of file ins_flow.c.

References ahrs_flow_id, ahrs_icq, dev, AhrsIntCmplQuat::ltp_to_body_quat, Int32Quat::qi, Int32Quat::qx, Int32Quat::qy, Int32Quat::qz, stateGetNedToBodyQuat_i(), and AhrsIntCmplQuat::weight.

Referenced by ins_flow_init().

Here is the call graph for this function:

Here is the caller graph for this function:

set_body_state_from_quat()

static void set_body_state_from_quat ( void  )

static

Rotate angles and rates from imu to body frame and set state.

Definition at line 1398 of file ins_flow.c.

References ahrs_icq, AhrsIntCmplQuat::body_rate, OrientationReps::eulers_f, GT_phi, GT_theta, AhrsIntCmplQuat::ltp_to_body_quat, OF_ANGLE_IND, OF_THETA_IND, OF_TWO_DIM, OF_X, orientationGetEulers_f(), orientationGetQuat_i(), ORREP_EULER_F, ORREP_QUAT_I, FloatEulers::phi, OrientationReps::quat_i, stateSetBodyRates_i(), stateSetNedToBodyQuat_i(), OrientationReps::status, FloatEulers::theta, USE_ANGLE, and use_filter.

Referenced by accel_cb(), gyro_cb(), and ins_flow_init().

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 105 of file ins_flow.c.

Referenced by ins_flow_init().

ahrs_flow_id

uint8_t ahrs_flow_id = AHRS_COMP_ID_FLOW

static

Component ID for FLOW.

Definition at line 129 of file ins_flow.c.

Referenced by send_bias(), send_euler(), send_filter_status(), send_geo_mag(), and send_quat().

ahrs_icq_last_stamp

uint32_t ahrs_icq_last_stamp

static

Definition at line 128 of file ins_flow.c.

Referenced by gyro_cb(), and send_filter_status().

aligner_ev

abi_event aligner_ev

static

Definition at line 109 of file ins_flow.c.

Referenced by ins_flow_init().

counter

uint32_t counter

Definition at line 187 of file ins_flow.c.

Referenced by actuators_esc32_play_melody(), airspeed_otf_parse(), gec_decrypt_message(), gec_encrypt_message(), high_speed_logger_spi_link_periodic(), ins_mekf_wind_wrapper_init(), ins_reset_filter(), logger_file_start(), motor_mixing_run_spinup(), nav_fish_velocity_run(), periodic_light(), save_shot_on_disk(), stereo_avoid_run(), video_usb_logger_start(), and wind_estimator_init().

gps_ev

abi_event gps_ev

static

Definition at line 106 of file ins_flow.c.

Referenced by ins_flow_init().

GT_phi

float GT_phi

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

Definition at line 191 of file ins_flow.c.

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

gyro_ev

abi_event gyro_ev

static

Definition at line 104 of file ins_flow.c.

Referenced by ins_flow_init().

ins_act_feedback_ev

abi_event ins_act_feedback_ev

static

Definition at line 108 of file ins_flow.c.

Referenced by ins_flow_init().

ins_flow

struct InsFlow ins_flow

Definition at line 131 of file ins_flow.c.

Referenced by gps_cb(), gyro_cb(), ins_act_feedback_cb(), ins_flow_init(), ins_flow_update(), ins_optical_flow_cb(), reset_cb(), send_ins(), send_ins_flow(), and send_ins_ref().

ins_optical_flow_ev

abi_event ins_optical_flow_ev

static

Definition at line 107 of file ins_flow.c.

Referenced by ins_flow_init().

lp_factor

float lp_factor

Definition at line 182 of file ins_flow.c.

Referenced by gyro_cb(), horizontal_ctrl_module_run(), ins_flow_init(), PID_divergence_control(), PID_flow_control(), update_errors(), and vertical_ctrl_module_run().

lp_factor_strong

float lp_factor_strong

Definition at line 183 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

moment_ind

int moment_ind

Definition at line 170 of file ins_flow.c.

Referenced by ins_flow_init().

moments

float moments[MOMENT_DELAY] = {0.}

Definition at line 169 of file ins_flow.c.

Referenced by find_contour().

OF_P

float OF_P[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}

Definition at line 174 of file ins_flow.c.

Referenced by ins_flow_update(), and ins_reset_filter().

of_prev_time

float of_prev_time

Definition at line 181 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

OF_Q

float OF_Q[N_STATES_OF_KF][N_STATES_OF_KF] = {{0.}}

Definition at line 173 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

OF_R

float OF_R[N_MEAS_OF_KF][N_MEAS_OF_KF] = {{0.}}

Definition at line 175 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

of_time

float of_time

Definition at line 180 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

OF_X

float OF_X[N_STATES_OF_KF] = {0.}

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

parameters

float parameters[20] = {8.407886e-03, 4.056093e-01, 1.555919e-01, 1.291584e-01, 2.594766e-01, 1.927331e-01, 9.599609e-02, 1.688265e-01, 5.589618e-02, 1.605665e-01, 1.195912e-01, 1.809532e+00, 4.268251e-02, 3.003060e+00, 1.098473e+00, 1.944433e-01, 2.363352e-01, 1.110390e+00, 1.190994e+00, 6.211962e-01}

Definition at line 235 of file ins_flow.c.

Referenced by fit_linear_model(), fit_linear_model_OF(), fit_linear_model_prior(), ins_flow_init(), ins_flow_update(), ins_reset_filter(), and send_ins_flow().

reset_ev

abi_event reset_ev

static

Definition at line 110 of file ins_flow.c.

Referenced by ins_flow_init().

reset_filter

bool reset_filter

Definition at line 184 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

RPM_FACTORS

float RPM_FACTORS[OF_N_ROTORS]

Definition at line 178 of file ins_flow.c.

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

run_filter

bool run_filter

Definition at line 186 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

thrust_factor

float thrust_factor

Definition at line 188 of file ins_flow.c.

Referenced by ins_flow_update(), and send_ins_flow().

use_filter

int use_filter

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