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 _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 _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))
