torque calculations and convenience functions for testing

@njit

def body_to_inertial(vector: np.ndarray, dcm: np.ndarray) -> np.ndarray:

    dcm_inverse = dcm.T

    return dcm_inverse @ vector

    """

    View body coordinates in terms of coordinates on inertial axes. Assumes

    both frames share same origin.

    Parameters

    ----------

    vector : np.ndarray

        The 3D vector of points, or a 3 x N matrix of N points

    dcm : np.ndarray

        The 3x3 direction cosine matrix

    Returns

    -------

    np.ndarray

        The transformed coordinates in the intertial frame

    """

    return dcm.T @ vector

@njit

def direction_cosine_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray:

    cr = cos(roll)

    cr = cos(roll)  # phi

    sr = sin(roll)

    cp = cos(pitch)

    cp = cos(pitch) # theta

    sp = sin(pitch)

    cy = cos(yaw)

    cy = cos(yaw)   # psi

    sy = sin(yaw)

    dcm = np.asarray([

        [cp * cy,                   cp * sy,                    -sp],

@njit

def rotating_frame_derivative(value: np.ndarray, local_derivative: np.ndarray, omega: np.ndarray) -> np.ndarray:

def rotating_frame_derivative(

    value: np.ndarray, local_derivative: np.ndarray, omega: np.ndarray

) -> np.ndarray:

    # d (value . vector) / dt

    # = vector . d value / dt + value . d vector / dt

    #  ( local derivative )  ( coriolis term )

@njit

def angular_to_euler_rate(angular_velocity: np.ndarray, orientation: np.ndarray) -> np.ndarray:

def angular_to_euler_rate(

    angular_velocity: np.ndarray, orientation: np.ndarray

) -> np.ndarray:

    roll, pitch, yaw = orientation

    p, q, r = angular_velocity

    roll_rate = p + q * tan(pitch) * (q * sin(roll) + r * cos(roll))

@njit

def euler_to_angular_rate(euler_velocity: np.ndarray, orientation: np.ndarray) -> np.ndarray:

def euler_to_angular_rate(

    euler_velocity: np.ndarray, orientation: np.ndarray

) -> np.ndarray:

    roll_rate, pitch_rate, yaw_rate = euler_velocity

    roll, pitch, yaw = orientation

    cr, cp = cos(roll), cos(pitch)

from typing import Callable, Iterable

from typing import Callable, Iterable, Tuple

import numpy as np

from scipy.optimize import fsolve

        residual = thrust - weight

        return residual

    return fsolve(balance, 1e3)[0]

def learn_thrust_coefficient(

    thrust_fn: Callable[[float], float], domain: Tuple=(1, 10000)

) -> float:

    speeds = np.linspace(domain[0], domain[1], num=250)

    thrust = np.zeros_like(speeds)

    for i, speed in enumerate(speeds):

        thrust[i] = thrust_fn(speed)

    return np.polyfit(speeds, thrust, deg=2)[0] # return coefficient of quadradic term

def moment_of_inertia_disk(m: float, r: float) -> float:

    return 0.5 * m * r**2

 No newline at end of file

import numpy as np

@dataclass

class MotorParams:

    # See: http://learningrc.com/motor-kv/, http://web.mit.edu/first/scooter/motormath.pdf

    k_m: float

    "Motor constant, where omega = k_m * back_emf, and k_m = 1/k_e"

    k_e: float = None

    "Back-EMF constant, relates motor speed induced voltage, back_emf = k_e * omega"

    k_q: float = None

    "Torque constant k_q, where torque Q = k_q * current. Equal to k_e"

@dataclass

class PropellerParams:

    moment_of_inertia: float

    # Manufacturer propeller length x pitch specification:

    diameter: float = 6  #inches

    pitch: float = 3   #inches

    a: float = 5.7

    "Lift curve slope used in example in Stevens & Lewis" # TODO: read more

    "Lift curve slope used in example in Stevens & Lewis (eq 2.2-6a)"

    # d C_L / d alpha = 2 pi / (sqrt(1-M^2)), where M is mach number, alpha is 

    # aerodynamic angle of x-axis of body and x-axis of stability

    b: float = 2

    "Number of blades"

    c: float = 0.0274

    "Mean chord length (m)"

    "Mean chord length (m) of the propeller blade"

    eta: float = 1.

    "Propeller efficiency"

    k_lift: float = None

    "Propeller's aerodynamic lift coefficient"

    k_drag: float = None

    "Propeller's aerodynamic drag coefficient"

    k_thrust: float = None

    "Propeller's aerodynamic thrust coefficient, where thrust =  k_thrust * angular velocity^2"

    k_torque: float = None

    "Torque constant, where torque = k_torque * angular velocity^2"

    motor: MotorParams = None

    "The parameters of the motor to simulate speed, otherwise instantaneous."

    def __post_init__(self):

        # ensure torque function gets floats so adding float + int in the njit

        # torque function is not ignored

        self.moment_of_inertia = float(self.moment_of_inertia)

        self.R = self.diameter * 0.0254 # inches to metres

        "Radius in metres"

        self.A = np.pi * self.R**2

        "Area of propeller disc in metres squared"

        self.theta0 = 2*np.arctan2(self.pitch, (2 * np.pi * 3/4 * self.diameter/2))

        "Pitch angle at root of blade"

        self.theta1 = -4 / 3 * np.arctan2(self.pitch, 2 * np.pi * 3/4 * self.diameter/2)

@dataclass

class MotorParams:

    kt: float

    "Torque constant"

    km: float

    "Motor constant"

    ke: float = None

    "Back-EMF constant=Torque constant"

    def __post_init__(self):

        self.ke = self.kt

        "Change in pitch angle towards tip of blade"

        # Pitch angel is reduced to allow for even lift as blade velocity increases

        # with increasing radius. Assuming linear reduction from root to tip.

    propellers: List[PropellerParams]

    angles: np.ndarray

    "Angle (radians) of propeller arm from the positive x-axis (forward) of the body frame."

    distances: np.ndarray

    "Distance (m) of each propeller from the centre of mass."

    clockwise: np.ndarray = None

    """1 if motor spins clockwise, -1 if anti-clockwise, looking from the top.

    Defaults to alternating clockwise/anti-clockwise."""

    mass: float = 1.

    inertia_matrix: np.matrix = np.eye(3)

    def __post_init__(self):

        self.distances = self.distances.astype(float)

        self.inertia_matrix_inverse = np.linalg.inv(self.inertia_matrix)

        if self.clockwise is None:

            self.clockwise = np.ones(len(self.propellers), dtype=int)

            self.clockwise[::2] = 1

            self.clockwise[1::2] = -1

        # Set up control allocation matrix if thrust and motor torque constants

        # are provided

        if (

            all([p.k_thrust is not None for p in self.propellers]) and \

            all([p.k_torque is not None for p in self.propellers])

        ):

            alloc = np.zeros((4, len(self.propellers))) #[Fz, Mx, My, Mz] x n-Propellers

            x = self.distances * np.sin(self.angles)

            y = self.distances * np.cos(self.angles)

            for i, p in enumerate(self.propellers):

                alloc[0, i] = -p.k_thrust                       # vertical force (negative up)

                alloc[1, i] = p.k_thrust * x[i]                 # torque about x-axis

                alloc[2, i] = p.k_thrust * y[i]                 # torque about y-axis

                alloc[3, i] = p.k_torque * self.clockwise[i]    # torque about z-axis

            self.alloc = alloc

            self.alloc_inverse = np.linalg.pinv(alloc)

        else:

            self.alloc = None

            self.alloc_inverse = None

@dataclass