def _cmd_to_thrust_omega(self, cmd, rot):
"""Computes the thrust as a 3D numpy array from the RC command."""
# unpacking the control input
roll_cmd, pitch_cmd, throttle_cmd, yaw_rate_cmd = cmd
# getting the current yaw from rot
current_yaw = uf.rot_to_ea(rot)[2]
# maximum yaw rate
max_yaw_rate_deg = isp.max_yaw_rate_deg
max_yaw_rate = max_yaw_rate_deg * np.pi / 180.0
# angular speed
omega = np.zeros(3)
omega[2] = -(float(yaw_rate_cmd) - 1500.0) * max_yaw_rate / 500.0
# maximum tilt
max_tilt_deg = isp.max_tilt_deg
max_tilt = max_tilt_deg * np.pi / 180.0
# desired roll
roll_des = (float(roll_cmd) - 1500.0) * max_tilt / 500.0
# desired pitch
pitch_des = -(float(pitch_cmd) - 1500.0) * max_tilt / 500.0
# desired acceleration
ea = roll_des, pitch_des, current_yaw
rot = uf.ea_to_rot(ea)
throttle_gain = isp.quad_mass * isp.gravity / isp.neutral_throttle
thrust = throttle_gain * throttle_cmd * rot.dot(uf.e3)
return thrust, omega
def dynamics(self, pos, rot, vel, cmd):
# unpacking the control input
roll_cmd, pitch_cmd, thrust_cmd, yaw_rate_cmd = cmd
ea = uf.rot_to_ea(rot)
# current yaw
yaw = ea[2]
# maximum yaw rate
max_yaw_rate_deg = sp.max_yaw_rate_deg
max_yaw_rate = max_yaw_rate_deg * numpy.pi / 180
# maximum tilt
max_tilt_deg = sp.max_tilt_deg
max_tilt = max_tilt_deg * numpy.pi / 180
# desired roll
roll_des = (roll_cmd - 1500) * max_tilt / 500
# desired pitch
pitch_des = -(pitch_cmd - 1500) * max_tilt / 500
# gain of the inner loop for attitude control
ktt = sp.inner_loop_gain
# desired thrust versor
e3 = numpy.array([0.0, 0.0, 1.0])
dtv = uf.ea_to_rot((roll_des, pitch_des, yaw)).dot(e3)
# actual thrust versor
atv = rot.dot(e3)
# angular velocity
aux = ktt * uf.skew(atv).dot(dtv)
rot_t = numpy.transpose(rot)
omega = rot_t.dot(aux)
# yaw rate
omega[2] = -(yaw_rate_cmd - 1500) * max_yaw_rate / 500
# neutral thrust
nt = sp.neutral_thrust
# thrust gain
kt = sp.quad_mass * sp.g / nt
# thrust command adjustment
# The thrust sent to the motors is automatically adjusted according to
# the tilt angle of the vehicle (bigger tilt leads to bigger thrust)
# to reduce the thrust compensation that the pilot must give.
thrust_cmd = thrust_cmd / numpy.dot(atv, e3)
# dynamics
dpos = vel
dvel = kt * thrust_cmd * atv / sp.quad_mass - sp.g * e3
drot = rot.dot(uf.skew(omega))
return dpos, drot, dvel