float test_quat_comp_inv(struct FloatQuat qa2c_f, struct FloatQuat qb2c_f, int display)
{
struct FloatQuat qa2b_f;
float_quat_comp_inv(&qa2b_f, &qa2c_f, &qb2c_f);
struct Int32Quat qa2c_i;
QUAT_BFP_OF_REAL(qa2c_i, qa2c_f);
struct Int32Quat qb2c_i;
QUAT_BFP_OF_REAL(qb2c_i, qb2c_f);
struct Int32Quat qa2b_i;
int32_quat_comp_inv(&qa2b_i, &qa2c_i, &qb2c_i);
struct FloatQuat err;
QUAT_DIFF(err, qa2b_f, qa2b_i);
float norm_err = FLOAT_QUAT_NORM(err);
if (display) {
printf("quat comp_inv\n");
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("a2bf", qa2b_f);
DISPLAY_INT32_QUAT_AS_EULERS_DEG("a2bi", qa2b_i);
}
return norm_err;
}
void orientationCalcRMat_i(struct OrientationReps* orientation) {
if (bit_is_set(orientation->status, ORREP_RMAT_I))
return;
if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
}
else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
}
else if (bit_is_set(orientation->status, ORREP_EULER_I)) {
INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
}
else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
SetBit(orientation->status, ORREP_QUAT_I);
INT32_RMAT_OF_QUAT(orientation->rmat_i, orientation->quat_i);
}
else if (bit_is_set(orientation->status, ORREP_EULER_F)) {
EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
SetBit(orientation->status, ORREP_EULER_I);
INT32_RMAT_OF_EULERS(orientation->rmat_i, orientation->eulers_i);
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_RMAT_I);
}
void quat_from_rpy_cmd_i(struct Int32Quat *quat, struct Int32Eulers *rpy) {
struct FloatEulers rpy_f;
EULERS_FLOAT_OF_BFP(rpy_f, *rpy);
struct FloatQuat quat_f;
quat_from_rpy_cmd_f(&quat_f, &rpy_f);
QUAT_BFP_OF_REAL(*quat, quat_f);
}
void stabilization_attitude_read_rc(bool_t in_flight, bool_t in_carefree, bool_t coordinated_turn) {
struct FloatQuat q_sp;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_setpoint_quat_earth_bound_f(&q_sp, in_flight, in_carefree, coordinated_turn);
#else
stabilization_attitude_read_rc_setpoint_quat_f(&q_sp, in_flight, in_carefree, coordinated_turn);
#endif
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
}
void sim_overwrite_ahrs(void) {
EULERS_BFP_OF_REAL(ahrs.ltp_to_body_euler, fdm.ltp_to_body_eulers);
QUAT_BFP_OF_REAL(ahrs.ltp_to_body_quat, fdm.ltp_to_body_quat);
RATES_BFP_OF_REAL(ahrs.body_rate, fdm.body_ecef_rotvel);
INT32_RMAT_OF_QUAT(ahrs.ltp_to_body_rmat, ahrs.ltp_to_body_quat);
}
void sim_overwrite_ahrs(void) {
struct Int32Quat quat;
QUAT_BFP_OF_REAL(quat, fdm.ltp_to_body_quat);
stateSetNedToBodyQuat_i(&quat);
struct Int32Rates rates;
RATES_BFP_OF_REAL(rates, fdm.body_ecef_rotvel);
stateSetBodyRates_i(&rates);
}
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);
}
void stabilization_attitude_read_rc(bool_t in_flight) {
#if USE_SETPOINTS_WITH_TRANSITIONS
stabilization_attitude_read_rc_absolute(in_flight);
#else
//FIXME: remove me, do in quaternion directly
stabilization_attitude_read_rc_setpoint_eulers(&stab_att_sp_euler, in_flight);
struct FloatQuat q_rp_cmd;
#if USE_EARTH_BOUND_RC_SETPOINT
stabilization_attitude_read_rc_roll_pitch_earth_quat(&q_rp_cmd);
#else
stabilization_attitude_read_rc_roll_pitch_quat(&q_rp_cmd);
#endif
/* get current heading */
const struct FloatVect3 zaxis = {0., 0., 1.};
struct FloatQuat q_yaw;
FLOAT_QUAT_OF_AXIS_ANGLE(q_yaw, zaxis, ANGLE_FLOAT_OF_BFP(stateGetNedToBodyEulers_i()->psi));
/* apply roll and pitch commands with respect to current heading */
struct FloatQuat q_sp;
FLOAT_QUAT_COMP(q_sp, q_yaw, q_rp_cmd);
FLOAT_QUAT_NORMALIZE(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(stab_att_sp_euler.psi));
/* rotation between current yaw and yaw setpoint */
struct FloatQuat q_yaw_diff;
FLOAT_QUAT_COMP_INV(q_yaw_diff, q_yaw_sp, q_yaw);
/* temporary copy with roll/pitch command and current heading */
struct FloatQuat q_rp_sp;
QUAT_COPY(q_rp_sp, q_sp);
/* compute final setpoint with yaw */
FLOAT_QUAT_COMP_NORM_SHORTEST(q_sp, q_rp_sp, q_yaw_diff);
}
QUAT_BFP_OF_REAL(stab_att_sp_quat, q_sp);
#endif
}
float test_eulers_of_quat(struct FloatQuat fq, int display) {
struct FloatEulers fe;
FLOAT_EULERS_OF_QUAT(fe, fq);
struct Int32Quat iq;
QUAT_BFP_OF_REAL(iq, fq);
struct Int32Eulers ie;
INT32_EULERS_OF_QUAT(ie, iq);
struct FloatEulers fe2;
EULERS_FLOAT_OF_BFP(fe2, ie);
EULERS_SUB(fe2, ie);
float norm_err = FLOAT_EULERS_NORM(fe2);
if (display) {
printf("euler of quat\n");
DISPLAY_FLOAT_QUAT("fq", fq);
DISPLAY_FLOAT_EULERS_DEG("fe", fe);
DISPLAY_INT32_EULERS("ie", ie);
DISPLAY_INT32_EULERS_AS_FLOAT_DEG("ieaf", ie);
}
return norm_err;
}
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);
}
void orientationCalcEulers_i(struct OrientationReps* orientation)
{
if (bit_is_set(orientation->status, ORREP_EULER_I)) {
return;
}
if (bit_is_set(orientation->status, ORREP_EULER_F)) {
EULERS_BFP_OF_REAL(orientation->eulers_i, orientation->eulers_f);
} else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
} else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
} else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
RMAT_BFP_OF_REAL(orientation->rmat_i, orientation->rmat_f);
SetBit(orientation->status, ORREP_RMAT_I);
int32_eulers_of_rmat(&(orientation->eulers_i), &(orientation->rmat_i));
} else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
QUAT_BFP_OF_REAL(orientation->quat_i, orientation->quat_f);
SetBit(orientation->status, ORREP_QUAT_I);
int32_eulers_of_quat(&(orientation->eulers_i), &(orientation->quat_i));
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_EULER_I);
}
void UM6_packet_read_message(void)
{
if ((UM6_status == UM6Running) && PacketLength > 11) {
switch (PacketAddr) {
case IMU_UM6_GYRO_PROC:
UM6_rate.p =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0610352;
UM6_rate.q =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0610352;
UM6_rate.r =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0610352;
RATES_SMUL(UM6_rate, UM6_rate, 0.0174532925); //Convert deg/sec to rad/sec
RATES_BFP_OF_REAL(imu.gyro, UM6_rate);
break;
case IMU_UM6_ACCEL_PROC:
UM6_accel.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000183105;
UM6_accel.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000183105;
UM6_accel.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000183105;
VECT3_SMUL(UM6_accel, UM6_accel, 9.80665); //Convert g into m/s2
ACCELS_BFP_OF_REAL(imu.accel, UM6_accel); // float to int
break;
case IMU_UM6_MAG_PROC:
UM6_mag.x =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.000305176;
UM6_mag.y =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.000305176;
UM6_mag.z =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.000305176;
// Assume the same units for magnetic field
// Magnitude at the Earth's surface ranges from 25 to 65 microteslas (0.25 to 0.65 gauss).
MAGS_BFP_OF_REAL(imu.mag, UM6_mag);
break;
case IMU_UM6_EULER:
UM6_eulers.phi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0109863;
UM6_eulers.theta =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0109863;
UM6_eulers.psi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0109863;
EULERS_SMUL(UM6_eulers, UM6_eulers, 0.0174532925); //Convert deg to rad
EULERS_BFP_OF_REAL(ahrs_impl.ltp_to_imu_euler, UM6_eulers);
break;
case IMU_UM6_QUAT:
UM6_quat.qi =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 0] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 1]))) * 0.0000335693;
UM6_quat.qx =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 2] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 3]))) * 0.0000335693;
UM6_quat.qy =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 4] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 5]))) * 0.0000335693;
UM6_quat.qz =
((float)((int16_t)
((UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 6] << 8) | UM6_packet.msg_buf[IMU_UM6_DATA_OFFSET + 7]))) * 0.0000335693;
QUAT_BFP_OF_REAL(ahrs_impl.ltp_to_imu_quat, UM6_quat);
break;
default:
break;
}
}
}