guidance_indi.c File Reference

A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 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 dependency graph for guidance_indi.c:

Go to the source code of this file.

Functions
void	guidance_indi_enter (void)
void	guidance_indi_run (bool_t in_flight, int32_t heading)
void	guidance_indi_filter_accel (void)
void	guidance_indi_filter_attitude (void)
void	guidance_indi_calcG (struct FloatMat33 *Gmat)
void	stabilization_attitude_set_setpoint_rp_quat_f (bool_t in_flight, int32_t 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}
struct FloatVect3	filt_accel_ned
struct FloatVect3	filt_accel_ned_d
struct FloatVect3	filt_accel_ned_dd
float	filt_accelzbody = 0
float	filt_accelzbodyd = 0
float	filt_accelzbodydd = 0
float	roll_filt = 0
float	roll_filtd = 0
float	roll_filtdd = 0
float	pitch_filt = 0
float	pitch_filtd = 0
float	pitch_filtdd = 0
struct FloatMat33	Ga
struct FloatMat33	Ga_inv
struct FloatVect3	euler_cmd
float	filter_omega = 20.0
float	filter_zeta = 0.65
struct FloatEulers	guidance_euler_cmd

Detailed Description

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

Definition in file guidance_indi.c.

Function Documentation

void guidance_indi_calcG

(

struct FloatMat33 *

Gmat

)

Definition at line 162 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

)

Definition at line 70 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, and roll_filtdd.

Referenced by guidance_h_init(), and guidance_h_mode_changed().

Here is the caller graph for this function:

void guidance_indi_filter_accel

(

void

)

Definition at line 134 of file guidance_indi.c.

References filt_accel_ned, filt_accel_ned_d, filt_accel_ned_dd, filter_omega, filter_zeta, PERIODIC_FREQUENCY, 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

)

Definition at line 148 of file guidance_indi.c.

References filter_omega, filter_zeta, PERIODIC_FREQUENCY, 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_run	(	bool_t	in_flight,
		int32_t	heading
	)

Definition at line 85 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_pos_gain, guidance_indi_speed_gain, guidance_v_z_sp, MAT33_INV, MAT33_VECT3_MUL, FloatEulers::phi, pitch_filt, HorizontalGuidanceReference::pos, POS_FLOAT_OF_BFP, FloatEulers::psi, HorizontalGuidance::ref, roll_filt, stabilization_attitude_set_setpoint_rp_quat_f(), stateGetPositionNed_f(), stateGetSpeedNed_f(), FloatEulers::theta, 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	(	bool_t	in_flight,
		int32_t	heading
	)

Definition at line 187 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(), guidance_euler_cmd, 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 63 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatVect3 filt_accel_ned

Definition at line 48 of file guidance_indi.c.

Referenced by guidance_indi_enter(), guidance_indi_filter_accel(), and guidance_indi_run().

struct FloatVect3 filt_accel_ned_d

Definition at line 49 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_accel().

struct FloatVect3 filt_accel_ned_dd

Definition at line 50 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_accel().

float filt_accelzbody = 0

Definition at line 51 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float filt_accelzbodyd = 0

Definition at line 52 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float filt_accelzbodydd = 0

Definition at line 53 of file guidance_indi.c.

Referenced by guidance_indi_enter().

float filter_omega = 20.0

Definition at line 65 of file guidance_indi.c.

Referenced by guidance_indi_filter_accel(), and guidance_indi_filter_attitude().

float filter_zeta = 0.65

Definition at line 66 of file guidance_indi.c.

Referenced by guidance_indi_filter_accel(), and guidance_indi_filter_attitude().

struct FloatMat33 Ga

Definition at line 61 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatMat33 Ga_inv

Definition at line 62 of file guidance_indi.c.

Referenced by guidance_indi_run().

struct FloatEulers guidance_euler_cmd

Definition at line 68 of file guidance_indi.c.

Referenced by guidance_indi_run(), and stabilization_attitude_set_setpoint_rp_quat_f().

float guidance_indi_pos_gain = 0.5

Definition at line 44 of file guidance_indi.c.

Referenced by guidance_indi_run().

float guidance_indi_speed_gain = 1.8

Definition at line 45 of file guidance_indi.c.

Referenced by guidance_indi_run().

float pitch_filt = 0

Definition at line 57 of file guidance_indi.c.

Referenced by guidance_indi_enter(), guidance_indi_filter_attitude(), and guidance_indi_run().

float pitch_filtd = 0

Definition at line 58 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

float pitch_filtdd = 0

Definition at line 59 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

float roll_filt = 0

Definition at line 54 of file guidance_indi.c.

Referenced by guidance_indi_enter(), guidance_indi_filter_attitude(), and guidance_indi_run().

float roll_filtd = 0

Definition at line 55 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

float roll_filtdd = 0

Definition at line 56 of file guidance_indi.c.

Referenced by guidance_indi_enter(), and guidance_indi_filter_attitude().

struct FloatVect3 sp_accel = {0.0,0.0,0.0}

Definition at line 46 of file guidance_indi.c.