Struct PIDController

pub struct PIDController {
    pub kpid_pos: [Vector3<f32>; 3],
    pub kpid_att: [Vector3<f32>; 3],
    pub integral_pos_error: Vector3<f32>,
    pub integral_att_error: Vector3<f32>,
    pub max_integral_pos: Vector3<f32>,
    pub max_integral_att: Vector3<f32>,
    pub mass: f32,
    pub gravity: f32,
}

PID controller for quadrotor position and attitude control

The kpid_pos and kpid_att gains are following the format of proportional, derivative and integral gains

Example

use nalgebra::Vector3;
use peng_quad::PIDController;
let kpid_pos = [
    [1.0, 1.0, 1.0],
    [0.1, 0.1, 0.1],
    [0.01, 0.01, 0.01],
];
let kpid_att = [
    [1.0, 1.0, 1.0],
    [0.1, 0.1, 0.1],
    [0.01, 0.01, 0.01],
];
let max_integral_pos = [1.0, 1.0, 1.0];
let max_integral_att = [1.0, 1.0, 1.0];
let mass = 1.0;
let gravity = 9.81;
let pid_controller = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);

Fields

kpid_pos: [Vector3<f32>; 3]

PID gain for position control including proportional, derivative, and integral gains

kpid_att: [Vector3<f32>; 3]

PID gain for attitude control including proportional, derivative, and integral gains

integral_pos_error: Vector3<f32>

Accumulated integral error for position

integral_att_error: Vector3<f32>

Accumulated integral error for attitude

max_integral_pos: Vector3<f32>

Maximum allowed integral error for position

max_integral_att: Vector3<f32>

Maximum allowed integral error for attitude

mass: f32

Mass of the quadrotor

gravity: f32

Gravity constant

Implementations

impl PIDController

Implementation of PIDController

pub fn new( _kpid_pos: [[f32; 3]; 3], _kpid_att: [[f32; 3]; 3], _max_integral_pos: [f32; 3], _max_integral_att: [f32; 3], _mass: f32, _gravity: f32, ) -> Self

Creates a new PIDController with default gains gains are in the order of proportional, derivative, and integral

Arguments

• _kpid_pos - PID gains for position control
• _kpid_att - PID gains for attitude control
• _max_integral_pos - Maximum allowed integral error for position
• _max_integral_att - Maximum allowed integral error for attitude

Returns

• A new PIDController instance

Example

use nalgebra::Vector3;
use peng_quad::PIDController;
let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let max_integral_pos = [1.0, 1.0, 1.0];
let max_integral_att = [1.0, 1.0, 1.0];
let mass = 1.0;
let gravity = 9.81;
let pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);

pub fn compute_attitude_control( &mut self, desired_orientation: &UnitQuaternion<f32>, current_orientation: &UnitQuaternion<f32>, current_angular_velocity: &Vector3<f32>, dt: f32, ) -> Vector3<f32>

Computes attitude control torques

Arguments

• desired_orientation - The desired orientation quaternion
• current_orientation - The current orientation quaternion
• current_angular_velocity - The current angular velocity
• dt - Time step

Returns

• The computed attitude control torques

Example

use nalgebra::{UnitQuaternion, Vector3};
use peng_quad::PIDController;

let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let max_integral_pos = [1.0, 1.0, 1.0];
let max_integral_att = [1.0, 1.0, 1.0];
let mass = 1.0;
let gravity = 9.81;
let mut pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
let desired_orientation = UnitQuaternion::identity();
let current_orientation = UnitQuaternion::identity();
let current_angular_velocity = Vector3::zeros();
let dt = 0.01;
let control_torques = pid.compute_attitude_control(&desired_orientation, &current_orientation, &current_angular_velocity, dt);

pub fn compute_position_control( &mut self, desired_position: &Vector3<f32>, desired_velocity: &Vector3<f32>, desired_yaw: f32, current_position: &Vector3<f32>, current_velocity: &Vector3<f32>, dt: f32, ) -> (f32, UnitQuaternion<f32>)

Computes position control thrust and desired orientation

Arguments

• desired_position - The desired position
• desired_velocity - The desired velocity
• desired_yaw - The desired yaw angle
• current_position - The current position
• current_velocity - The current velocity
• dt - Time step
• mass - Mass of the quadrotor
• gravity - Gravitational acceleration

Returns

• A tuple containing the computed thrust and desired orientation quaternion

Example

use nalgebra::{UnitQuaternion, Vector3};
use peng_quad::PIDController;

let kpid_pos = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let kpid_att = [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]];
let max_integral_pos = [1.0, 1.0, 1.0];
let max_integral_att = [1.0, 1.0, 1.0];
let mass = 1.0;
let gravity = 9.81;
let mut pid = PIDController::new(kpid_pos, kpid_att, max_integral_pos, max_integral_att, mass, gravity);
let desired_position = Vector3::new(0.0, 0.0, 1.0);
let desired_velocity = Vector3::zeros();
let desired_yaw = 0.0;
let current_position = Vector3::zeros();
let current_velocity = Vector3::zeros();
let dt = 0.01;
let (thrust, desired_orientation) = pid.compute_position_control(&desired_position, &desired_velocity, desired_yaw, &current_position, &current_velocity, dt);