//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); } }
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) { stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight, in_carefree, coordinated_turn); }
void stabilization_attitude_read_rc(bool_t in_flight) { stabilization_attitude_read_rc_setpoint_eulers_f(&stab_att_sp_euler, in_flight); }