/** Read attitude setpoint from RC as quaternion
* Interprets the stick positions as axes.
* @param[in] coordinated_turn true if in horizontal mode forward
* @param[in] in_carefree true if in carefree mode
* @param[in] in_flight true if in flight
* @param[out] q_sp attitude setpoint as quaternion
*/
void stabilization_attitude_read_rc_setpoint_quat_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
struct FloatQuat q_rp_cmd;
stabilization_attitude_read_rc_roll_pitch_quat_f(&q_rp_cmd);
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
//Care Free mode
if (in_carefree) {
//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);
}
}
void stabilization_attitude_set_setpoint_rp_quat_f(bool in_flight, int32_t heading)
{
struct FloatQuat q_rp_cmd;
float_quat_of_eulers(&q_rp_cmd, &guidance_euler_cmd); //TODO this is a quaternion without yaw! add the desired yaw before you use it!
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
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);
struct FloatQuat q_sp;
if (in_flight) {
/* get current heading setpoint */
struct FloatQuat q_yaw_sp;
float_quat_of_axis_angle(&q_yaw_sp, &zaxis, ANGLE_FLOAT_OF_BFP(heading));
/* 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);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat,q_sp);
}
