def __init__(self, constants, initials):
self.r = RMatrix()
self.t = TMatrix()
self.c = self._defaultConstants.copy()
self.c.update(constants)
self.state = self._defaultInits.copy()
self.state.update(initials)
class QuadCopter:
_defaultConstants = {
# Acceleration of gravity in m/(s^2)
"g": 9.81,
# Mass in kg
"mass": 1,
# Length of each arm in m
"armLength": 0.25,
# Angular mass in kg*(m^2)
"intertia": 1,
# Thrust multiplier. We simplify the thrust calculation
# for each motor by making it a multiple of the propeller's
# angular velocity.
"thrust": 1,
# Torque multiplier. We simplify the torque calculation
# for each motor by making it a multiple of the propeller's
# angular velocity.
"torque": 1,
# Drag multiplier. We simplify the drag calculation by
# making it a multiple of the linear velocity of the
# copter.
"drag": 1,
}
_defaultInits = {
# X, y, and z.
"position": ut.makeVector((0, 0, 10)),
# Roll, pitch, and yaw angles. TODO - what frame?
"attitude": ut.makeVector((0, 0, 0)),
# The rate of change for each of the state vectors.
"positionRates": ut.makeVector((0, 0, 0)),
"attitudeRates": ut.makeVector((0, 0, 0)),
# The inputs to each motor, representing the target
# angular velocity.
"inputs": [0, 0, 0, 0],
}
def __init__(self, constants, initials):
self.r = RMatrix()
self.t = TMatrix()
self.c = self._defaultConstants.copy()
self.c.update(constants)
self.state = self._defaultInits.copy()
self.state.update(initials)
def setInputs(motor, value, values=None):
"""Sets the input value for one or all the motors."""
if values is not None:
self.state["inputs"] = values
else:
self.state["inputs"][motor] = value
def updateState(elapsedTime):
angularVelocity = self._calcAngularVelocity()
acceleration = self._calcLinearAcceleration()
angularAcceleration = self._calcAngularAcceleration()
angularVelocity = angularVelocity + (elapsedTime * angularAcceleration)
attitudeRates = self._calcAttitudeRates(angularVelocity)
attitude = ut.normalizeAngleVector(attitude + (elapsedTime * attitudeRates))
positionRates = positionRates + (elapsedTime * acceleration)
position = position + (elapsedTime * positionRates)
self.state.update(
{"position": position, "attitude": attitude, "positionRates": positionRates, "attitudeRates": attitudeRates}
)
def _calcAngularVelocity(self):
"""Angular velocity in the world frame."""
return self.t.get(self.attitude) * self.attitudeRates
def _calcAttitudeRates(self, angularVelocity):
"""Attitude rates in the body frame."""
return self.t.get(self.attitude).getI() * angularVelocity
def _calcThrust(self):
"""Thrust in the body frame."""
xComponent = 0
yComponent = 0
zComponent = self.c["thrust"] * sum(self.state["inputs"])
return ut.makeVector((xComponent, yComponent, zComponent))
def _calcTorque(self):
"""Torque in the body frame."""
inputs = self.state["inputs"]
l = self.c["armLength"]
th = self.c["thrust"]
tq = self.c["torque"]
rollComponent = l * th * (inputs[0] - inputs[2])
pitchComponent = l * th * (inputs[1] - inputs[3])
yawComponent = tq * (inputs[0] - inputs[1] + inputs[2] - inputs[3])
return ut.makeVector((rollComponent, pitchComponent, yawComponent))
def _calcLinearAcceleration(self):
"""Linear acceleration in the world frame."""
r = self.r.get(self.attitude)
gravity = ut.makeVector((0, 0, -self.c["g"]))
thrust = r * self._calcThrust()
drag = -self.c["drag"] * self.state["positionRates"]
return gravity + (thrust / self.c["mass"]) + drag
def _calcAngularAcceleration(self):
"""Angular acceleration in the world frame."""
torque = self._calcTorque()
angularVelocity = self._calcAngularVelocity()
crossed = np.cross(angularVelocity, self.c["intertia"] * angularVelocity, axis=0)
return inertia.getI() * (torque - crossed)
