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)