Integrated Navigation System interface. More...

#include <time.h>
#include "ins_ext_pose.h"
#include "state.h"
#include "math/pprz_algebra_float.h"
#include "modules/imu/imu.h"
#include "modules/ins/ins.h"
#include "generated/flight_plan.h"
#include "modules/core/abi.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for ins_ext_pose.c:

Go to the source code of this file.

Data Structures
struct	InsExtPose
	Data for telemetry and LTP origin. More...

Macros
#define	DEBUG_PRINT(...) {}

#define	INS_EXT_POSE_IMU_ID ABI_BROADCAST
	Import Gyro and Acc from ABI. More...

Functions
static void	ins_ext_pose_init_from_flightplan (void)

static void	send_ins (struct transport_tx trans, struct link_device dev)
	Provide telemetry. More...

static void	send_ins_z (struct transport_tx trans, struct link_device dev)

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

static void	send_external_pose_down (struct transport_tx trans, struct link_device dev)

static void	send_ahrs_bias (struct transport_tx trans, struct link_device dev)

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

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

void	ins_ext_pose_msg_update (uint8_t *buf)
	Import External Pose Message. More...

void	ins_reset_local_origin (void)
	INS local origin reset. More...

void	ins_reset_altitude_ref (void)
	INS altitude reference reset. More...

static void	ekf_init (void)
	EKF protos. More...

static void	ekf_run (void)

void	ins_ext_pose_init (void)
	Module. More...

void	ins_ext_pose_run (void)

static void	ekf_f (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES])

static void	ekf_F (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_STATES])

static void	ekf_L (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], float out[EKF_NUM_STATES][EKF_NUM_INPUTS])

static void	ekf_f_rk4 (const float X[EKF_NUM_STATES], const float U[EKF_NUM_INPUTS], const float dt, float out[EKF_NUM_STATES])

static void	ekf_step (const float U[EKF_NUM_INPUTS], const float Z[EKF_NUM_OUTPUTS], const float dt)

static void	ekf_prediction_step (const float U[EKF_NUM_INPUTS], const float dt)

static void	ekf_measurement_step (const float Z[EKF_NUM_OUTPUTS])

void	ekf_set_diag (float *a, float b, int n)

void	ins_ext_pos_log_header (FILE *file)
	Logging. More...

void	ins_ext_pos_log_data (FILE *file)

Variables
struct InsExtPose	ins_ext_pos

static abi_event	accel_ev

static abi_event	gyro_ev

float	ekf_X [EKF_NUM_STATES]

float	ekf_U [EKF_NUM_INPUTS]

float	ekf_Z [EKF_NUM_OUTPUTS]

float	ekf_P [EKF_NUM_STATES][EKF_NUM_STATES]

float	ekf_Q [EKF_NUM_INPUTS][EKF_NUM_INPUTS]

float	ekf_R [EKF_NUM_OUTPUTS][EKF_NUM_OUTPUTS]

float	ekf_H [EKF_NUM_OUTPUTS][EKF_NUM_STATES] = {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}}

float	t0

float	t1

Detailed Description

Integrated Navigation System interface.

Definition in file ins_ext_pose.c.

Data Structure Documentation

◆ InsExtPose

struct InsExtPose

Data for telemetry and LTP origin.

Definition at line 48 of file ins_ext_pose.c.

Collaboration diagram for InsExtPose:

Data Fields
struct FloatVect3	accels_f
struct FloatEulers	ev_att
struct FloatVect3	ev_pos
struct FloatQuat	ev_quat
float	ev_time
struct FloatVect3	ev_vel
struct FloatRates	gyros_f
bool	has_new_acc
bool	has_new_ext_pose
bool	has_new_gyro
struct NedCoor_i	ltp_accel
struct LtpDef_i	ltp_def
struct NedCoor_i	ltp_pos
struct NedCoor_i	ltp_speed

Macro Definition Documentation

◆ DEBUG_PRINT

#define DEBUG_PRINT ( ... ) {}

Definition at line 43 of file ins_ext_pose.c.

◆ INS_EXT_POSE_IMU_ID

#define INS_EXT_POSE_IMU_ID ABI_BROADCAST

Import Gyro and Acc from ABI.

Definition at line 161 of file ins_ext_pose.c.

Function Documentation

◆ accel_cb()

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

static

Definition at line 181 of file ins_ext_pose.c.

References InsExtPose::accels_f, ACCELS_FLOAT_OF_BFP, InsExtPose::has_new_acc, and ins_ext_pos.

Referenced by ins_ext_pose_init().

Here is the caller graph for this function:

◆ ekf_f()

static void ekf_f	(	const float	X[EKF_NUM_STATES],
		const float	U[EKF_NUM_INPUTS],
		float	out[EKF_NUM_STATES]
	)

inlinestatic

Definition at line 376 of file ins_ext_pose.c.

References mesonh.mesonh_atmosphere::X, and x12.

Referenced by ekf_f_rk4(), and ekf_prediction_step().

Here is the caller graph for this function:

◆ ekf_F()

static void ekf_F	(	const float	X[EKF_NUM_STATES],
		const float	U[EKF_NUM_INPUTS],
		float	out[EKF_NUM_STATES][EKF_NUM_STATES]
	)

inlinestatic

Definition at line 415 of file ins_ext_pose.c.

References mesonh.mesonh_atmosphere::X, and x12.

Referenced by ekf_prediction_step(), and ekf_step().

Here is the caller graph for this function:

◆ ekf_f_rk4()

static void ekf_f_rk4	(	const float	X[EKF_NUM_STATES],
		const float	U[EKF_NUM_INPUTS],
		const float	dt,
		float	out[EKF_NUM_STATES]
	)

inlinestatic

Definition at line 793 of file ins_ext_pose.c.

References ekf_f(), EKF_NUM_STATES, float_vect_add(), float_vect_scale(), float_vect_smul(), float_vect_sum(), and mesonh.mesonh_atmosphere::X.

Referenced by ekf_step().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_init()

static void ekf_init ( void )

inlinestatic

EKF protos.

Definition at line 351 of file ins_ext_pose.c.

References DEBUG_PRINT, EKF_NUM_INPUTS, EKF_NUM_OUTPUTS, EKF_NUM_STATES, ekf_P, ekf_Q, ekf_R, ekf_set_diag(), ekf_U, ekf_X, ekf_Z, float_vect_copy(), and MAKE_MATRIX_PTR.

Referenced by ins_ext_pose_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_L()

static void ekf_L	(	const float	X[EKF_NUM_STATES],
		const float	U[EKF_NUM_INPUTS],
		float	out[EKF_NUM_STATES][EKF_NUM_INPUTS]
	)

inlinestatic

Definition at line 684 of file ins_ext_pose.c.

References mesonh.mesonh_atmosphere::X.

Referenced by ekf_prediction_step(), and ekf_step().

Here is the caller graph for this function:

◆ ekf_measurement_step()

static void ekf_measurement_step ( const float Z[EKF_NUM_OUTPUTS] )

inlinestatic

Definition at line 1075 of file ins_ext_pose.c.

References ekf_H, EKF_NUM_OUTPUTS, EKF_NUM_STATES, ekf_P, ekf_R, ekf_X, float_mat_copy(), float_mat_invert(), float_mat_mul(), float_mat_mul_copy(), float_mat_scale(), float_mat_sum_scaled(), float_mat_transpose(), float_mat_vect_mul(), float_vect_add(), float_vect_copy(), float_vect_scale(), K, MAKE_MATRIX_PTR, and mesonh.mesonh_atmosphere::Z.

Referenced by ekf_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_prediction_step()

static void ekf_prediction_step	(	const float	U[EKF_NUM_INPUTS],
		const float	dt
	)

inlinestatic

Definition at line 994 of file ins_ext_pose.c.

References ekf_f(), ekf_F(), ekf_L(), EKF_NUM_INPUTS, EKF_NUM_STATES, ekf_P, ekf_Q, ekf_X, float_mat_copy(), float_mat_diagonal_scal(), float_mat_mul(), float_mat_scale(), float_mat_sum_scaled(), float_mat_transpose(), float_mat_transpose_square(), float_vect_add(), float_vect_copy(), float_vect_scale(), and MAKE_MATRIX_PTR.

Referenced by ekf_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_run()

static void ekf_run ( void )

inlinestatic

Definition at line 1163 of file ins_ext_pose.c.

References ACCELS_BFP_OF_REAL, InsExtPose::accels_f, DEBUG_PRINT, ekf_measurement_step(), ekf_prediction_step(), ekf_U, ekf_X, ekf_Z, InsExtPose::ev_att, InsExtPose::ev_pos, float_rmat_transp_vmult(), get_sys_time_float(), InsExtPose::gyros_f, InsExtPose::has_new_acc, InsExtPose::has_new_ext_pose, InsExtPose::has_new_gyro, ins_ext_pos, FloatRates::p, FloatEulers::phi, FloatEulers::psi, FloatRates::q, FloatRates::r, stateGetNedToBodyRMat_f(), stateSetAccelBody_i(), stateSetAccelNed_f(), stateSetBodyRates_f(), stateSetNedToBodyEulers_f(), stateSetPositionNed_f(), stateSetSpeedNed_f(), t0, t1, FloatEulers::theta, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by ins_ext_pose_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ekf_set_diag()

void ekf_set_diag	(	float **	a,
		float *	b,
		int	n
	)

Definition at line 335 of file ins_ext_pose.c.

References b.

Referenced by ekf_init().

Here is the caller graph for this function:

◆ ekf_step()

static void ekf_step	(	const float	U[EKF_NUM_INPUTS],
		const float	Z[EKF_NUM_OUTPUTS],
		const float	dt
	)

inlinestatic

Definition at line 842 of file ins_ext_pose.c.

References ekf_F(), ekf_f_rk4(), ekf_H, ekf_L(), EKF_NUM_INPUTS, EKF_NUM_OUTPUTS, EKF_NUM_STATES, ekf_P, ekf_Q, ekf_R, ekf_X, float_mat_diagonal_scal(), float_mat_invert(), float_mat_mul(), float_mat_mul_copy(), float_mat_scale(), float_mat_sum_scaled(), float_mat_transpose(), float_mat_transpose_square(), float_mat_vect_mul(), float_vect_add(), float_vect_scale(), K, MAKE_MATRIX_PTR, and mesonh.mesonh_atmosphere::Z.

Here is the call graph for this function:

◆ gyro_cb()

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

static

Definition at line 173 of file ins_ext_pose.c.

References InsExtPose::gyros_f, InsExtPose::has_new_gyro, ins_ext_pos, and RATES_FLOAT_OF_BFP.

Referenced by ins_ext_pose_init().

Here is the caller graph for this function:

◆ ins_ext_pos_log_data()

void ins_ext_pos_log_data ( FILE * file )

Definition at line 1298 of file ins_ext_pose.c.

Referenced by logger_file_write_row().

Here is the caller graph for this function:

◆ ins_ext_pos_log_header()

void ins_ext_pos_log_header ( FILE * file )

Logging.

Definition at line 1290 of file ins_ext_pose.c.

Referenced by logger_file_write_header().

Here is the caller graph for this function:

◆ ins_ext_pose_init()

void ins_ext_pose_init ( void )

Module.

Definition at line 263 of file ins_ext_pose.c.

References accel_cb(), accel_ev, DefaultPeriodic, ekf_init(), gyro_cb(), gyro_ev, InsExtPose::has_new_acc, InsExtPose::has_new_ext_pose, InsExtPose::has_new_gyro, ins_ext_pos, INS_EXT_POSE_IMU_ID, ins_ext_pose_init_from_flightplan(), register_periodic_telemetry(), send_ahrs_bias(), send_external_pose_down(), send_ins(), send_ins_ref(), and send_ins_z().

Here is the call graph for this function:

◆ ins_ext_pose_init_from_flightplan()

static void ins_ext_pose_init_from_flightplan ( void )

static

Definition at line 73 of file ins_ext_pose.c.

References LlaCoor_i::alt, ecef_of_lla_i(), LtpDef_i::hmsl, ins_ext_pos, LlaCoor_i::lat, LlaCoor_i::lon, InsExtPose::ltp_def, ltp_def_from_ecef_i(), stateSetLocalOrigin_i(), and waypoints_localize_all().

Referenced by ins_ext_pose_init().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ ins_ext_pose_msg_update()

void ins_ext_pose_msg_update ( uint8_t * buf )

Import External Pose Message.

Definition at line 194 of file ins_ext_pose.c.

References DEBUG_PRINT, InsExtPose::ev_att, InsExtPose::ev_pos, InsExtPose::ev_quat, InsExtPose::ev_time, InsExtPose::ev_vel, float_eulers_of_quat(), get_sys_time_usec(), InsExtPose::has_new_ext_pose, ins_ext_pos, FloatEulers::phi, FloatEulers::psi, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatEulers::theta, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Here is the call graph for this function:

◆ ins_ext_pose_run()

void ins_ext_pose_run ( void )

Definition at line 292 of file ins_ext_pose.c.

References ekf_run().

Here is the call 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 247 of file ins_ext_pose.c.

◆ 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 242 of file ins_ext_pose.c.

◆ send_ahrs_bias()

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

static

Definition at line 139 of file ins_ext_pose.c.

References dev, and ekf_X.

Referenced by ins_ext_pose_init().

Here is the caller graph for this function:

◆ send_external_pose_down()

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

static

Definition at line 124 of file ins_ext_pose.c.

References dev, InsExtPose::ev_pos, InsExtPose::ev_quat, InsExtPose::ev_time, InsExtPose::ev_vel, ins_ext_pos, FloatQuat::qi, FloatQuat::qx, FloatQuat::qy, FloatQuat::qz, FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by ins_ext_pose_init().

Here is the caller graph for this function:

◆ send_ins()

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

static

Provide telemetry.

Definition at line 99 of file ins_ext_pose.c.

References dev, ins_ext_pos, InsExtPose::ltp_accel, InsExtPose::ltp_pos, InsExtPose::ltp_speed, NedCoor_i::x, NedCoor_i::y, and NedCoor_i::z.

Referenced by ins_ext_pose_init().

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 115 of file ins_ext_pose.c.

References LlaCoor_i::alt, dev, LtpDef_i::ecef, LtpDef_i::hmsl, ins_ext_pos, LlaCoor_i::lat, LtpDef_i::lla, LlaCoor_i::lon, InsExtPose::ltp_def, EcefCoor_i::x, EcefCoor_i::y, and EcefCoor_i::z.

Referenced by ins_ext_pose_init().

Here is the caller graph for this function:

◆ send_ins_z()

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

static

Definition at line 107 of file ins_ext_pose.c.

References dev, ins_ext_pos, InsExtPose::ltp_accel, InsExtPose::ltp_pos, InsExtPose::ltp_speed, and NedCoor_i::z.

Referenced by ins_ext_pose_init().

Here is the caller graph for this function:

Variable Documentation

◆ accel_ev

abi_event accel_ev

static

Definition at line 165 of file ins_ext_pose.c.

Referenced by ins_ext_pose_init().

◆ ekf_H

float ekf_H[EKF_NUM_OUTPUTS][EKF_NUM_STATES] = {{1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0}, {0, 0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0}}

Definition at line 328 of file ins_ext_pose.c.

Referenced by ekf_measurement_step(), and ekf_step().