void stabilization_attitude_set_earth_cmd_i(struct Int32Vect2 *cmd, int32_t heading) { struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); float heading_f; heading_f = ANGLE_FLOAT_OF_BFP(heading); quat_from_earth_cmd_f(&stab_att_sp_quat, &cmd_f, heading_f); }
void quat_from_earth_cmd_i(struct Int32Quat *quat, struct Int32Vect2 *cmd, int32_t heading) { // use float conversion for now... struct FloatVect2 cmd_f; cmd_f.x = ANGLE_FLOAT_OF_BFP(cmd->x); cmd_f.y = ANGLE_FLOAT_OF_BFP(cmd->y); float heading_f = ANGLE_FLOAT_OF_BFP(heading); struct FloatQuat quat_f; quat_from_earth_cmd_f(&quat_f, &cmd_f, heading_f); // convert back to fixed point QUAT_BFP_OF_REAL(*quat, quat_f); }