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

Guidance controller with PID for rotorcrafts. More...

+ 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 Int32Vect2guidance_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()

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

Referenced by guidance_hybrid_v_run_accel().

+ Here is the caller graph for this function:

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

Referenced by guidance_hybrid_v_run_pos().

+ Here is the caller graph for this function:

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

Referenced by guidance_hybrid_v_run_speed().

+ Here is the caller graph for this function:

◆ send_hover_loop()

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

◆ send_vert_loop()

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

Variable Documentation

◆ guidance_pid

struct GuidancePID guidance_pid

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