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 51 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 42 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 164 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 184 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 389 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 428 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 806 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 364 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 697 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 1088 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 1007 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 1176 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 348 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 855 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 176 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 1311 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 1303 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 276 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 76 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 197 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(), float_quat_comp(), 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 305 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 260 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 255 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 142 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 127 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 102 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 118 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 110 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 168 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 341 of file ins_ext_pose.c.

Referenced by ekf_measurement_step(), and ekf_step().