fn angle_vel_control_loop(
        &mut self,
        vel_set: (f32, f32, f32),
        vel_act: (f32, f32, f32),
        dt: f32,
    ) -> (f32, f32, f32) {
        // Deconstruct arguments
        let (r_vel_set, p_vel_set, y_vel_set) = vel_set;
        let (r_vel_act, p_vel_act, y_vel_act) = vel_act;

        // Calculate AngleVelocity Controler
        // - Input Anglevelocity [rad/s]
        // - Output AngleAcceleration [rad/s²]
        let r_acc_set = self
            .roll_vel_ctrl
            .calc_next_output(Some(r_vel_set), r_vel_act, dt);
        let p_acc_set = self
            .pitch_vel_ctrl
            .calc_next_output(Some(p_vel_set), p_vel_act, dt);
        let y_acc_set = self
            .yaw_vel_ctrl
            .calc_next_output(Some(y_vel_set), y_vel_act, dt);

        // Return accelerations
        (r_acc_set, p_acc_set, y_acc_set)
    }
/// Angle Velocity Control Loop
/// Returns the angle acceleration for yaw pitch and roll rotation
Angle Velocity Control Loop
Returns the angle acceleration for yaw pitch and roll rotation
Angle Velocity Control Loop Returns the angle acceleration for yaw pitch and roll rotation
fn angle_vel_control_loop(
        &mut self,
        vel_set: (f32, f32, f32),
        vel_act: (f32, f32, f32),
        dt: f32,
    ) -> (f32, f32, f32) {
        // Deconstruct arguments
        let (r_vel_set, p_vel_set, y_vel_set) = vel_set;
        let (r_vel_act, p_vel_act, y_vel_act) = vel_act;
        // Calculate AngleVelocity Controler
        // - Input Anglevelocity [rad/s]
        // - Output AngleAcceleration [rad/s²]
        let r_acc_set = self
            .roll_vel_ctrl
            .calc_next_output(Some(r_vel_set), r_vel_act, dt);
        let p_acc_set = self
            .pitch_vel_ctrl
            .calc_next_output(Some(p_vel_set), p_vel_act, dt);
        let y_acc_set = self
            .yaw_vel_ctrl
            .calc_next_output(Some(y_vel_set), y_vel_act, dt);
        // Return accelerations
        (r_acc_set, p_acc_set, y_acc_set)
    }
fn angle_vel_control_loop ( & mut self , vel_set : ( f32 , f32 , f32 ) , vel_act : ( f32 , f32 , f32 ) , dt : f32 , ) -> ( f32 , f32 , f32 ) { // Deconstruct arguments let ( r_vel_set , p_vel_set , y_vel_set ) = vel_set ; let ( r_vel_act , p_vel_act , y_vel_act ) = vel_act ; // Calculate AngleVelocity Controler // - Input Anglevelocity [rad/s] // - Output AngleAcceleration [rad/s²] let r_acc_set = self . roll_vel_ctrl . calc_next_output ( Some ( r_vel_set ) , r_vel_act , dt ) ; let p_acc_set = self . pitch_vel_ctrl . calc_next_output ( Some ( p_vel_set ) , p_vel_act , dt ) ; let y_acc_set = self . yaw_vel_ctrl . calc_next_output ( Some ( y_vel_set ) , y_vel_act , dt ) ; // Return accelerations ( r_acc_set , p_acc_set , y_acc_set ) }
Angle Velocity Control Loop
Returns the angle acceleration for yaw pitch and roll rotation
Angle Velocity Control Loop Returns the angle acceleration for yaw pitch and roll rotation
&mut self
vel_set
(f32, f32, f32)
vel_act
(f32, f32, f32)
dt
f32
&mut self
vel_set
(f32, f32, f32)
vel_act
(f32, f32, f32)
dt
f32