Commit fa3d2d30 authored by Ibrahim's avatar Ibrahim
Browse files

torque calculations and convenience functions for testing

parent 0944ee8c
......@@ -6,8 +6,23 @@ from numba import njit
@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
......@@ -19,11 +34,11 @@ def inertial_to_body(vector: np.ndarray, dcm=np.ndarray) -> np.ndarray:
@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],
......@@ -35,7 +50,9 @@ def direction_cosine_matrix(roll: float, pitch: float, yaw: float) -> np.ndarray
@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 )
......@@ -49,7 +66,9 @@ def rotating_frame_derivative(value: np.ndarray, local_derivative: np.ndarray, o
@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))
......@@ -60,7 +79,9 @@ def angular_to_euler_rate(angular_velocity: np.ndarray, orientation: np.ndarray)
@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
......@@ -67,3 +67,19 @@ def find_nominal_speed(thrust_fn: Callable[[float], float], weight: float) -> fl
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
......@@ -3,50 +3,62 @@ from dataclasses import dataclass
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.
......@@ -55,20 +67,45 @@ class VehicleParams:
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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment