Struct Quadrotor

pub struct Quadrotor {
    pub position: Vector3<f32>,
    pub velocity: Vector3<f32>,
    pub acceleration: Vector3<f32>,
    pub orientation: UnitQuaternion<f32>,
    pub angular_velocity: Vector3<f32>,
    pub mass: f32,
    pub gravity: f32,
    pub time_step: f32,
    pub drag_coefficient: f32,
    pub inertia_matrix: Matrix3<f32>,
    pub inertia_matrix_inv: Matrix3<f32>,
}

Represents a quadrotor with its physical properties and state

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;
let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);

Fields

position: Vector3<f32>

Current position of the quadrotor in 3D space

velocity: Vector3<f32>

Current velocity of the quadrotor

acceleration: Vector3<f32>

Current acceleration of the quadrotor

orientation: UnitQuaternion<f32>

Current orientation of the quadrotor

angular_velocity: Vector3<f32>

Current angular velocity of the quadrotor

mass: f32

Mass of the quadrotor in kg

gravity: f32

Gravitational acceleration in m/s^2

time_step: f32

Simulation time step in seconds

drag_coefficient: f32

Drag coefficient

inertia_matrix: Matrix3<f32>

Inertia matrix of the quadrotor

inertia_matrix_inv: Matrix3<f32>

Inverse of the inertia matrix

Implementations

impl Quadrotor

Implementation of the Quadrotor struct

pub fn new( time_step: f32, mass: f32, gravity: f32, drag_coefficient: f32, inertia_matrix: [f32; 9], ) -> Result<Self, SimulationError>

Creates a new Quadrotor with default parameters

Arguments

• time_step - The simulation time step in seconds
• mass - The mass of the quadrotor in kg
• gravity - The gravitational acceleration in m/s^2
• drag_coefficient - The drag coefficient
• inertia_matrix - The inertia matrix of the quadrotor

Returns

• A new Quadrotor instance

Errors

• Returns a SimulationError if the inertia matrix cannot be inverted

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;

let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix);

pub fn update_dynamics_with_controls_euler( &mut self, control_thrust: f32, control_torque: &Vector3<f32>, )

Updates the quadrotor’s dynamics with control inputs

Arguments

• control_thrust - The total thrust force applied to the quadrotor
• control_torque - The 3D torque vector applied to the quadrotor

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;

let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
let control_thrust = mass * gravity;
let control_torque = Vector3::new(0.0, 0.0, 0.0);
quadrotor.update_dynamics_with_controls_euler(control_thrust, &control_torque);

pub fn update_dynamics_with_controls_rk4( &mut self, control_thrust: f32, control_torque: &Vector3<f32>, )

Updates the quadrotor’s dynamics with control inputs using the Runge-Kutta 4th order method

Arguments

• control_thrust - The total thrust force applied to the quadrotor
• control_torque - The 3D torque vector applied to the quadrotor

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;
let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
let control_thrust = mass * gravity;
let control_torque = Vector3::new(0.0, 0.0, 0.0);
quadrotor.update_dynamics_with_controls_rk4(control_thrust, &control_torque);

pub fn get_state(&self) -> [f32; 13]

Returns the state derivative of the quadrotor

Arguments

• state - The state of the quadrotor

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;
use nalgebra::UnitQuaternion;
let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
let state = quadrotor.get_state();

pub fn set_state(&mut self, state: &[f32; 13])

Sets the state of the quadrotor

Arguments

• state - The state of the quadrotor

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;
use nalgebra::UnitQuaternion;
let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
let state = [
   0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
];
quadrotor.set_state(&state);

pub fn state_derivative( &mut self, state: &[f32], control_thrust: f32, control_torque: &Vector3<f32>, ) -> [f32; 13]

Calculates the derivative of the state of the quadrotor

Arguments

• state - The current state of the quadrotor
• control_thrust - The thrust applied to the quadrotor
• control_torque - The torque applied to the quadrotor

Returns

The derivative of the state

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;
use nalgebra::UnitQuaternion;
let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let mut quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
let state = [
  0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
];
let control_thrust = 0.0;
let control_torque = Vector3::new(0.0, 0.0, 0.0);
let derivative = quadrotor.state_derivative(&state, control_thrust, &control_torque);

pub fn read_imu(&self) -> Result<(Vector3<f32>, Vector3<f32>), SimulationError>

Simulates IMU readings

Returns

• A tuple containing the true acceleration and angular velocity of the quadrotor

Errors

• Returns a SimulationError if the IMU readings cannot be calculated

Example

use nalgebra::Vector3;
use peng_quad::Quadrotor;

let (time_step, mass, gravity, drag_coefficient) = (0.01, 1.3, 9.81, 0.01);
let inertia_matrix = [0.0347563, 0.0, 0.0, 0.0, 0.0458929, 0.0, 0.0, 0.0, 0.0977];
let quadrotor = Quadrotor::new(time_step, mass, gravity, drag_coefficient, inertia_matrix).unwrap();
let (true_acceleration, true_angular_velocity) = quadrotor.read_imu().unwrap();