Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.
struct StabilizationSetpoint guidance_hybrid_run (void)
 Runs the Hybrid Guidance main functions.
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)
struct ThrustSetpoint guidance_hybrid_v_run_pos (bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_hybrid_v_run_speed (bool in_flight, struct VerticalGuidance *gv)
struct ThrustSetpoint guidance_hybrid_v_run_accel (bool in_flight, struct VerticalGuidance *gv)
void guidance_hybrid_airspeed_to_attitude (struct Int32Eulers *ypr_sp)
 Convert a required airspeed to a certain attitude for the Quadshot.
void guidance_hybrid_groundspeed_to_airspeed (void)
 Description.
void guidance_hybrid_determine_wind_estimate (void)
 Description.
struct StabilizationSetpoint guidance_hybrid_set_cmd_i (struct Int32Eulers *sp_cmd)
 Creates the attitude set-points from an orientation vector.
struct ThrustSetpoint guidance_hybrid_vertical (struct ThrustSetpoint *th)
 Take a thrust command as input and returns the modified value according to the current flight regime.

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

◆ AIRSPEED_HOVER

#define AIRSPEED_HOVER   4

◆ 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.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ 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.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ 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)

Convert a required airspeed to a certain attitude for the Quadshot.

Convert a required airspeed to a certain attitude for the Hybrid.

if required speed is lower than 4 m/s act like a rotorcraft

if required speed is higher than 4 m/s act like a fixedwing

Definition at line 206 of file guidance_hybrid.c.

References AIRSPEED_FORWARD, AIRSPEED_HOVER, airspeed_sp_heading_disp, ANGLE_BFP_OF_REAL, ANGLE_FLOAT_OF_BFP, FLOAT_ANGLE_NORMALIZE, FLOAT_OF_BFP, fwd_nominal_pitch, guidance_hybrid_airspeed_sp, guidance_hybrid_norm_ref_airspeed, guidance_hybrid_norm_ref_airspeed_f, guidance_hybrid_ref_airspeed, heading_diff_disp, high_res_psi, hover_p_gain, INT32_ANGLE_FRAC, INT32_ANGLE_HIGH_RES_FRAC, INT32_ANGLE_HIGH_RES_NORMALIZE, INT32_ANGLE_PI, INT32_POS_FRAC, INT32_TRIG_FRAC, int32_vect2_norm(), max_turn_bank, norm_sp_airspeed_disp, omega_disp, Int32Eulers::phi, POS_FLOAT_OF_BFP, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, Int32Eulers::theta, turn_bank_gain, v_control_pitch, Int32Vect2::x, and Int32Vect2::y.

Referenced by guidance_hybrid_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_determine_wind_estimate()

void guidance_hybrid_determine_wind_estimate ( void )

Description.

Definition at line 385 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.

Referenced by guidance_hybrid_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_groundspeed_to_airspeed()

void guidance_hybrid_groundspeed_to_airspeed ( void )

◆ guidance_hybrid_h_run_accel()

struct StabilizationSetpoint guidance_hybrid_h_run_accel ( bool in_flight UNUSED,
struct HorizontalGuidance *gh UNUSED )

Definition at line 179 of file guidance_hybrid.c.

References guidance_hybrid_h_run_accel(), StabilizationSetpoint::sp, and UNUSED.

Referenced by guidance_hybrid_h_run_accel(), and guidance_hybrid_h_run_accel().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_h_run_pos()

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

Definition at line 163 of file guidance_hybrid.c.

References guidance_h_pos_err, guidance_hybrid_groundspeed_sp, guidance_hybrid_h_run_pos(), guidance_hybrid_run(), horizontal_speed_gain, stateGetPositionNed_i(), UNUSED, VECT2_DIFF, and VECT2_SDIV.

Referenced by guidance_hybrid_h_run_pos().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_h_run_speed()

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

Definition at line 172 of file guidance_hybrid.c.

References guidance_hybrid_groundspeed_sp, guidance_hybrid_h_run_speed(), guidance_hybrid_run(), UNUSED, and VECT2_COPY.

Referenced by guidance_hybrid_h_run_speed().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_init()

◆ guidance_hybrid_run()

struct StabilizationSetpoint guidance_hybrid_run ( void )

Runs the Hybrid Guidance main functions.

Definition at line 155 of file guidance_hybrid.c.

References guidance_hybrid_airspeed_to_attitude(), guidance_hybrid_determine_wind_estimate(), guidance_hybrid_groundspeed_to_airspeed(), guidance_hybrid_run(), guidance_hybrid_set_cmd_i(), and guidance_hybrid_ypr_sp.

Referenced by guidance_hybrid_h_run_pos(), guidance_hybrid_h_run_speed(), and guidance_hybrid_run().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ 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 403 of file guidance_hybrid.c.

References ANGLE_FLOAT_OF_BFP, float_quat_of_orientation_vect(), guidance_hybrid_set_cmd_i(), int32_quat_comp(), int32_quat_of_axis_angle(), QUAT_BFP_OF_REAL, stab_sp_from_quat_i(), FloatVect3::x, FloatVect3::y, and FloatVect3::z.

Referenced by guidance_hybrid_h_run_accel(), guidance_hybrid_run(), and guidance_hybrid_set_cmd_i().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_v_run_accel()

struct ThrustSetpoint guidance_hybrid_v_run_accel ( bool in_flight,
struct VerticalGuidance * gv )

Definition at line 198 of file guidance_hybrid.c.

References guidance_hybrid_v_run_accel(), guidance_hybrid_vertical(), and guidance_pid_v_run_accel().

Referenced by guidance_hybrid_h_run_accel(), and guidance_hybrid_v_run_accel().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_v_run_pos()

struct ThrustSetpoint guidance_hybrid_v_run_pos ( bool in_flight,
struct VerticalGuidance * gv )

Definition at line 186 of file guidance_hybrid.c.

References guidance_hybrid_v_run_pos(), guidance_hybrid_vertical(), and guidance_pid_v_run_pos().

Referenced by guidance_hybrid_h_run_accel(), and guidance_hybrid_v_run_pos().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_v_run_speed()

struct ThrustSetpoint guidance_hybrid_v_run_speed ( bool in_flight,
struct VerticalGuidance * gv )

Definition at line 192 of file guidance_hybrid.c.

References guidance_hybrid_v_run_speed(), guidance_hybrid_vertical(), and guidance_pid_v_run_speed().

Referenced by guidance_hybrid_h_run_accel(), and guidance_hybrid_v_run_speed().

Here is the call graph for this function:
Here is the caller graph for this function:

◆ guidance_hybrid_vertical()

struct ThrustSetpoint guidance_hybrid_vertical ( struct ThrustSetpoint * th)

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

Description.

Definition at line 436 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_hybrid_vertical(), guidance_pid, guidance_v, MAX_PPRZ, th_sp_from_thrust_i(), th_sp_to_thrust_i(), THRUST_AXIS_Z, and v_control_pitch.

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

Here is the call graph for this function:
Here is the caller graph for this function:

◆ send_hybrid_guidance()

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

Definition at line 102 of file guidance_hybrid.c.

References dev, guidance_h_pos_err, guidance_hybrid_airspeed_sp, guidance_hybrid_norm_ref_airspeed, guidance_hybrid_ypr_sp, heading_diff_disp, stateGetPositionNed_i(), stateGetSpeedNed_i(), wind_estimate, NedCoor_i::x, and NedCoor_i::y.

Referenced by guidance_hybrid_init().

Here is the call graph for this function:
Here is the caller graph for this function:

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 84 of file guidance_hybrid.c.

Referenced by guidance_hybrid_h_run_pos(), and send_hybrid_guidance().

◆ guidance_hovering

bool guidance_hovering
static

Definition at line 95 of file guidance_hybrid.c.

Referenced by guidance_hybrid_init().

◆ guidance_hybrid_airspeed_ref

struct Int32Vect2 guidance_hybrid_airspeed_ref
static

Definition at line 85 of file guidance_hybrid.c.

Referenced by guidance_hybrid_init().

◆ 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

struct Int32Eulers guidance_hybrid_ypr_sp
static

◆ 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

int32_t high_res_psi
static

Definition at line 93 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), and guidance_hybrid_init().

◆ horizontal_speed_gain

int32_t horizontal_speed_gain

Definition at line 58 of file guidance_hybrid.c.

Referenced by guidance_hybrid_h_run_pos(), and guidance_hybrid_init().

◆ 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

◆ max_turn_bank

float max_turn_bank

Definition at line 59 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), and guidance_hybrid_init().

◆ 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

Definition at line 60 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), and guidance_hybrid_init().

◆ 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