fn calc_motor_speed(
        &self,
        acc_set: (f32, f32, f32),
        thrust: f32,
        par: &Parameter,
    ) -> MotorSpeed {
        // Deconstruct tuple
        let (r_acc_set, p_acc_set, y_acc_set) = acc_set;

        // Force from the three controllers is calculated
        let f_roll = r_acc_set * par.inertia_x / par.lever_y * par.thrust_fact; // [N]
        let f_pitch = p_acc_set * par.inertia_y / par.lever_x * par.thrust_fact; // [N]
        let f_yaw = y_acc_set * par.inertia_z * par.torque_fact * par.torque_fact; // [Nm]
        let f_thrust = thrust * par.mass * par.thrust_fact; // [N]
        let force = Self::motor_mixer((f_roll, f_pitch, f_yaw, f_thrust));
        let mut force = [force.0, force.1, force.2, force.3];

        // The force rises with the power of 2 of the speed.
        // Prevent sqrt with values smaler than zero
        // Limit the speed output to max and min speed
        for f in force.iter_mut() {
            *f = f.max(0.0).sqrt().min(par.speed_max).max(par.speed_min);
        }
        (force[0], force[1], force[2], force[3])
    }
/// Calculate motorspeed from angle velocity setpoint and thrust setpoint
Calculate motorspeed from angle velocity setpoint and thrust setpoint
Calculate motorspeed from angle velocity setpoint and thrust setpoint
fn calc_motor_speed(
        &self,
        acc_set: (f32, f32, f32),
        thrust: f32,
        par: &Parameter,
    ) -> MotorSpeed {
        // Deconstruct tuple
        let (r_acc_set, p_acc_set, y_acc_set) = acc_set;
        // Force from the three controllers is calculated
        let f_roll = r_acc_set * par.inertia_x / par.lever_y * par.thrust_fact; // [N]
        let f_pitch = p_acc_set * par.inertia_y / par.lever_x * par.thrust_fact; // [N]
        let f_yaw = y_acc_set * par.inertia_z * par.torque_fact * par.torque_fact; // [Nm]
        let f_thrust = thrust * par.mass * par.thrust_fact; // [N]
        let force = Self::motor_mixer((f_roll, f_pitch, f_yaw, f_thrust));
        let mut force = [force.0, force.1, force.2, force.3];
        // The force rises with the power of 2 of the speed.
        // Prevent sqrt with values smaler than zero
        // Limit the speed output to max and min speed
        for f in force.iter_mut() {
            *f = f.max(0.0).sqrt().min(par.speed_max).max(par.speed_min);
        }
        (force[0], force[1], force[2], force[3])
    }
fn calc_motor_speed ( & self , acc_set : ( f32 , f32 , f32 ) , thrust : f32 , par : & Parameter , ) -> MotorSpeed { // Deconstruct tuple let ( r_acc_set , p_acc_set , y_acc_set ) = acc_set ; // Force from the three controllers is calculated let f_roll = r_acc_set * par . inertia_x / par . lever_y * par . thrust_fact ; // [N] let f_pitch = p_acc_set * par . inertia_y / par . lever_x * par . thrust_fact ; // [N] let f_yaw = y_acc_set * par . inertia_z * par . torque_fact * par . torque_fact ; // [Nm] let f_thrust = thrust * par . mass * par . thrust_fact ; // [N] let force = Self :: motor_mixer ( ( f_roll , f_pitch , f_yaw , f_thrust ) ) ; let mut force = [ force . 0 , force . 1 , force . 2 , force . 3 ] ; // The force rises with the power of 2 of the speed. // Prevent sqrt with values smaler than zero // Limit the speed output to max and min speed for f in force . iter_mut ( ) { * f = f . max ( 0.0 ) . sqrt ( ) . min ( par . speed_max ) . max ( par . speed_min ) ; } ( force [ 0 ] , force [ 1 ] , force [ 2 ] , force [ 3 ] ) }
Calculate motorspeed from angle velocity setpoint and thrust setpoint
Calculate motorspeed from angle velocity setpoint and thrust setpoint
&self
acc_set
(f32, f32, f32)
thrust
f32
par
&Parameter
&self
acc_set
(f32, f32, f32)
thrust
f32
par
&Parameter