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