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); );
void ahrs_propagate(void) { struct NedCoor_f accel; struct FloatRates body_rates; struct FloatEulers eulers; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; INT32_RMAT_TRANSP_RATEMULT(gyro_meas_body, imu.body_to_imu_rmat, imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; INT32_RMAT_TRANSP_VMULT(accel_meas_body, imu.body_to_imu_rmat, imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float*)&new_state, (float*)&ins_impl.state, INV_STATE_DIM, (float*)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); #if INS_UPDATE_FW_ESTIMATOR // Some stupid lines of code for neutrals eulers.phi -= ins_roll_neutral; eulers.theta -= ins_pitch_neutral; stateSetNedToBodyEulers_f(&eulers); #else stateSetNedToBodyQuat_f(&ins_impl.state.quat); #endif RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity FLOAT_QUAT_RMAT_B2N(accel, ins_impl.state.quat, ins_impl.cmd.accel); FLOAT_VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); FLOAT_VECT3_ADD(accel, A); stateSetAccelNed_f(&accel); //------------------------------------------------------------// RunOnceEvery(3,{ DOWNLINK_SEND_INV_FILTER(DefaultChannel, DefaultDevice, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #endif }
void ahrs_propagate(float dt) { struct FloatVect3 accel; struct FloatRates body_rates; // realign all the filter if needed // a complete init cycle is required if (ins_impl.reset) { ins_impl.reset = FALSE; ins.status = INS_UNINIT; ahrs.status = AHRS_UNINIT; init_invariant_state(); } // fill command vector struct Int32Rates gyro_meas_body; struct Int32RMat *body_to_imu_rmat = orientationGetRMat_i(&imu.body_to_imu); int32_rmat_transp_ratemult(&gyro_meas_body, body_to_imu_rmat, &imu.gyro); RATES_FLOAT_OF_BFP(ins_impl.cmd.rates, gyro_meas_body); struct Int32Vect3 accel_meas_body; int32_rmat_transp_vmult(&accel_meas_body, body_to_imu_rmat, &imu.accel); ACCELS_FLOAT_OF_BFP(ins_impl.cmd.accel, accel_meas_body); // update correction gains error_output(&ins_impl); // propagate model struct inv_state new_state; runge_kutta_4_float((float *)&new_state, (float *)&ins_impl.state, INV_STATE_DIM, (float *)&ins_impl.cmd, INV_COMMAND_DIM, invariant_model, dt); ins_impl.state = new_state; // normalize quaternion FLOAT_QUAT_NORMALIZE(ins_impl.state.quat); // set global state stateSetNedToBodyQuat_f(&ins_impl.state.quat); RATES_DIFF(body_rates, ins_impl.cmd.rates, ins_impl.state.bias); stateSetBodyRates_f(&body_rates); stateSetPositionNed_f(&ins_impl.state.pos); stateSetSpeedNed_f(&ins_impl.state.speed); // untilt accel and remove gravity struct FloatQuat q_b2n; float_quat_invert(&q_b2n, &ins_impl.state.quat); float_quat_vmult(&accel, &q_b2n, &ins_impl.cmd.accel); VECT3_SMUL(accel, accel, 1. / (ins_impl.state.as)); VECT3_ADD(accel, A); stateSetAccelNed_f((struct NedCoor_f *)&accel); //------------------------------------------------------------// #if SEND_INVARIANT_FILTER struct FloatEulers eulers; FLOAT_EULERS_OF_QUAT(eulers, ins_impl.state.quat); RunOnceEvery(3, { pprz_msg_send_INV_FILTER(trans, dev, AC_ID, &ins_impl.state.quat.qi, &eulers.phi, &eulers.theta, &eulers.psi, &ins_impl.state.speed.x, &ins_impl.state.speed.y, &ins_impl.state.speed.z, &ins_impl.state.pos.x, &ins_impl.state.pos.y, &ins_impl.state.pos.z, &ins_impl.state.bias.p, &ins_impl.state.bias.q, &ins_impl.state.bias.r, &ins_impl.state.as, &ins_impl.state.hb, &ins_impl.meas.baro_alt, &ins_impl.meas.pos_gps.z) }); #endif #if LOG_INVARIANT_FILTER if (pprzLogFile.fs != NULL) { if (!log_started) { // log file header sdLogWriteLog(&pprzLogFile, "p q r ax ay az gx gy gz gvx gvy gvz mx my mz b qi qx qy qz bp bq br vx vy vz px py pz hb as\n"); log_started = TRUE; } else { sdLogWriteLog(&pprzLogFile, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f %.3f\n", ins_impl.cmd.rates.p, ins_impl.cmd.rates.q, ins_impl.cmd.rates.r, ins_impl.cmd.accel.x, ins_impl.cmd.accel.y, ins_impl.cmd.accel.z, ins_impl.meas.pos_gps.x, ins_impl.meas.pos_gps.y, ins_impl.meas.pos_gps.z, ins_impl.meas.speed_gps.x, ins_impl.meas.speed_gps.y, ins_impl.meas.speed_gps.z, ins_impl.meas.mag.x, ins_impl.meas.mag.y, ins_impl.meas.mag.z, ins_impl.meas.baro_alt, ins_impl.state.quat.qi, ins_impl.state.quat.qx, ins_impl.state.quat.qy, ins_impl.state.quat.qz, ins_impl.state.bias.p, ins_impl.state.bias.q, ins_impl.state.bias.r, ins_impl.state.speed.x, ins_impl.state.speed.y, ins_impl.state.speed.z, ins_impl.state.pos.x, ins_impl.state.pos.y, ins_impl.state.pos.z, ins_impl.state.hb, ins_impl.state.as); } } #endif }