guidance_indi.c File Reference

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

#include "generated/airframe.h"
#include "firmwares/rotorcraft/guidance/guidance_indi.h"
#include "subsystems/ins/ins_int.h"
#include "subsystems/radio_control.h"
#include "state.h"
#include "subsystems/imu.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/autopilot_rc_helpers.h"
#include "mcu_periph/sys_time.h"
#include "autopilot.h"
#include "stabilization/stabilization_attitude_ref_quat_int.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "stdio.h"
#include "filters/low_pass_filter.h"
#include "subsystems/abi.h"

Include dependency graph for guidance_indi.c:

Go to the source code of this file.

Macros
#define	GUIDANCE_INDI_FILTER_CUTOFF   3.0

Functions
static void	guidance_indi_propagate_filters (void)
	Low pass the accelerometer measurements to remove noise from vibrations. More...
static void	guidance_indi_calcG (struct FloatMat33 *Gmat)
void	guidance_indi_enter (void)
	Call upon entering indi guidance. More...
void	guidance_indi_run (bool in_flight, float heading_sp)
void	stabilization_attitude_set_setpoint_rp_quat_f (struct FloatEulers *indi_rp_cmd, bool in_flight, float heading)

Variables
float	guidance_indi_pos_gain = 0.5
float	guidance_indi_speed_gain = 1.8
struct FloatVect3	sp_accel = {0.0,0.0,0.0}
float	thrust_act = 0
Butterworth2LowPass	filt_accel_ned [3]
Butterworth2LowPass	roll_filt
Butterworth2LowPass	pitch_filt
Butterworth2LowPass	thrust_filt
struct FloatMat33	Ga
struct FloatMat33	Ga_inv
struct FloatVect3	euler_cmd
float	filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF
struct FloatEulers	guidance_euler_cmd
float	thrust_in

Detailed Description

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

Definition in file guidance_indi.c.

Macro Definition Documentation

#define GUIDANCE_INDI_FILTER_CUTOFF   3.0

Definition at line 83 of file guidance_indi.c.

Function Documentation

void guidance_indi_calcG ( struct FloatMat33 *  Gmat )

static

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 250 of file guidance_indi.c.

References FloatEulers::phi, FloatEulers::psi, RMAT_ELMT, stateGetNedToBodyEulers_f(), mesonh.mesonh_atmosphere::T, 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 109 of file guidance_indi.c.

References filter_cutoff, init_butterworth_2_low_pass(), FloatEulers::phi, stabilization_cmd, stateGetNedToBodyEulers_f(), FloatEulers::theta, thrust_act, and thrust_in.

Referenced by guidance_h_mode_changed().

Here is the call graph for this function:

Here is the caller graph for this function:

void guidance_indi_propagate_filters ( void  )

static

Low pass the accelerometer measurements to remove noise from vibrations.

The roll and pitch also need to be filtered to synchronize them with the acceleration

Definition at line 234 of file guidance_indi.c.

References stateGetAccelNed_f(), stateGetNedToBodyEulers_f(), update_butterworth_2_low_pass(), NedCoor_f::x, NedCoor_f::y, 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_run	(	bool	in_flight,
		float	heading_sp
	)

Parameters

in_flight	in flight boolean
heading_sp	the desired heading [rad]

main indi guidance function

Definition at line 129 of file guidance_indi.c.

References euler_cmd, Ga, Ga_inv, guidance_euler_cmd, guidance_h, GUIDANCE_H_MAX_BANK, guidance_indi_calcG(), guidance_indi_pos_gain, guidance_indi_propagate_filters(), guidance_indi_speed_gain, guidance_v_z_ref, MAT33_INV, MAT33_VECT3_MUL, SecondOrderLowPass::o, FloatEulers::phi, HorizontalGuidanceReference::pos, POS_FLOAT_OF_BFP, FloatEulers::psi, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, HorizontalGuidance::ref, stabilization_attitude_set_setpoint_rp_quat_f(), stabilization_cmd, stateGetNedToBodyEulers_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), FloatEulers::theta, thrust_in, THRUST_INCREMENT_ID, 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_from_nav(), and guidance_h_guided_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,
		float	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 281 of file guidance_indi.c.

References 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

struct FloatVect3 euler_cmd

Definition at line 95 of file guidance_indi.c.

Referenced by guidance_indi_run().

Butterworth2LowPass filt_accel_ned[3]

Definition at line 88 of file guidance_indi.c.

float filter_cutoff = GUIDANCE_INDI_FILTER_CUTOFF

Definition at line 97 of file guidance_indi.c.

Referenced by guidance_indi_enter().

struct FloatMat33 Ga

Definition at line 93 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatMat33 Ga_inv

Definition at line 94 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatEulers guidance_euler_cmd

Definition at line 99 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_pos_gain = 0.5

Definition at line 55 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_speed_gain = 1.8

Definition at line 61 of file guidance_indi.c.

Referenced by guidance_indi_run().

Butterworth2LowPass pitch_filt

Definition at line 90 of file guidance_indi.c.

Butterworth2LowPass roll_filt

Definition at line 89 of file guidance_indi.c.

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 64 of file guidance_indi.c.

float thrust_act = 0

Definition at line 87 of file guidance_indi.c.

Referenced by guidance_indi_enter().

Butterworth2LowPass thrust_filt

Definition at line 91 of file guidance_indi.c.

float thrust_in

Definition at line 100 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_run().