guidance_indi.h File Reference

A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more! More...

#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"

Include dependency graph for guidance_indi.h:

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

Go to the source code of this file.

Functions
void	guidance_indi_enter (void)
	Call upon entering indi guidance. More...
void	guidance_indi_run (bool in_flight, int32_t heading)
void	guidance_indi_filter_attitude (void)
	Filter the attitude, such that it corresponds to the filtered measured acceleration. More...
void	guidance_indi_calcG (struct FloatMat33 *Gmat)
void	guidance_indi_filter_accel (void)
	low pass the accelerometer measurements with a second order filter to remove noise from vibrations More...
void	stabilization_attitude_set_setpoint_rp_quat_f (struct FloatEulers *indi_rp_cmd, bool in_flight, int32_t heading)
void	guidance_indi_filter_thrust (void)

Variables
float	guidance_indi_thrust_specific_force_gain

Detailed Description

A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!

Definition in file guidance_indi.h.

Function Documentation

void guidance_indi_calcG

(

struct FloatMat33 *

Gmat

)

Parameters

Gmat	array to write the matrix to [3x3]

Calculate the matrix of partial derivatives of the roll, pitch and thrust w.r.t. the NED accelerations

Definition at line 256 of file guidance_indi.c.

References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by guidance_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_indi_enter

(

void

)

Call upon entering indi guidance.

Definition at line 94 of file guidance_indi.c.

References filt_accel_ned, filt_accel_ned_d, filt_accel_ned_dd, filt_accelzbody, filt_accelzbodyd, filt_accelzbodydd, FLOAT_VECT3_ZERO, pitch_filt, pitch_filtd, pitch_filtdd, roll_filt, roll_filtd, roll_filtdd, thrust_act, thrust_filt, thrust_filtd, thrust_filtdd, and thrust_in.

Referenced by guidance_h_init(), and guidance_h_mode_changed().

Here is the caller graph for this function:

void guidance_indi_filter_accel

(

void

)

low pass the accelerometer measurements with a second order filter to remove noise from vibrations

Definition at line 223 of file guidance_indi.c.

References filt_accel_ned, filt_accel_ned_d, filt_accel_ned_dd, filter_omega, filter_zeta, stateGetAccelNed_f(), VECT3_ADD_SCALED, FloatVect3::x, NedCoor_f::x, FloatVect3::y, NedCoor_f::y, FloatVect3::z, and NedCoor_f::z.

Referenced by guidance_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_indi_filter_attitude

(

void

)

Filter the attitude, such that it corresponds to the filtered measured acceleration.

Definition at line 238 of file guidance_indi.c.

References filter_omega, filter_zeta, FloatEulers::phi, pitch_filt, pitch_filtd, pitch_filtdd, roll_filt, roll_filtd, roll_filtdd, stateGetNedToBodyEulers_f(), and FloatEulers::theta.

Referenced by guidance_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_indi_filter_thrust

(

void

)

Referenced by guidance_indi_run().

Here is the caller graph for this function:

void guidance_indi_run	(	bool	in_flight,
		int32_t	heading
	)

Parameters

in_flight	in flight boolean
heading	the desired heading [rad]

main indi guidance function

Definition at line 120 of file guidance_indi.c.

References euler_cmd, filt_accel_ned, Ga, Ga_inv, guidance_euler_cmd, guidance_h, GUIDANCE_H_MAX_BANK, guidance_indi_calcG(), guidance_indi_filter_accel(), guidance_indi_filter_attitude(), guidance_indi_filter_thrust(), guidance_indi_pos_gain, guidance_indi_speed_gain, guidance_v_z_ref, MAT33_INV, MAT33_VECT3_MUL, FloatEulers::phi, pitch_filt, HorizontalGuidanceReference::pos, POS_FLOAT_OF_BFP, FloatEulers::psi, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, HorizontalGuidance::ref, roll_filt, stabilization_attitude_set_setpoint_rp_quat_f(), stabilization_cmd, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), FloatEulers::theta, thrust_filt, thrust_in, RadioControl::values, FloatVect3::x, NedCoor_f::x, Int32Vect2::x, FloatVect3::y, NedCoor_f::y, Int32Vect2::y, FloatVect3::z, and NedCoor_f::z.

Referenced by guidance_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

void stabilization_attitude_set_setpoint_rp_quat_f	(	struct FloatEulers *	indi_rp_cmd,
		bool	in_flight,
		int32_t	heading
	)

Parameters

indi_rp_cmd	roll/pitch command from indi guidance [rad] (float)
in_flight	in flight boolean
heading	the desired heading [rad] in BFP with INT32_ANGLE_FRAC

function that creates a quaternion from a roll, pitch and yaw setpoint

Definition at line 287 of file guidance_indi.c.

References ANGLE_FLOAT_OF_BFP, float_quat_comp(), float_quat_comp_inv(), float_quat_comp_norm_shortest(), float_quat_normalize(), float_quat_of_axis_angle(), float_quat_of_eulers(), QUAT_BFP_OF_REAL, QUAT_COPY, stab_att_sp_quat, and stateGetNedToBodyEulers_f().

Referenced by guidance_indi_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

float guidance_indi_thrust_specific_force_gain