|
Paparazzi UAS v7.1_unstable
Paparazzi is a free software Unmanned Aircraft System.
|
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"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 | |
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 | |
Guidance controller with PID for rotorcrafts.
Definition in file guidance_pid.c.
| #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().
| #define GH_GAIN_SCALE 2 |
Definition at line 213 of file guidance_pid.c.
Referenced by computeOptiTrack(), and guidance_pid_h_run().
| #define GUIDANCE_H_AGAIN 0 |
Definition at line 36 of file guidance_pid.c.
Referenced by guidance_pid_init().
| #define GUIDANCE_H_APPROX_FORCE_BY_THRUST FALSE |
Definition at line 57 of file guidance_pid.c.
Referenced by guidance_pid_init().
| #define GUIDANCE_H_THRUST_CMD_FILTER 10 |
Definition at line 53 of file guidance_pid.c.
Referenced by guidance_pid_h_run().
| #define GUIDANCE_H_VGAIN 0 |
Definition at line 40 of file guidance_pid.c.
Referenced by guidance_pid_init().
| #define GUIDANCE_PID_USE_AS_DEFAULT TRUE |
The PID controller is used by default.
Definition at line 117 of file guidance_pid.c.
| #define GUIDANCE_V_ADAPT_THROTTLE_ENABLED TRUE |
Definition at line 83 of file guidance_pid.c.
Referenced by guidance_pid_init().
| #define GUIDANCE_V_MAX_CMD 0.9*MAX_PPRZ |
Definition at line 109 of file guidance_pid.c.
Referenced by guidance_pid_v_run().
| #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().
| #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().
| #define GUIDANCE_V_MAX_SUM_ERR 2000000 |
Definition at line 105 of file guidance_pid.c.
Referenced by guidance_pid_v_run().
| #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().
| #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().
| #define MAX_POS_ERR POS_BFP_OF_REAL(16.) |
Definition at line 61 of file guidance_pid.c.
Referenced by computeOptiTrack(), guidance_pid_h_run_pos(), rover_guidance_run(), and rover_holo_guidance_run().
| #define MAX_SPEED_ERR SPEED_BFP_OF_REAL(16.) |
Definition at line 62 of file guidance_pid.c.
Referenced by computeOptiTrack(), guidance_pid_h_run_pos(), guidance_pid_h_run_speed(), rover_guidance_run(), and rover_holo_guidance_run().
| const struct Int32Vect2 * guidance_pid_get_h_pos_err | ( | void | ) |
Gets the position error.
| none. |
Definition at line 417 of file guidance_pid.c.
References guidance_pid_pos_err.
| void guidance_pid_h_enter | ( | void | ) |
Definition at line 389 of file guidance_pid.c.
References nav, FloatEulers::psi, and stateGetNedToBodyEulers_f().
|
static |
run horizontal control loop for position and speed control
Definition at line 218 of file guidance_pid.c.
References ANGLE_BFP_OF_REAL, BFP_OF_REAL, GH_GAIN_SCALE, guidance_h, GUIDANCE_H_MAX_BANK, GUIDANCE_H_THRUST_CMD_FILTER, guidance_pid, guidance_pid_h_run(), guidance_pid_pos_err, guidance_pid_speed_err, guidance_pid_trim_att_integrator, guidance_v, INT32_ACCEL_FRAC, INT32_ANGLE_FRAC, INT32_ANGLE_PI_2, INT32_POS_FRAC, INT32_SPEED_FRAC, INT32_TRIG_FRAC, INT_VECT2_ZERO, MAX_PPRZ, Min, stab_sp_from_ltp_i(), stabilization, and VECT2_STRIM.
Referenced by guidance_pid_h_run(), guidance_pid_h_run_pos(), and guidance_pid_h_run_speed().
| 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().
| 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().
| 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().
| void guidance_pid_init | ( | void | ) |
Definition at line 184 of file guidance_pid.c.
References 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, register_periodic_telemetry(), send_hover_loop(), and send_vert_loop().
| 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.
| 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.
| void guidance_pid_v_enter | ( | void | ) |
Definition at line 395 of file guidance_pid.c.
References guidance_pid_z_sum_err.
|
static |
Definition at line 321 of file guidance_pid.c.
References BFP_OF_REAL, FF_CMD_FRAC, guidance_pid, guidance_pid_v_fb_cmd, guidance_pid_v_ff_cmd, guidance_pid_v_run(), guidance_pid_z_sum_err, 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, gv_adapt_X, GV_ADAPT_X_FRAC, INT32_ACCEL_FRAC, INT32_TRIG_FRAC, MAX_PPRZ, stateGetPositionNed_i(), stateGetSpeedNed_i(), th_sp_from_thrust_i(), THRUST_AXIS_Z, and NedCoor_i::z.
Referenced by guidance_pid_v_run(), guidance_pid_v_run_pos(), and guidance_pid_v_run_speed().
| 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().
| 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().
| 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().
|
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().
|
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().
| struct GuidancePID guidance_pid |
Guidance PID structyre.
Definition at line 124 of file guidance_pid.c.
Referenced by guidance_hybrid_vertical(), guidance_pid_h_run(), guidance_pid_init(), guidance_pid_set_h_igain(), guidance_pid_set_v_igain(), guidance_pid_v_run(), send_hover_loop(), and send_vert_loop().
| struct Int32Vect2 guidance_pid_pos_err |
Definition at line 130 of file guidance_pid.c.
Referenced by guidance_pid_get_h_pos_err(), guidance_pid_h_run(), guidance_pid_h_run_pos(), guidance_pid_h_run_speed(), guidance_pid_init(), and send_hover_loop().
| struct Int32Vect2 guidance_pid_speed_err |
Definition at line 131 of file guidance_pid.c.
Referenced by guidance_pid_h_run(), guidance_pid_h_run_pos(), guidance_pid_h_run_speed(), guidance_pid_init(), and send_hover_loop().
| struct Int32Vect2 guidance_pid_trim_att_integrator |
Definition at line 132 of file guidance_pid.c.
Referenced by guidance_pid_h_run(), guidance_pid_init(), guidance_pid_set_h_igain(), and send_hover_loop().
| 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().
| 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().
| 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().