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

#include "firmwares/rotorcraft/guidance/guidance_hybrid.h"
#include "firmwares/rotorcraft/guidance/guidance_h.h"
#include "modules/radio_control/radio_control.h"
#include "firmwares/rotorcraft/stabilization/stabilization_attitude.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
#include "firmwares/rotorcraft/guidance/guidance_pid.h"
#include "modules/datalink/telemetry.h"

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)

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

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

    {             \
    while ((_a) > (INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC)))  (_a) -= (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC));    \
    while ((_a) < -(INT32_ANGLE_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_FRAC))) (_a) += (INT32_ANGLE_2_PI << (INT32_ANGLE_HIGH_RES_FRAC - INT32_ANGLE_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 )

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.

Here is the call 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.

Here is the call graph for this function:

◆ guidance_hybrid_groundspeed_to_airspeed()

void guidance_hybrid_groundspeed_to_airspeed ( void )

Description.

Definition at line 324 of file guidance_hybrid.c.

References force_forward_flight, guidance_hybrid_airspeed_sp, guidance_hybrid_groundspeed_sp, guidance_hybrid_ypr_sp, INT32_TRIG_FRAC, int32_vect2_norm(), INT_MULT_RSHIFT, INT_VECT2_ZERO, max_airspeed, POS_BFP_OF_REAL, POS_FLOAT_OF_BFP, PPRZ_ITRIG_COS, PPRZ_ITRIG_SIN, Int32Eulers::psi, VECT2_ADD, wind_estimate, Int32Vect2::x, and Int32Vect2::y.

Here is the call graph for this function:

◆ guidance_hybrid_h_run_accel()

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

Definition at line 123 of file guidance_hybrid.c.

References DefaultPeriodic, force_forward_flight, guidance_hovering, guidance_hybrid_airspeed_ref, guidance_hybrid_airspeed_sp, guidance_hybrid_groundspeed_sp, guidance_hybrid_norm_ref_airspeed, guidance_hybrid_norm_ref_airspeed_f, guidance_hybrid_ref_airspeed, guidance_hybrid_ypr_sp, high_res_psi, horizontal_speed_gain, INT_EULERS_ZERO, INT_VECT2_ZERO, max_turn_bank, register_periodic_telemetry(), send_hybrid_guidance(), turn_bank_gain, wind_estimate, wind_estimate_high_res, and wind_gain.

Here is the call 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 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_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_cmd The orientation vector

Todo:: calc sp_quat in fixed-point

Definition at line 385 of file guidance_hybrid.c.

◆ guidance_hybrid_v_run_accel()

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

Definition at line 123 of file guidance_hybrid.c.

◆ guidance_hybrid_v_run_pos()

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

Definition at line 123 of file guidance_hybrid.c.

◆ guidance_hybrid_v_run_speed()

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

Definition at line 123 of file guidance_hybrid.c.

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

◆ send_hybrid_guidance()

static 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, Int32Eulers::phi, Int32Eulers::psi, stateGetPositionNed_i(), stateGetSpeedNed_i(), Int32Eulers::theta, wind_estimate, Int32Vect2::x, NedCoor_i::x, Int32Vect2::y, and NedCoor_i::y.

Referenced by guidance_hybrid_h_run_accel().

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.

◆ force_forward_flight

bool force_forward_flight

Definition at line 96 of file guidance_hybrid.c.

Referenced by guidance_hybrid_groundspeed_to_airspeed(), and guidance_hybrid_h_run_accel().

◆ fwd_alt_thrust_gain

float fwd_alt_thrust_gain = FWD_ALT_THRUST_GAIN

Definition at line 74 of file guidance_hybrid.c.

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

◆ fwd_pitch_gain

float fwd_pitch_gain = FWD_PITCH_GAIN

Definition at line 77 of file guidance_hybrid.c.

◆ fwd_speed_p_gain

int32_t fwd_speed_p_gain = FWD_SPEED_P_GAIN

Definition at line 73 of file guidance_hybrid.c.

◆ 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

struct Int32Vect2 guidance_hybrid_airspeed_sp

static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), guidance_hybrid_groundspeed_to_airspeed(), guidance_hybrid_h_run_accel(), and send_hybrid_guidance().

◆ guidance_hybrid_groundspeed_sp

struct Int32Vect2 guidance_hybrid_groundspeed_sp

static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_groundspeed_to_airspeed(), and guidance_hybrid_h_run_accel().

◆ guidance_hybrid_norm_ref_airspeed

int32_t guidance_hybrid_norm_ref_airspeed

Definition at line 54 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), guidance_hybrid_h_run_accel(), and send_hybrid_guidance().

◆ guidance_hybrid_norm_ref_airspeed_f

float guidance_hybrid_norm_ref_airspeed_f

Definition at line 55 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), and guidance_hybrid_h_run_accel().

◆ guidance_hybrid_ref_airspeed

struct Int32Vect2 guidance_hybrid_ref_airspeed

static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude(), guidance_hybrid_determine_wind_estimate(), and guidance_hybrid_h_run_accel().

◆ guidance_hybrid_ypr_sp

struct Int32Eulers guidance_hybrid_ypr_sp

static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_groundspeed_to_airspeed(), guidance_hybrid_h_run_accel(), and send_hybrid_guidance().

◆ 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_h_run_accel().

◆ 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 compute_accel_from_speed_sp(), guidance_hybrid_groundspeed_to_airspeed(), and guidance_set_min_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_h_run_accel().

◆ 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_h_run_accel().

◆ v_control_pitch

int32_t v_control_pitch

static

Definition at line 97 of file guidance_hybrid.c.

Referenced by guidance_hybrid_airspeed_to_attitude().

◆ wind_estimate

struct Int32Vect2 wind_estimate

static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_determine_wind_estimate(), guidance_hybrid_groundspeed_to_airspeed(), guidance_hybrid_h_run_accel(), and send_hybrid_guidance().

◆ wind_estimate_high_res

struct Int32Vect2 wind_estimate_high_res

static

Definition at line 78 of file guidance_hybrid.c.

Referenced by guidance_hybrid_determine_wind_estimate(), and guidance_hybrid_h_run_accel().

◆ wind_gain

int32_t wind_gain

Definition at line 57 of file guidance_hybrid.c.

Referenced by guidance_hybrid_determine_wind_estimate(), and guidance_hybrid_h_run_accel().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ AIRSPEED_FORWARD

◆ AIRSPEED_HOVER

◆ CRUISE_THROTTLE

◆ FWD_ALT_THRUST_GAIN

◆ FWD_NOMINAL_PITCH

◆ FWD_PID_DIV

◆ FWD_PITCH_GAIN

◆ FWD_SPEED_P_GAIN

◆ HOVER_P_GAIN

◆ INT32_ANGLE_HIGH_RES_FRAC

◆ INT32_ANGLE_HIGH_RES_NORMALIZE

◆ MAX_AIRSPEED

Function Documentation

◆ guidance_hybrid_airspeed_to_attitude()

◆ guidance_hybrid_determine_wind_estimate()

◆ guidance_hybrid_groundspeed_to_airspeed()

◆ guidance_hybrid_h_run_accel()

◆ guidance_hybrid_h_run_pos()

◆ guidance_hybrid_h_run_speed()

◆ guidance_hybrid_init()

◆ guidance_hybrid_run()

◆ guidance_hybrid_set_cmd_i()

◆ guidance_hybrid_v_run_accel()

◆ guidance_hybrid_v_run_pos()

◆ guidance_hybrid_v_run_speed()

◆ guidance_hybrid_vertical()

◆ send_hybrid_guidance()

Variable Documentation

◆ airspeed_sp_heading_disp

◆ cruise_throttle

◆ force_forward_flight

◆ fwd_alt_thrust_gain

◆ fwd_nominal_pitch

◆ fwd_pid_div

◆ fwd_pitch_gain

◆ fwd_speed_p_gain

◆ guidance_h_pos_err

◆ guidance_hovering

◆ guidance_hybrid_airspeed_ref

◆ guidance_hybrid_airspeed_sp

◆ guidance_hybrid_groundspeed_sp

◆ guidance_hybrid_norm_ref_airspeed

◆ guidance_hybrid_norm_ref_airspeed_f

◆ guidance_hybrid_ref_airspeed

◆ guidance_hybrid_ypr_sp

◆ heading_diff_disp

◆ high_res_psi

◆ horizontal_speed_gain

◆ hover_p_gain

◆ max_airspeed

◆ max_turn_bank

◆ norm_sp_airspeed_disp

◆ omega_disp

◆ turn_bank_gain

◆ v_control_pitch

◆ wind_estimate

◆ wind_estimate_high_res

◆ wind_gain