static void send_att(struct transport_tx *trans, struct link_device *dev)
{
/* compute eulers in int (IMU frame) */
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_float_inv.state.quat);
struct Int32Eulers eulers_imu;
EULERS_BFP_OF_REAL(eulers_imu, ltp_to_imu_euler);
/* compute Eulers in int (body frame) */
struct FloatQuat ltp_to_body_quat;
struct FloatQuat *body_to_imu_quat = orientationGetQuat_f(&ahrs_float_inv.body_to_imu);
float_quat_comp_inv(<p_to_body_quat, &ahrs_float_inv.state.quat, body_to_imu_quat);
struct FloatEulers ltp_to_body_euler;
float_eulers_of_quat(<p_to_body_euler, <p_to_body_quat);
struct Int32Eulers eulers_body;
EULERS_BFP_OF_REAL(eulers_body, ltp_to_body_euler);
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&eulers_imu.phi,
&eulers_imu.theta,
&eulers_imu.psi,
&eulers_body.phi,
&eulers_body.theta,
&eulers_body.psi,
&ahrs_finv_id);
}
static void send_inv_filter(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers eulers;
struct FloatQuat quat = ins_mekf_wind_get_quat();
float_eulers_of_quat(&eulers, &quat);
//struct FloatRates rates = ins_mekf_wind_get_body_rates();
struct NedCoor_f pos = ins_mekf_wind_get_pos_ned();
struct NedCoor_f speed = ins_mekf_wind_get_speed_ned();
//struct NedCoor_f accel = ins_mekf_wind_get_accel_ned();
struct FloatVect3 ab = ins_mekf_wind_get_accel_bias();
struct FloatRates rb = ins_mekf_wind_get_rates_bias();
float airspeed = ins_mekf_wind_get_airspeed_norm();
pprz_msg_send_INV_FILTER(trans, dev,
AC_ID,
&quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&speed.x,
&speed.y,
&speed.z,
&pos.x,
&pos.y,
&pos.z,
&rb.p,
&rb.q,
&rb.r,
&ab.x,
&ab.y,
&ab.z,
&airspeed);
}
void ahrs_float_invariant_propagate(struct FloatRates* gyro, float dt)
{
// realign all the filter if needed
// a complete init cycle is required
if (ahrs_float_inv.reset) {
ahrs_float_inv.reset = false;
ahrs_float_inv.is_aligned = false;
init_invariant_state();
}
// fill command vector
struct FloatRates gyro_meas_body;
struct FloatRMat *body_to_imu_rmat = orientationGetRMat_f(&ahrs_float_inv.body_to_imu);
float_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, gyro);
ahrs_float_inv.cmd.rates = gyro_meas_body;
// update correction gains
error_output(&ahrs_float_inv);
// propagate model
struct inv_state new_state;
runge_kutta_4_float((float *)&new_state,
(float *)&ahrs_float_inv.state, INV_STATE_DIM,
(float *)&ahrs_float_inv.cmd, INV_COMMAND_DIM,
invariant_model, dt);
ahrs_float_inv.state = new_state;
// normalize quaternion
float_quat_normalize(&ahrs_float_inv.state.quat);
//------------------------------------------------------------//
#if SEND_INVARIANT_FILTER
struct FloatEulers eulers;
float foo = 0.f;
float_eulers_of_quat(&eulers, &ahrs_float_inv.state.quat);
RunOnceEvery(3,
pprz_msg_send_INV_FILTER(&(DefaultChannel).trans_tx, &(DefaultDevice).device,
AC_ID,
&ahrs_float_inv.state.quat.qi,
&eulers.phi,
&eulers.theta,
&eulers.psi,
&foo,
&foo,
&foo,
&foo,
&foo,
&foo,
&ahrs_float_inv.state.bias.p,
&ahrs_float_inv.state.bias.q,
&ahrs_float_inv.state.bias.r,
&ahrs_float_inv.state.as,
&ahrs_float_inv.state.cs,
&foo,
&foo);
);
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_mlkf.ltp_to_imu_quat);
pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
<p_to_imu_euler.phi,
<p_to_imu_euler.theta,
<p_to_imu_euler.psi,
&ahrs_mlkf_id);
}
static void traj_static_sine_update(void)
{
aos.imu_rates.p = RadOfDeg(200) * cos(aos.time);
aos.imu_rates.q = RadOfDeg(200) * cos(0.7 * aos.time + 2);
aos.imu_rates.r = RadOfDeg(200) * cos(0.8 * aos.time + 1);
float_quat_integrate(&aos.ltp_to_imu_quat, &aos.imu_rates, aos.dt);
float_eulers_of_quat(&aos.ltp_to_imu_euler, &aos.ltp_to_imu_quat);
}
static void send_euler(struct transport_tx *trans, struct link_device *dev)
{
struct FloatEulers ltp_to_imu_euler;
struct FloatQuat quat = ins_mekf_wind_get_quat();
float_eulers_of_quat(<p_to_imu_euler, &quat);
uint8_t id = INS_MEKF_WIND_FILTER_ID;
pprz_msg_send_AHRS_EULER(trans, dev, AC_ID,
<p_to_imu_euler.phi,
<p_to_imu_euler.theta,
<p_to_imu_euler.psi,
&id);
}
static void send_att(struct transport_tx *trans, struct link_device *dev) {
struct FloatEulers ltp_to_imu_euler;
float_eulers_of_quat(<p_to_imu_euler, &ahrs_impl.ltp_to_imu_quat);
struct Int32Eulers euler_i;
EULERS_BFP_OF_REAL(euler_i, ltp_to_imu_euler);
struct Int32Eulers* eulers_body = stateGetNedToBodyEulers_i();
pprz_msg_send_AHRS_EULER_INT(trans, dev, AC_ID,
&euler_i.phi,
&euler_i.theta,
&euler_i.psi,
&(eulers_body->phi),
&(eulers_body->theta),
&(eulers_body->psi));
}
static void traj_step_phi_2nd_order_update(void)
{
if (aos.time > 15) {
const float omega = RadOfDeg(100);
const float xi = 0.9;
struct FloatRates raccel;
RATES_ASSIGN(raccel, -2.*xi * omega * aos.imu_rates.p - omega * omega * (aos.ltp_to_imu_euler.phi - RadOfDeg(5)), 0.,
0.);
float_rates_integrate_fi(&aos.imu_rates, &raccel, aos.dt);
float_quat_integrate(&aos.ltp_to_imu_quat, &aos.imu_rates, aos.dt);
float_eulers_of_quat(&aos.ltp_to_imu_euler, &aos.ltp_to_imu_quat);
}
}
void orientationCalcEulers_f(struct OrientationReps* orientation)
{
if (bit_is_set(orientation->status, ORREP_EULER_F)) {
return;
}
if (bit_is_set(orientation->status, ORREP_EULER_I)) {
EULERS_FLOAT_OF_BFP(orientation->eulers_f, orientation->eulers_i);
} else if (bit_is_set(orientation->status, ORREP_RMAT_F)) {
float_eulers_of_rmat(&(orientation->eulers_f), &(orientation->rmat_f));
} else if (bit_is_set(orientation->status, ORREP_QUAT_F)) {
float_eulers_of_quat(&(orientation->eulers_f), &(orientation->quat_f));
} else if (bit_is_set(orientation->status, ORREP_RMAT_I)) {
RMAT_FLOAT_OF_BFP(orientation->rmat_f, orientation->rmat_i);
SetBit(orientation->status, ORREP_RMAT_F);
float_eulers_of_rmat(&(orientation->eulers_f), &(orientation->rmat_f));
} else if (bit_is_set(orientation->status, ORREP_QUAT_I)) {
QUAT_FLOAT_OF_BFP(orientation->quat_f, orientation->quat_i);
SetBit(orientation->status, ORREP_QUAT_F);
float_eulers_of_quat(&(orientation->eulers_f), &(orientation->quat_f));
}
/* set bit to indicate this representation is computed */
SetBit(orientation->status, ORREP_EULER_F);
}
float test_quat_of_rmat(void)
{
// struct FloatEulers eul = {-0.280849, 0.613423, -1.850440};
struct FloatEulers eul = {RadOfDeg(0.131579), RadOfDeg(-62.397659), RadOfDeg(-110.470299)};
// struct FloatEulers eul = {RadOfDeg(0.13), RadOfDeg(180.), RadOfDeg(-61.)};
struct FloatRMat rm;
float_rmat_of_eulers(&rm, &eul);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("rmat", rm);
struct FloatQuat q;
float_quat_of_rmat(&q, &rm);
DISPLAY_FLOAT_QUAT("q_of_rm ", q);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_rm ", q);
struct FloatQuat qref;
float_quat_of_eulers(&qref, &eul);
DISPLAY_FLOAT_QUAT("q_of_euler", qref);
DISPLAY_FLOAT_QUAT_AS_EULERS_DEG("q_of_euler", qref);
printf("\n\n\n");
struct FloatRMat r_att;
struct FloatEulers e312 = { eul.phi, eul.theta, eul.psi };
float_rmat_of_eulers_312(&r_att, &e312);
DISPLAY_FLOAT_RMAT("r_att ", r_att);
DISPLAY_FLOAT_RMAT_AS_EULERS_DEG("r_att ", r_att);
struct FloatQuat q_att;
float_quat_of_rmat(&q_att, &r_att);
struct FloatEulers e_att;
float_eulers_of_rmat(&e_att, &r_att);
DISPLAY_FLOAT_EULERS_DEG("of rmat", e_att);
float_eulers_of_quat(&e_att, &q_att);
DISPLAY_FLOAT_EULERS_DEG("of quat", e_att);
return 0.;
}
static void test_4_float(void)
{
printf("euler to quat to euler - float\n");
/* initial euler angles */
struct FloatEulers e;
EULERS_ASSIGN(e, RadOfDeg(-10.66), RadOfDeg(-0.7), RadOfDeg(0.));
DISPLAY_FLOAT_EULERS_DEG("e", e);
/* transform to quaternion */
struct FloatQuat q;
float_quat_of_eulers(&q, &e);
// DISPLAY_FLOAT_QUAT("q", q);
float_quat_normalize(&q);
DISPLAY_FLOAT_QUAT("q_n", q);
DISPLAY_FLOAT_QUAT_AS_INT("q_n as int", q);
/* back to eulers */
struct FloatEulers e2;
float_eulers_of_quat(&e2, &q);
DISPLAY_FLOAT_EULERS_DEG("e2", e2);
}
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;
}
static inline void parse_ins_msg(void)
{
if (last_received_packet.ErrID != VN100_Error_None) {
//TODO send error
return;
}
// parse message (will work only with read and write register)
switch (last_received_packet.RegID) {
case VN100_REG_ADOR :
ins_ador = last_received_packet.Data[0].UInt;
break;
case VN100_REG_ADOF :
ins_adof = last_received_packet.Data[0].UInt;
break;
case VN100_REG_SBAUD :
ins_baud = last_received_packet.Data[0].UInt;
break;
case VN100_REG_YPR :
ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
break;
case VN100_REG_QTN :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
break;
case VN100_REG_QTM :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
ins_mag.x = last_received_packet.Data[4].Float;
ins_mag.y = last_received_packet.Data[5].Float;
ins_mag.z = last_received_packet.Data[6].Float;
break;
case VN100_REG_QTA :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
ins_accel.x = last_received_packet.Data[4].Float;
ins_accel.y = last_received_packet.Data[5].Float;
ins_accel.z = last_received_packet.Data[6].Float;
break;
case VN100_REG_QTR :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
ins_rates.p = last_received_packet.Data[4].Float;
ins_rates.q = last_received_packet.Data[5].Float;
ins_rates.r = last_received_packet.Data[6].Float;
break;
case VN100_REG_QMA :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
ins_mag.x = last_received_packet.Data[4].Float;
ins_mag.y = last_received_packet.Data[5].Float;
ins_mag.z = last_received_packet.Data[6].Float;
ins_accel.x = last_received_packet.Data[7].Float;
ins_accel.y = last_received_packet.Data[8].Float;
ins_accel.z = last_received_packet.Data[9].Float;
break;
case VN100_REG_QAR :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
ins_accel.x = last_received_packet.Data[4].Float;
ins_accel.y = last_received_packet.Data[5].Float;
ins_accel.z = last_received_packet.Data[6].Float;
ins_rates.p = last_received_packet.Data[7].Float;
ins_rates.q = last_received_packet.Data[8].Float;
ins_rates.r = last_received_packet.Data[9].Float;
break;
case VN100_REG_QMR :
ins_quat.qi = last_received_packet.Data[0].Float;
ins_quat.qx = last_received_packet.Data[1].Float;
ins_quat.qy = last_received_packet.Data[2].Float;
ins_quat.qz = last_received_packet.Data[3].Float;
float_eulers_of_quat(&ins_eulers, &ins_quat);
ins_mag.x = last_received_packet.Data[4].Float;
ins_mag.y = last_received_packet.Data[5].Float;
ins_mag.z = last_received_packet.Data[6].Float;
ins_accel.x = last_received_packet.Data[7].Float;
ins_accel.y = last_received_packet.Data[8].Float;
ins_accel.z = last_received_packet.Data[9].Float;
ins_rates.p = last_received_packet.Data[10].Float;
ins_rates.q = last_received_packet.Data[11].Float;
ins_rates.r = last_received_packet.Data[12].Float;
break;
case VN100_REG_YMR :
ins_eulers.phi = RadOfDeg(last_received_packet.Data[2].Float);
ins_eulers.theta = RadOfDeg(last_received_packet.Data[1].Float);
ins_eulers.psi = RadOfDeg(last_received_packet.Data[0].Float);
ins_mag.x = last_received_packet.Data[3].Float;
ins_mag.y = last_received_packet.Data[4].Float;
ins_mag.z = last_received_packet.Data[5].Float;
ins_accel.x = last_received_packet.Data[6].Float;
ins_accel.y = last_received_packet.Data[7].Float;
ins_accel.z = last_received_packet.Data[8].Float;
ins_rates.p = last_received_packet.Data[9].Float;
ins_rates.q = last_received_packet.Data[10].Float;
ins_rates.r = last_received_packet.Data[11].Float;
break;
default:
break;
}
}