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	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_reset_local_origin (void)
	INS local origin reset. More...
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 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 138 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 173 of file ins_flow.c.

OF_N_ROTORS

#define OF_N_ROTORS   4

Definition at line 182 of file ins_flow.c.

PAR_BASE

#define PAR_BASE   2

Definition at line 262 of file ins_flow.c.

PAR_IX

#define PAR_IX   0

Definition at line 260 of file ins_flow.c.

PAR_K0

#define PAR_K0   3

Definition at line 263 of file ins_flow.c.

PAR_K1

#define PAR_K1   4

Definition at line 264 of file ins_flow.c.

PAR_K2

#define PAR_K2   5

Definition at line 265 of file ins_flow.c.

PAR_K3

#define PAR_K3   6

Definition at line 266 of file ins_flow.c.

PAR_KD

#define PAR_KD   19

Definition at line 279 of file ins_flow.c.

PAR_MASS

#define PAR_MASS   1

Definition at line 261 of file ins_flow.c.

PAR_P0

#define PAR_P0   14

Definition at line 274 of file ins_flow.c.

PAR_P1

#define PAR_P1   15

Definition at line 275 of file ins_flow.c.

PAR_P2

#define PAR_P2   16

Definition at line 276 of file ins_flow.c.

PAR_P3

#define PAR_P3   17

Definition at line 277 of file ins_flow.c.

PAR_P4

#define PAR_P4   18

Definition at line 278 of file ins_flow.c.

PAR_P_TB

#define PAR_P_TB   21

Definition at line 281 of file ins_flow.c.

PAR_PRED_ROLL_1

#define PAR_PRED_ROLL_1   22

Definition at line 282 of file ins_flow.c.

PAR_PRED_ROLL_2

#define PAR_PRED_ROLL_2   23

Definition at line 283 of file ins_flow.c.

PAR_PRED_ROLL_3

#define PAR_PRED_ROLL_3   24

Definition at line 284 of file ins_flow.c.

PAR_Q0

#define PAR_Q0   9

Definition at line 269 of file ins_flow.c.

PAR_Q1

#define PAR_Q1   10

Definition at line 270 of file ins_flow.c.

PAR_Q2

#define PAR_Q2   11

Definition at line 271 of file ins_flow.c.

PAR_Q3

#define PAR_Q3   12

Definition at line 272 of file ins_flow.c.

PAR_Q4

#define PAR_Q4   13

Definition at line 273 of file ins_flow.c.

PAR_Q_TB

#define PAR_Q_TB   20

Definition at line 280 of file ins_flow.c.

PAR_R0

#define PAR_R0   7

Definition at line 267 of file ins_flow.c.

PAR_R1

#define PAR_R1   8

Definition at line 268 of file ins_flow.c.

USE_STANDARD_PARAMS

#define USE_STANDARD_PARAMS   0

Definition at line 199 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 1379 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 1565 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 1493 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_local_origin(), 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, 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 1346 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 1481 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 537 of file ins_flow.c.

References 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_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 732 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 675 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 494 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:

ins_reset_local_origin()

void ins_reset_local_origin

(

void

)

INS local origin reset.

Reset horizontal and vertical reference to the current position. Does nothing if not implemented by specific INS algorithm.

INS local origin reset.

Definition at line 666 of file ins_flow.c.

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

Referenced by gps_cb().

Here is the call graph for this function:

Here is the caller graph for this function:

print_ins_flow_state()

void print_ins_flow_state ( void  )

static

Definition at line 694 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 719 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:

send_bias()

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

static

Definition at line 336 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 320 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 353 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 343 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 365 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 395 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 381 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 304 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 1436 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 134 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 133 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 192 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 195 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 196 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 136 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(), ins_reset_local_origin(), 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 187 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 188 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

moment_ind

int moment_ind

Definition at line 175 of file ins_flow.c.

Referenced by ins_flow_init().

moments

float moments[MOMENT_DELAY] = {0.}

Definition at line 174 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 179 of file ins_flow.c.

Referenced by ins_flow_update(), and ins_reset_filter().

of_prev_time

float of_prev_time

Definition at line 186 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 178 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 180 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

of_time

float of_time

Definition at line 185 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 177 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 240 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_filter

bool reset_filter

Definition at line 189 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 183 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 191 of file ins_flow.c.

Referenced by ins_flow_init(), and ins_flow_update().

thrust_factor

float thrust_factor

Definition at line 193 of file ins_flow.c.

Referenced by ins_flow_update(), and send_ins_flow().

use_filter

int use_filter

Definition at line 190 of file ins_flow.c.

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