Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
Loading...
Searching...
No Matches
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.
#define GH_GAIN_SCALE   2
#define FF_CMD_FRAC   18
 run vertical control loop for position and speed control

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
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
void guidance_pid_set_v_igain (uint32_t igain)
const struct Int32Vect2 * guidance_pid_get_h_pos_err (void)
 Gets the position error.

Variables

struct GuidancePID guidance_pid
 Guidance PID structyre.
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
int32_t guidance_pid_v_ff_cmd
 feed-forward command
int32_t guidance_pid_v_fb_cmd
 feed-back command

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.

Referenced by guidance_pid_v_run().

◆ GH_GAIN_SCALE

#define GH_GAIN_SCALE   2

Definition at line 213 of file guidance_pid.c.

Referenced by computeOptiTrack(), and guidance_pid_h_run().

◆ GUIDANCE_H_AGAIN

#define GUIDANCE_H_AGAIN   0

Definition at line 36 of file guidance_pid.c.

Referenced by guidance_pid_init().

◆ GUIDANCE_H_APPROX_FORCE_BY_THRUST

#define GUIDANCE_H_APPROX_FORCE_BY_THRUST   FALSE

Definition at line 57 of file guidance_pid.c.

Referenced by guidance_pid_init().

◆ GUIDANCE_H_THRUST_CMD_FILTER

#define GUIDANCE_H_THRUST_CMD_FILTER   10

Definition at line 53 of file guidance_pid.c.

Referenced by guidance_pid_h_run().

◆ GUIDANCE_H_VGAIN

#define GUIDANCE_H_VGAIN   0

Definition at line 40 of file guidance_pid.c.

Referenced by guidance_pid_init().

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

Referenced by guidance_pid_init().

◆ GUIDANCE_V_MAX_CMD

#define GUIDANCE_V_MAX_CMD   0.9*MAX_PPRZ

Definition at line 109 of file guidance_pid.c.

Referenced by guidance_pid_v_run().

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

Referenced by guidance_pid_v_run().

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

Referenced by guidance_pid_v_run().

◆ GUIDANCE_V_MAX_SUM_ERR

#define GUIDANCE_V_MAX_SUM_ERR   2000000

Definition at line 105 of file guidance_pid.c.

Referenced by guidance_pid_v_run().

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

Referenced by guidance_pid_v_run().

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

Referenced by guidance_pid_v_run().

◆ MAX_POS_ERR

#define MAX_POS_ERR   POS_BFP_OF_REAL(16.)

◆ MAX_SPEED_ERR

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 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 308 of file guidance_pid.c.

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

Referenced by guidance_pid_h_run_accel(), and guidance_pid_h_run_accel().

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

◆ guidance_pid_h_run_pos()

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

Definition at line 278 of file guidance_pid.c.

References guidance_pid_h_run(), guidance_pid_h_run_pos(), guidance_pid_pos_err, guidance_pid_speed_err, MAX_POS_ERR, MAX_SPEED_ERR, stateGetPositionNed_i(), stateGetSpeedNed_i(), VECT2_DIFF, and VECT2_STRIM.

Referenced by guidance_pid_h_run_pos().

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

◆ guidance_pid_h_run_speed()

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

Definition at line 294 of file guidance_pid.c.

References guidance_pid_h_run(), guidance_pid_h_run_speed(), guidance_pid_pos_err, guidance_pid_speed_err, INT_VECT2_ZERO, MAX_SPEED_ERR, stateGetSpeedNed_i(), VECT2_DIFF, and VECT2_STRIM.

Referenced by guidance_pid_h_run_speed().

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

◆ guidance_pid_init()

◆ 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, and INT_VECT2_ZERO.

◆ 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, and guidance_pid_z_sum_err.

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

◆ guidance_pid_v_run_accel()

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

Definition at line 381 of file guidance_pid.c.

References guidance_pid_v_run_accel(), ThrustSetpoint::sp, THRUST_SP_SET_ZERO, and UNUSED.

Referenced by guidance_hybrid_v_run_accel(), guidance_pid_v_run_accel(), and guidance_pid_v_run_accel().

Here is the call graph for this function:
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 371 of file guidance_pid.c.

References guidance_pid_v_run(), and guidance_pid_v_run_pos().

Referenced by guidance_hybrid_v_run_pos(), guidance_pid_h_run_accel(), and guidance_pid_v_run_pos().

Here is the call graph for this function:
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 376 of file guidance_pid.c.

References guidance_pid_v_run(), and guidance_pid_v_run_speed().

Referenced by guidance_hybrid_v_run_speed(), guidance_pid_h_run_accel(), and guidance_pid_v_run_speed().

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

◆ send_hover_loop()

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

Definition at line 142 of file guidance_pid.c.

References dev, guidance_h, guidance_pid, guidance_pid_pos_err, guidance_pid_speed_err, guidance_pid_trim_att_integrator, stateGetAccelNed_i(), stateGetPositionNed_i(), stateGetSpeedNed_i(), NedCoor_i::x, and NedCoor_i::y.

Referenced by guidance_pid_init().

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

◆ send_vert_loop()

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

Definition at line 164 of file guidance_pid.c.

References 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(), and NedCoor_i::z.

Referenced by guidance_pid_init().

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

Variable Documentation

◆ guidance_pid

◆ guidance_pid_pos_err

◆ guidance_pid_speed_err

struct Int32Vect2 guidance_pid_speed_err

◆ guidance_pid_trim_att_integrator

struct Int32Vect2 guidance_pid_trim_att_integrator

◆ 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_init(), guidance_pid_v_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_init(), guidance_pid_v_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_init(), guidance_pid_set_v_igain(), guidance_pid_v_enter(), guidance_pid_v_run(), and send_vert_loop().