void stabilization_attitude_read_rc_setpoint_quat_f(struct FloatQuat* q_sp, bool_t in_flight) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); #endif struct FloatQuat q_rp_cmd; #if USE_EARTH_BOUND_RC_SETPOINT stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); #else stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd); #endif /* get current heading */ const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_yaw; //Care Free mode if (guidance_h_mode == GUIDANCE_H_MODE_CARE_FREE) { //care_free_heading has been set to current psi when entering care free mode. FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, care_free_heading); } else { FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); } /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd); FLOAT_QUAT_NORMALIZE(q_rp_sp); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi); #endif /* rotation between current yaw and yaw setpoint */ struct FloatQuat q_yaw_diff; FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw); /* compute final setpoint with yaw */ FLOAT_QUAT_COMP_NORM_SHORTEST(*q_sp, q_rp_sp, q_yaw_diff); } else { QUAT_COPY(*q_sp, q_rp_sp); } }
//Function that reads the rc setpoint in an earth bound frame void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat *q_sp, bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); #endif const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_rp_cmd; stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else float_quat_of_axis_angle(&q_yaw_sp, &zaxis, stab_att_sp_euler.psi); #endif float_quat_comp(q_sp, &q_yaw_sp, &q_rp_cmd); } else { struct FloatQuat q_yaw; float_quat_of_axis_angle(&q_yaw, &zaxis, stateGetNedToBodyEulers_f()->psi); /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; float_quat_comp(&q_rp_sp, &q_yaw, &q_rp_cmd); float_quat_normalize(&q_rp_sp); QUAT_COPY(*q_sp, q_rp_sp); } }
//Function that reads the rc setpoint in an earth bound frame void stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(struct FloatQuat* q_sp, bool_t in_flight) { // FIXME: remove me, do in quaternion directly // is currently still needed, since the yaw setpoint integration is done in eulers #if defined STABILIZATION_ATTITUDE_TYPE_INT stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight); #else stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); #endif const struct FloatVect3 zaxis = {0., 0., 1.}; struct FloatQuat q_rp_cmd; stabilization_attitude_read_rc_roll_pitch_earth_quat_f(&q_rp_cmd); if (in_flight) { /* get current heading setpoint */ struct FloatQuat q_yaw_sp; #if defined STABILIZATION_ATTITUDE_TYPE_INT FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, ANGLE_FLOAT_OF_BFP(stab_att_sp_euler.psi)); #else FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw_sp, zaxis, stab_att_sp_euler.psi); #endif FLOAT_QUAT_COMP(*q_sp, q_yaw_sp, q_rp_cmd); } else { struct FloatQuat q_yaw; FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, stateGetNedToBodyEulers_f()->psi); /* roll/pitch commands applied to to current heading */ struct FloatQuat q_rp_sp; FLOAT_QUAT_COMP(q_rp_sp, q_yaw, q_rp_cmd); FLOAT_QUAT_NORMALIZE(q_rp_sp); QUAT_COPY(*q_sp, q_rp_sp); } }