Guidance controller with PID for rotorcrafts. More...

#include "firmwares/rotorcraft/guidance/guidance_pid.h"
#include "firmwares/rotorcraft/guidance/guidance_v_adapt.h"
#include "firmwares/rotorcraft/stabilization.h"
#include "generated/airframe.h"
#include "state.h"
#include "modules/datalink/telemetry.h"

Include dependency graph for guidance_pid.c:

Go to the source code of this file.

Macros
#define	GUIDANCE_H_AGAIN 0

#define	GUIDANCE_H_VGAIN 0

#define	GUIDANCE_H_THRUST_CMD_FILTER 10

#define	GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE

#define	MAX_POS_ERR POS_BFP_OF_REAL(16.)

#define	MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

#define	GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE

#define	GUIDANCE_V_MIN_ERR_Z POS_BFP_OF_REAL(-10.)

#define	GUIDANCE_V_MAX_ERR_Z POS_BFP_OF_REAL(10.)

#define	GUIDANCE_V_MIN_ERR_ZD SPEED_BFP_OF_REAL(-10.)

#define	GUIDANCE_V_MAX_ERR_ZD SPEED_BFP_OF_REAL(10.)

#define	GUIDANCE_V_MAX_SUM_ERR 2000000

#define	GUIDANCE_V_MAX_CMD 0.9*MAX_PPRZ

#define	GUIDANCE_PID_USE_AS_DEFAULT TRUE
	The PID controller is used by default. More...

#define	GH_GAIN_SCALE 2

#define	FF_CMD_FRAC 18
	run vertical control loop for position and speed control More...

Functions
static void	send_hover_loop (struct transport_tx trans, struct link_device dev)

static void	send_vert_loop (struct transport_tx trans, struct link_device dev)

void	guidance_pid_init (void)

static struct StabilizationSetpoint	guidance_pid_h_run (bool in_flight, struct HorizontalGuidance *gh)
	run horizontal control loop for position and speed control More...

struct StabilizationSetpoint	guidance_pid_h_run_pos (bool in_flight, struct HorizontalGuidance *gh)

struct StabilizationSetpoint	guidance_pid_h_run_speed (bool in_flight, struct HorizontalGuidance *gh)

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

static struct ThrustSetpoint	guidance_pid_v_run (bool in_flight, struct VerticalGuidance *gv)

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

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

struct ThrustSetpoint	guidance_pid_v_run_accel (bool in_flight UNUSED, struct VerticalGuidance *gv UNUSED)

void	guidance_pid_h_enter (void)

void	guidance_pid_v_enter (void)

void	guidance_pid_set_h_igain (uint32_t igain)
	settings handler More...

void	guidance_pid_set_v_igain (uint32_t igain)

const struct Int32Vect2 *	guidance_pid_get_h_pos_err (void)
	Gets the position error. More...

Variables
struct GuidancePID	guidance_pid
	Guidance PID structyre. More...

struct Int32Vect2	guidance_pid_pos_err

struct Int32Vect2	guidance_pid_speed_err

struct Int32Vect2	guidance_pid_trim_att_integrator

int32_t	guidance_pid_z_sum_err
	accumulator for I-gain More...

int32_t	guidance_pid_v_ff_cmd
	feed-forward command More...

int32_t	guidance_pid_v_fb_cmd
	feed-back command More...

Detailed Description

Guidance controller with PID for rotorcrafts.

Definition in file guidance_pid.c.

Macro Definition Documentation

◆ FF_CMD_FRAC

#define FF_CMD_FRAC 18

run vertical control loop for position and speed control

Definition at line 319 of file guidance_pid.c.

◆ GH_GAIN_SCALE

#define GH_GAIN_SCALE 2

Definition at line 213 of file guidance_pid.c.

◆ GUIDANCE_H_AGAIN

#define GUIDANCE_H_AGAIN 0

Definition at line 36 of file guidance_pid.c.

◆ GUIDANCE_H_APPROX_FORCE_BY_THRUST

#define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE

Definition at line 57 of file guidance_pid.c.

◆ GUIDANCE_H_THRUST_CMD_FILTER

#define GUIDANCE_H_THRUST_CMD_FILTER 10

Definition at line 53 of file guidance_pid.c.

◆ GUIDANCE_H_VGAIN

#define GUIDANCE_H_VGAIN 0

Definition at line 40 of file guidance_pid.c.

◆ GUIDANCE_PID_USE_AS_DEFAULT

#define GUIDANCE_PID_USE_AS_DEFAULT TRUE

The PID controller is used by default.

Definition at line 117 of file guidance_pid.c.

◆ GUIDANCE_V_ADAPT_THROTTLE_ENABLED

#define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE

Definition at line 83 of file guidance_pid.c.

◆ GUIDANCE_V_MAX_CMD

#define GUIDANCE_V_MAX_CMD 0.9*MAX_PPRZ

Definition at line 109 of file guidance_pid.c.

◆ GUIDANCE_V_MAX_ERR_Z

#define GUIDANCE_V_MAX_ERR_Z POS_BFP_OF_REAL(10.)

Definition at line 93 of file guidance_pid.c.

◆ GUIDANCE_V_MAX_ERR_ZD

#define GUIDANCE_V_MAX_ERR_ZD SPEED_BFP_OF_REAL(10.)

Definition at line 101 of file guidance_pid.c.

◆ GUIDANCE_V_MAX_SUM_ERR

#define GUIDANCE_V_MAX_SUM_ERR 2000000

Definition at line 105 of file guidance_pid.c.

◆ GUIDANCE_V_MIN_ERR_Z

#define GUIDANCE_V_MIN_ERR_Z POS_BFP_OF_REAL(-10.)

Definition at line 89 of file guidance_pid.c.

◆ GUIDANCE_V_MIN_ERR_ZD

#define GUIDANCE_V_MIN_ERR_ZD SPEED_BFP_OF_REAL(-10.)

Definition at line 97 of file guidance_pid.c.

◆ MAX_POS_ERR

#define MAX_POS_ERR POS_BFP_OF_REAL(16.)

Definition at line 61 of file guidance_pid.c.

◆ MAX_SPEED_ERR

#define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.)

Definition at line 62 of file guidance_pid.c.

Function Documentation

◆ guidance_pid_get_h_pos_err()

const struct Int32Vect2* guidance_pid_get_h_pos_err ( void )

Gets the position error.

Parameters

none.

Returns: Pointer to a structure containing x and y position errors

Definition at line 417 of file guidance_pid.c.

References guidance_pid_pos_err.

◆ guidance_pid_h_enter()

void guidance_pid_h_enter ( void )

Definition at line 389 of file guidance_pid.c.

References RotorcraftNavigation::heading, nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().

Here is the call graph for this function:

◆ guidance_pid_h_run()

static struct StabilizationSetpoint guidance_pid_h_run	(	bool	in_flight,
		struct HorizontalGuidance *	gh
	)

static

run horizontal control loop for position and speed control

Definition at line 184 of file guidance_pid.c.

References GuidancePID::adapt_throttle_enabled, GuidancePID::approx_force_by_thrust, GuidancePID::cmd_earth, GuidancePID::cmd_thrust, DefaultPeriodic, GUIDANCE_H_AGAIN, GUIDANCE_H_APPROX_FORCE_BY_THRUST, GUIDANCE_H_VGAIN, guidance_pid, guidance_pid_pos_err, guidance_pid_speed_err, guidance_pid_trim_att_integrator, guidance_pid_v_fb_cmd, guidance_pid_v_ff_cmd, guidance_pid_z_sum_err, GUIDANCE_V_ADAPT_THROTTLE_ENABLED, INT_VECT2_ZERO, GuidancePID::ka, GuidancePID::kd, GuidancePID::ki, GuidancePID::kp, GuidancePID::kv, register_periodic_telemetry(), send_hover_loop(), send_vert_loop(), GuidancePID::v_kd, GuidancePID::v_ki, and GuidancePID::v_kp.

Here is the call graph for this function:

◆ guidance_pid_h_run_accel()

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

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_h_run_pos()

struct StabilizationSetpoint guidance_pid_h_run_pos	(	bool	in_flight,
		struct HorizontalGuidance *	gh
	)

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_h_run_speed()

struct StabilizationSetpoint guidance_pid_h_run_speed	(	bool	in_flight,
		struct HorizontalGuidance *	gh
	)

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_init()

void guidance_pid_init ( void )

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_set_h_igain()

void guidance_pid_set_h_igain ( uint32_t igain )

settings handler

Definition at line 404 of file guidance_pid.c.

References guidance_pid, guidance_pid_trim_att_integrator, INT_VECT2_ZERO, and GuidancePID::ki.

◆ guidance_pid_set_v_igain()

void guidance_pid_set_v_igain ( uint32_t igain )

Definition at line 410 of file guidance_pid.c.

References guidance_pid, guidance_pid_z_sum_err, and GuidancePID::v_ki.

◆ guidance_pid_v_enter()

void guidance_pid_v_enter ( void )

Definition at line 395 of file guidance_pid.c.

References guidance_pid_z_sum_err.

◆ guidance_pid_v_run()

static struct ThrustSetpoint guidance_pid_v_run	(	bool	in_flight,
		struct VerticalGuidance *	gv
	)

static

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_v_run_accel()

struct ThrustSetpoint guidance_pid_v_run_accel	(	bool in_flight	UNUSED,
		struct VerticalGuidance *gv	UNUSED
	)

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_v_run_pos()

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

Definition at line 184 of file guidance_pid.c.

◆ guidance_pid_v_run_speed()

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

Definition at line 184 of file guidance_pid.c.

◆ send_hover_loop()

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

static

Definition at line 142 of file guidance_pid.c.

References GuidancePID::cmd_earth, dev, guidance_h, guidance_pid, guidance_pid_pos_err, guidance_pid_speed_err, guidance_pid_trim_att_integrator, HorizontalGuidanceSetpoint::heading, HorizontalGuidanceSetpoint::pos, HorizontalGuidance::sp, stateGetAccelNed_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), Int32Vect2::x, NedCoor_i::x, Int32Vect2::y, and NedCoor_i::y.

Referenced by guidance_pid_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

◆ send_vert_loop()

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

static

Definition at line 164 of file guidance_pid.c.

References GuidancePID::cmd_thrust, dev, guidance_pid, guidance_pid_v_fb_cmd, guidance_pid_v_ff_cmd, guidance_pid_z_sum_err, guidance_v, gv_adapt_P, gv_adapt_X, gv_adapt_Xmeas, stateGetAccelNed_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), NedCoor_i::z, VerticalGuidance::z_ref, VerticalGuidance::z_sp, VerticalGuidance::zd_ref, VerticalGuidance::zd_sp, and VerticalGuidance::zdd_ref.

Referenced by guidance_pid_h_run().

Here is the call graph for this function:

Here is the caller graph for this function:

Variable Documentation

◆ guidance_pid

struct GuidancePID guidance_pid

Guidance PID structyre.

Definition at line 1 of file guidance_pid.c.

Referenced by guidance_pid_h_run(), guidance_pid_set_h_igain(), guidance_pid_set_v_igain(), send_hover_loop(), and send_vert_loop().

◆ guidance_pid_pos_err

struct Int32Vect2 guidance_pid_pos_err

Definition at line 1 of file guidance_pid.c.

Referenced by guidance_pid_get_h_pos_err(), guidance_pid_h_run(), and send_hover_loop().

◆ guidance_pid_speed_err

struct Int32Vect2 guidance_pid_speed_err

Definition at line 1 of file guidance_pid.c.

Referenced by guidance_pid_h_run(), and send_hover_loop().

◆ guidance_pid_trim_att_integrator

struct Int32Vect2 guidance_pid_trim_att_integrator

Definition at line 1 of file guidance_pid.c.

Referenced by guidance_pid_h_run(), guidance_pid_set_h_igain(), and send_hover_loop().

◆ guidance_pid_v_fb_cmd

int32_t guidance_pid_v_fb_cmd

feed-back command

Definition at line 136 of file guidance_pid.c.

Referenced by guidance_pid_h_run(), and send_vert_loop().

◆ guidance_pid_v_ff_cmd

int32_t guidance_pid_v_ff_cmd

feed-forward command

Definition at line 135 of file guidance_pid.c.

Referenced by guidance_pid_h_run(), and send_vert_loop().

◆ guidance_pid_z_sum_err

int32_t guidance_pid_z_sum_err

accumulator for I-gain

Definition at line 134 of file guidance_pid.c.

Referenced by guidance_pid_h_run(), guidance_pid_set_v_igain(), guidance_pid_v_enter(), and send_vert_loop().

Macros

Functions

Variables

Detailed Description

Macro Definition Documentation

◆ FF_CMD_FRAC

◆ GH_GAIN_SCALE

◆ GUIDANCE_H_AGAIN

◆ GUIDANCE_H_APPROX_FORCE_BY_THRUST

◆ GUIDANCE_H_THRUST_CMD_FILTER

◆ GUIDANCE_H_VGAIN

◆ GUIDANCE_PID_USE_AS_DEFAULT

◆ GUIDANCE_V_ADAPT_THROTTLE_ENABLED

◆ GUIDANCE_V_MAX_CMD

◆ GUIDANCE_V_MAX_ERR_Z

◆ GUIDANCE_V_MAX_ERR_ZD

◆ GUIDANCE_V_MAX_SUM_ERR

◆ GUIDANCE_V_MIN_ERR_Z

◆ GUIDANCE_V_MIN_ERR_ZD

◆ MAX_POS_ERR

◆ MAX_SPEED_ERR

Function Documentation

◆ guidance_pid_get_h_pos_err()

◆ guidance_pid_h_enter()

◆ guidance_pid_h_run()

◆ guidance_pid_h_run_accel()

◆ guidance_pid_h_run_pos()

◆ guidance_pid_h_run_speed()

◆ guidance_pid_init()

◆ guidance_pid_set_h_igain()

◆ guidance_pid_set_v_igain()

◆ guidance_pid_v_enter()

◆ guidance_pid_v_run()

◆ guidance_pid_v_run_accel()

◆ guidance_pid_v_run_pos()

◆ guidance_pid_v_run_speed()

◆ send_hover_loop()

◆ send_vert_loop()

Variable Documentation

◆ guidance_pid

◆ guidance_pid_pos_err

◆ guidance_pid_speed_err

◆ guidance_pid_trim_att_integrator

◆ guidance_pid_v_fb_cmd

◆ guidance_pid_v_ff_cmd

◆ guidance_pid_z_sum_err