![]() |
Paparazzi UAS
v6.2_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
#include "std.h"
#include "math/pprz_algebra_int.h"
#include "math/pprz_algebra_float.h"
#include "filters/high_pass_filter.h"
Go to the source code of this file.
Data Structures | |
struct | guidance_indi_hybrid_params |
Functions | |
void | guidance_indi_enter (void) |
Call upon entering indi guidance. More... | |
void | guidance_indi_run (float *heading_sp) |
void | stabilization_attitude_set_setpoint_rp_quat_f (struct FloatEulers *indi_rp_cmd, bool in_flight, int32_t heading) |
void | guidance_indi_init (void) |
Init function. More... | |
void | guidance_indi_propagate_filters (void) |
Low pass the accelerometer measurements to remove noise from vibrations. More... | |
Variables | |
struct guidance_indi_hybrid_params | gih_params |
float | guidance_indi_specific_force_gain |
float | guidance_indi_max_airspeed |
float | nav_max_speed |
bool | take_heading_control |
float | guidance_indi_max_bank |
A guidance mode based on Incremental Nonlinear Dynamic Inversion Come to ICRA2016 to learn more!
Definition in file guidance_indi_hybrid.h.
struct guidance_indi_hybrid_params |
Definition at line 44 of file guidance_indi_hybrid.h.
Data Fields | ||
---|---|---|
float | heading_bank_gain | |
float | pos_gain | |
float | pos_gainz | |
float | speed_gain | |
float | speed_gainz |
void guidance_indi_enter | ( | void | ) |
Call upon entering indi guidance.
Definition at line 159 of file guidance_indi.c.
References accely_filt, filt_accel_ned, filter_cutoff, init_butterworth_2_low_pass(), FloatEulers::phi, pitch_filt, roll_filt, stabilization_cmd, stateGetNedToBodyEulers_f(), FloatEulers::theta, thrust_act, thrust_filt, and thrust_in.
Referenced by guidance_h_mode_changed().
void guidance_indi_init | ( | void | ) |
Init function.
Definition at line 146 of file guidance_indi.c.
References accel_sp_cb(), accel_sp_ev, accely_filt, DefaultPeriodic, filt_accel_ned, filter_cutoff, GUIDANCE_INDI_ACCEL_SP_ID, init_butterworth_2_low_pass(), pitch_filt, register_periodic_telemetry(), roll_filt, send_guidance_indi_hybrid(), send_indi_guidance(), and thrust_filt.
void guidance_indi_propagate_filters | ( | void | ) |
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 Called as a periodic function with PERIODIC_FREQ
Definition at line 481 of file guidance_indi_hybrid.c.
References ACCEL_FLOAT_OF_BFP, accely_filt, eulers_zxy, filt_accel_ned, FloatEulers::phi, pitch_filt, roll_filt, stateGetAccelBody_i(), stateGetAccelNed_f(), FloatEulers::theta, update_butterworth_2_low_pass(), NedCoor_f::x, NedCoor_f::y, and NedCoor_f::z.
Referenced by guidance_indi_run().
void guidance_indi_run | ( | float * | heading_sp | ) |
heading_sp | the desired heading [rad] |
main indi guidance function
Definition at line 179 of file guidance_indi.c.
References HorizontalGuidanceReference::accel, ACCEL_FLOAT_OF_BFP, accely_filt, ANGLE_BFP_OF_REAL, ANGLE_FLOAT_OF_BFP, AP_MODE_NAV, autopilot, BFP_OF_REAL, control_increment, desired_airspeed, euler_cmd, eulers_zxy, filt_accel_ned, FLOAT_ANGLE_NORMALIZE, float_eulers_of_quat_yxz(), float_eulers_of_quat_zxy(), float_quat_normalize(), float_quat_of_eulers_yxz(), float_quat_of_eulers_zxy(), FLOAT_VECT2_NORM, force_forward, Ga, Ga_inv, get_sys_time_float(), gih_params, guidance_euler_cmd, guidance_h, guidance_indi_calcg_wing(), guidance_indi_calcG_yxz(), guidance_indi_max_airspeed, guidance_indi_max_bank, guidance_indi_pos_gain, guidance_indi_propagate_filters(), guidance_indi_specific_force_gain, guidance_indi_speed_gain, guidance_v_z_ref, guidance_v_zd_ref, guidance_v_zdd_ref, guidance_indi_hybrid_params::heading_bank_gain, indi_accel_sp, indi_accel_sp_set_2d, indi_accel_sp_set_3d, INT32_ANGLE_FRAC, INT32_PERCENTAGE_FRAC, INT_MULT_RSHIFT, MAT33_INV, MAT33_VECT3_MUL, Min, pprz_autopilot::mode, nav_get_speed_setpoint(), nav_heading, SecondOrderLowPass::o, FloatEulers::phi, pitch_filt, HorizontalGuidanceReference::pos, POS_FLOAT_OF_BFP, guidance_indi_hybrid_params::pos_gain, guidance_indi_hybrid_params::pos_gainz, FloatEulers::psi, QUAT_BFP_OF_REAL, radio_control, RADIO_PITCH, RADIO_ROLL, RADIO_THROTTLE, HorizontalGuidance::ref, roll_filt, sp_accel, HorizontalGuidanceReference::speed, SPEED_FLOAT_OF_BFP, guidance_indi_hybrid_params::speed_gain, guidance_indi_hybrid_params::speed_gainz, speed_sp, stab_att_sp_quat, stabilization_cmd, stateGetAirspeed_f(), stateGetNedToBodyEulers_f(), stateGetNedToBodyQuat_f(), stateGetPositionNed_f(), stateGetPositionNed_i(), stateGetSpeedNed_f(), take_heading_control, FloatEulers::theta, thrust_filt, thrust_in, THRUST_INCREMENT_ID, time_of_accel_sp_2d, time_of_accel_sp_3d, transition_percentage, transition_theta_offset, RadioControl::values, VECT2_DIFF, vect_bound_in_2d(), FloatVect2::x, FloatVect3::x, NedCoor_f::x, Int32Vect2::x, FloatVect2::y, FloatVect3::y, NedCoor_f::y, Int32Vect2::y, FloatVect3::z, and NedCoor_f::z.
Referenced by guidance_h_from_nav(), and guidance_h_guided_run().
void stabilization_attitude_set_setpoint_rp_quat_f | ( | struct FloatEulers * | indi_rp_cmd, |
bool | in_flight, | ||
int32_t | heading | ||
) |
struct guidance_indi_hybrid_params gih_params |
Definition at line 64 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run(), nav_get_speed_sp_from_go(), and nav_get_speed_sp_from_line().
float guidance_indi_max_airspeed |
Definition at line 77 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().
float guidance_indi_max_bank |
Definition at line 111 of file guidance_indi.c.
Referenced by guidance_indi_run().
float guidance_indi_specific_force_gain |
float nav_max_speed |
Definition at line 81 of file guidance_indi_hybrid.c.
bool take_heading_control |
Definition at line 88 of file guidance_indi_hybrid.c.
Referenced by guidance_indi_run().