Paparazzi UAS  v7.0_unstable
Paparazzi is a free software Unmanned Aircraft System.
guidance_hybrid.c File Reference

Guidance controllers (horizontal and vertical) for Hybrid UAV configurations. More...

+ Include dependency graph for guidance_hybrid.c:

Go to the source code of this file.

Macros

#define MAX_AIRSPEED   15
 
#define INT32_ANGLE_HIGH_RES_FRAC   18
 
#define AIRSPEED_HOVER   4
 
#define AIRSPEED_FORWARD   12
 
#define CRUISE_THROTTLE   4000
 
#define FWD_SPEED_P_GAIN   30
 
#define FWD_ALT_THRUST_GAIN   0.35
 
#define FWD_PID_DIV   2
 
#define FWD_NOMINAL_PITCH   78.0
 
#define FWD_PITCH_GAIN   2.1
 
#define HOVER_P_GAIN   12
 
#define INT32_ANGLE_HIGH_RES_NORMALIZE(_a)
 

Functions

static void send_hybrid_guidance (struct transport_tx *trans, struct link_device *dev)
 
void guidance_hybrid_init (void)
 Hybrid Guidance Initialization function. More...
 
struct StabilizationSetpoint guidance_hybrid_run (void)
 Runs the Hybrid Guidance main functions. More...
 
struct StabilizationSetpoint guidance_hybrid_h_run_pos (bool in_flight UNUSED, struct HorizontalGuidance *gh)
 
struct StabilizationSetpoint guidance_hybrid_h_run_speed (bool in_flight UNUSED, struct HorizontalGuidance *gh)
 
struct StabilizationSetpoint guidance_hybrid_h_run_accel (bool in_flight UNUSED, struct HorizontalGuidance *gh UNUSED)
 
int32_t guidance_hybrid_v_run_pos (bool in_flight, struct VerticalGuidance *gv)
 
int32_t guidance_hybrid_v_run_speed (bool in_flight, struct VerticalGuidance *gv)
 
int32_t guidance_hybrid_v_run_accel (bool in_flight, struct VerticalGuidance *gv)
 
void guidance_hybrid_reset_heading (struct Int32Eulers *sp_cmd)
 Description. More...
 
void guidance_hybrid_airspeed_to_attitude (struct Int32Eulers *ypr_sp)
 Convert a required airspeed to a certain attitude for the Quadshot. More...
 
void guidance_hybrid_groundspeed_to_airspeed (void)
 Description. More...
 
void guidance_hybrid_determine_wind_estimate (void)
 Description. More...
 
struct StabilizationSetpoint guidance_hybrid_set_cmd_i (struct Int32Eulers *sp_cmd)
 Creates the attitude set-points from an orientation vector. More...
 
int32_t guidance_hybrid_vertical (int32_t delta_t)
 Take a thrust command as input and returns the modified value according to the current flight regime. More...
 

Variables

int32_t guidance_hybrid_norm_ref_airspeed
 
float guidance_hybrid_norm_ref_airspeed_f
 
int32_t max_airspeed = MAX_AIRSPEED
 
int32_t wind_gain
 
int32_t horizontal_speed_gain
 
float max_turn_bank
 
float turn_bank_gain
 
int32_t cruise_throttle = CRUISE_THROTTLE
 
int32_t fwd_speed_p_gain = FWD_SPEED_P_GAIN
 
float fwd_alt_thrust_gain = FWD_ALT_THRUST_GAIN
 
float fwd_pid_div = FWD_PID_DIV
 
float fwd_nominal_pitch = FWD_NOMINAL_PITCH
 
float fwd_pitch_gain = FWD_PITCH_GAIN
 
int32_t hover_p_gain = HOVER_P_GAIN
 
static struct Int32Vect2 guidance_hybrid_groundspeed_sp
 
static struct Int32Eulers guidance_hybrid_ypr_sp
 
static struct Int32Vect2 guidance_hybrid_airspeed_sp
 
static struct Int32Vect2 guidance_h_pos_err
 
static struct Int32Vect2 guidance_hybrid_airspeed_ref
 
static struct Int32Vect2 wind_estimate
 
static struct Int32Vect2 wind_estimate_high_res
 
static struct Int32Vect2 guidance_hybrid_ref_airspeed
 
static int32_t norm_sp_airspeed_disp
 
static int32_t heading_diff_disp
 
static int32_t omega_disp
 
static int32_t high_res_psi
 
static int32_t airspeed_sp_heading_disp
 
static bool guidance_hovering
 
bool force_forward_flight
 
static int32_t v_control_pitch
 

Detailed Description

Guidance controllers (horizontal and vertical) for Hybrid UAV configurations.

Functionality: 1) hover with (helicopter) thrust vectoring and align the heading with the wind vector. 2) Forward flight with using pitch and a bit of thrust to control altitude and heading to control the velocity vector 3) Transition between the two, with the possibility to fly at any airspeed

Definition in file guidance_hybrid.c.

Macro Definition Documentation

◆ AIRSPEED_FORWARD

#define AIRSPEED_FORWARD   12

Definition at line 63 of file guidance_hybrid.c.

◆ AIRSPEED_HOVER

#define AIRSPEED_HOVER   4

Definition at line 62 of file guidance_hybrid.c.

◆ CRUISE_THROTTLE

#define CRUISE_THROTTLE   4000

Definition at line 64 of file guidance_hybrid.c.

◆ FWD_ALT_THRUST_GAIN

#define FWD_ALT_THRUST_GAIN   0.35

Definition at line 66 of file guidance_hybrid.c.

◆ FWD_NOMINAL_PITCH

#define FWD_NOMINAL_PITCH   78.0

Definition at line 68 of file guidance_hybrid.c.

◆ FWD_PID_DIV

#define FWD_PID_DIV   2

Definition at line 67 of file guidance_hybrid.c.

◆ FWD_PITCH_GAIN

#define FWD_PITCH_GAIN   2.1

Definition at line 69 of file guidance_hybrid.c.

◆ FWD_SPEED_P_GAIN

#define FWD_SPEED_P_GAIN   30

Definition at line 65 of file guidance_hybrid.c.

◆ HOVER_P_GAIN

#define HOVER_P_GAIN   12

Definition at line 70 of file guidance_hybrid.c.

◆ INT32_ANGLE_HIGH_RES_FRAC

#define INT32_ANGLE_HIGH_RES_FRAC   18

Definition at line 51 of file guidance_hybrid.c.

◆ INT32_ANGLE_HIGH_RES_NORMALIZE

#define INT32_ANGLE_HIGH_RES_NORMALIZE (   _a)
Value:
{ \
}
#define INT32_ANGLE_2_PI
#define INT32_ANGLE_PI
#define INT32_ANGLE_FRAC
#define INT32_ANGLE_HIGH_RES_FRAC

Definition at line 150 of file guidance_hybrid.c.

◆ MAX_AIRSPEED

#define MAX_AIRSPEED   15

Definition at line 47 of file guidance_hybrid.c.

Function Documentation

◆ guidance_hybrid_airspeed_to_attitude()

void guidance_hybrid_airspeed_to_attitude ( struct Int32Eulers ypr_sp)

◆ guidance_hybrid_determine_wind_estimate()

void guidance_hybrid_determine_wind_estimate ( void  )

Description.

Definition at line 392 of file guidance_hybrid.c.

References guidance_hybrid_ref_airspeed, INT32_VECT2_RSHIFT, stateGetSpeedNed_i(), VECT2_DIFF, wind_estimate, wind_estimate_high_res, wind_gain, Int32Vect2::x, and Int32Vect2::y.

+ Here is the call graph for this function:

◆ guidance_hybrid_groundspeed_to_airspeed()

◆ guidance_hybrid_h_run_accel()

◆ guidance_hybrid_h_run_pos()

struct StabilizationSetpoint guidance_hybrid_h_run_pos ( bool in_flight  UNUSED,
struct HorizontalGuidance gh 
)

Definition at line 123 of file guidance_hybrid.c.

◆ guidance_hybrid_h_run_speed()

struct StabilizationSetpoint guidance_hybrid_h_run_speed ( bool in_flight  UNUSED,
struct HorizontalGuidance gh 
)

Definition at line 123 of file guidance_hybrid.c.

◆ guidance_hybrid_init()

void guidance_hybrid_init ( void  )

Hybrid Guidance Initialization function.

Parameters

Definition at line 123 of file guidance_hybrid.c.

◆ guidance_hybrid_reset_heading()

void guidance_hybrid_reset_heading ( struct Int32Eulers sp_cmd)

Description.

Parameters
sp_cmdAdd Description

Definition at line 205 of file guidance_hybrid.c.

References guidance_hybrid_ypr_sp, high_res_psi, INT32_ANGLE_FRAC, INT32_ANGLE_HIGH_RES_FRAC, Int32Eulers::psi, and stabilization_attitude_set_rpy_setpoint_i().

+ Here is the call graph for this function:

◆ guidance_hybrid_run()

struct StabilizationSetpoint guidance_hybrid_run ( void  )

Runs the Hybrid Guidance main functions.

Definition at line 123 of file guidance_hybrid.c.

◆ guidance_hybrid_set_cmd_i()

struct StabilizationSetpoint guidance_hybrid_set_cmd_i ( struct Int32Eulers sp_cmd)

Creates the attitude set-points from an orientation vector.

Parameters
sp_cmdThe orientation vector
Todo:
calc sp_quat in fixed-point

Definition at line 392 of file guidance_hybrid.c.

◆ guidance_hybrid_v_run_accel()

int32_t guidance_hybrid_v_run_accel ( bool  in_flight,
struct VerticalGuidance gv 
)

Definition at line 198 of file guidance_hybrid.c.

References guidance_hybrid_vertical(), and guidance_pid_v_run_accel().

+ Here is the call graph for this function:

◆ guidance_hybrid_v_run_pos()

int32_t guidance_hybrid_v_run_pos ( bool  in_flight,
struct VerticalGuidance gv 
)

Definition at line 186 of file guidance_hybrid.c.

References guidance_hybrid_vertical(), and guidance_pid_v_run_pos().

+ Here is the call graph for this function:

◆ guidance_hybrid_v_run_speed()

int32_t guidance_hybrid_v_run_speed ( bool  in_flight,
struct VerticalGuidance gv 
)

Definition at line 192 of file guidance_hybrid.c.

References guidance_hybrid_vertical(), and guidance_pid_v_run_speed().

+ Here is the call graph for this function:

◆ guidance_hybrid_vertical()

int32_t guidance_hybrid_vertical ( int32_t  delta_t)

Take a thrust command as input and returns the modified value according to the current flight regime.

Description.

Definition at line 443 of file guidance_hybrid.c.

References AIRSPEED_FORWARD, AIRSPEED_HOVER, ANGLE_BFP_OF_REAL, cruise_throttle, fwd_alt_thrust_gain, fwd_pid_div, fwd_pitch_gain, fwd_speed_p_gain, guidance_hybrid_norm_ref_airspeed_f, guidance_pid, guidance_v, GuidancePID::kd, GuidancePID::ki, GuidancePID::kp, MAX_PPRZ, VerticalGuidance::nominal_throttle, and v_control_pitch.

Referenced by guidance_hybrid_v_run_accel(), guidance_hybrid_v_run_pos(), and guidance_hybrid_v_run_speed().

+ Here is the caller graph for this function:

◆ send_hybrid_guidance()

static void send_hybrid_guidance ( struct transport_tx *  trans,
struct link_device *  dev 
)
static

Variable Documentation

◆ airspeed_sp_heading_disp

int32_t airspeed_sp_heading_disp
static

Definition at line 94 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ cruise_throttle

int32_t cruise_throttle = CRUISE_THROTTLE

Definition at line 72 of file guidance_hybrid.c.

Referenced by guidance_hybrid_vertical().

◆ force_forward_flight

bool force_forward_flight

◆ fwd_alt_thrust_gain

float fwd_alt_thrust_gain = FWD_ALT_THRUST_GAIN

Definition at line 74 of file guidance_hybrid.c.

Referenced by guidance_hybrid_vertical().

◆ fwd_nominal_pitch

float fwd_nominal_pitch = FWD_NOMINAL_PITCH

Definition at line 76 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ fwd_pid_div

float fwd_pid_div = FWD_PID_DIV

Definition at line 75 of file guidance_hybrid.c.

Referenced by guidance_hybrid_vertical().

◆ fwd_pitch_gain

float fwd_pitch_gain = FWD_PITCH_GAIN

Definition at line 77 of file guidance_hybrid.c.

Referenced by guidance_hybrid_vertical().

◆ fwd_speed_p_gain

int32_t fwd_speed_p_gain = FWD_SPEED_P_GAIN

Definition at line 73 of file guidance_hybrid.c.

Referenced by guidance_hybrid_vertical().

◆ guidance_h_pos_err

struct Int32Vect2 guidance_h_pos_err
static

Definition at line 78 of file guidance_hybrid.c.

Referenced by send_hybrid_guidance().

◆ guidance_hovering

bool guidance_hovering
static

Definition at line 95 of file guidance_hybrid.c.

Referenced by guidance_hybrid_h_run_accel().

◆ guidance_hybrid_airspeed_ref

struct Int32Vect2 guidance_hybrid_airspeed_ref
static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_h_run_accel().

◆ guidance_hybrid_airspeed_sp

◆ guidance_hybrid_groundspeed_sp

struct Int32Vect2 guidance_hybrid_groundspeed_sp
static

◆ guidance_hybrid_norm_ref_airspeed

int32_t guidance_hybrid_norm_ref_airspeed

◆ guidance_hybrid_norm_ref_airspeed_f

float guidance_hybrid_norm_ref_airspeed_f

◆ guidance_hybrid_ref_airspeed

struct Int32Vect2 guidance_hybrid_ref_airspeed
static

◆ guidance_hybrid_ypr_sp

◆ heading_diff_disp

int32_t heading_diff_disp
static

Definition at line 91 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), and send_hybrid_guidance().

◆ high_res_psi

◆ horizontal_speed_gain

int32_t horizontal_speed_gain

Definition at line 58 of file guidance_hybrid.c.

Referenced by guidance_hybrid_h_run_accel().

◆ hover_p_gain

int32_t hover_p_gain = HOVER_P_GAIN

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ max_airspeed

int32_t max_airspeed = MAX_AIRSPEED

Definition at line 56 of file guidance_hybrid.c.

Referenced by guidance_hybrid_groundspeed_to_airspeed().

◆ max_turn_bank

float max_turn_bank

◆ norm_sp_airspeed_disp

int32_t norm_sp_airspeed_disp
static

Definition at line 90 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ omega_disp

int32_t omega_disp
static

Definition at line 92 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ turn_bank_gain

float turn_bank_gain

◆ v_control_pitch

int32_t v_control_pitch
static

◆ wind_estimate

◆ wind_estimate_high_res

struct Int32Vect2 wind_estimate_high_res
static

◆ wind_gain

int32_t wind_gain