def thrustToAttitude(self, quad, Ts):
# Create Full Desired Quaternion Based on Thrust Setpoint and Desired Yaw Angle
# ---------------------------
yaw_sp = self.eul_sp[2]
# Desired body_z axis direction
body_z = -utils.vectNormalize(self.thrust_sp)
if (config.orient == "ENU"):
body_z = -body_z
# Vector of desired Yaw direction in XY plane, rotated by pi/2 (fake body_y axis)
y_C = np.array([-sin(yaw_sp), cos(yaw_sp), 0.0])
# Desired body_x axis direction
body_x = np.cross(y_C, body_z)
body_x = utils.vectNormalize(body_x)
# Desired body_y axis direction
body_y = np.cross(body_z, body_x)
# Desired rotation matrix
R_sp = np.array([body_x, body_y, body_z]).T
# Full desired quaternion (full because it considers the desired Yaw angle)
self.qd_full = utils.RotToQuat(R_sp)
def attitude_control(self, quad, Ts, config):
# Current thrust orientation e_z and desired thrust orientation e_z_d
e_z = quad.dcm[:, 2]
e_z_d = -utils.vectNormalize(self.thrust_sp)
if (config.orient == "ENU"):
e_z_d = -e_z_d
# Quaternion error between the 2 vectors
qe_red = np.zeros(4)
qe_red[0] = np.dot(e_z, e_z_d) + sqrt(norm(e_z)**2 * norm(e_z_d)**2)
qe_red[1:4] = np.cross(e_z, e_z_d)
qe_red = utils.vectNormalize(qe_red)
# Reduced desired quaternion (reduced because it doesn't consider the desired Yaw angle)
self.qd_red = utils.quatMultiply(qe_red, quad.quat)
# Mixed desired quaternion (between reduced and full) and resulting desired quaternion qd
q_mix = utils.quatMultiply(utils.inverse(self.qd_red), self.qd_full)
q_mix = q_mix * np.sign(q_mix[0])
q_mix[0] = np.clip(q_mix[0], -1.0, 1.0)
q_mix[3] = np.clip(q_mix[3], -1.0, 1.0)
self.qd = utils.quatMultiply(
self.qd_red,
np.array([
cos(self.yaw_w * np.arccos(q_mix[0])), 0, 0,
sin(self.yaw_w * np.arcsin(q_mix[3]))
]))
# Resulting error quaternion
self.qe = utils.quatMultiply(utils.inverse(quad.quat), self.qd)
# Create rate setpoint from quaternion error
#@ self.rate_sp = (2.0*np.sign(self.qe[0])*self.qe[1:4])*att_P_gain
self.rate_sp = (2.0 * np.sign(self.qe[0]) *
self.qe[1:4]) * self.cTune_att_P_gain * att_P_gain
# Limit yawFF
self.yawFF = np.clip(self.yawFF, -rateMax[2], rateMax[2])
# Add Yaw rate feed-forward
self.rate_sp += utils.quat2Dcm(utils.inverse(
quad.quat))[:, 2] * self.yawFF
# Limit rate setpoint
self.rate_sp = np.clip(self.rate_sp, -rateMax, rateMax)
