int spi_ap_link_init() { if (spi_link_init()) { TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n"); return -1; } // Initialize IMU->Body orientation imuFloat.body_to_imu_quat = body_to_imu_quat; imuFloat.sample_count = 0; #ifdef IMU_ALIGN_BENCH // This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame) struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 }; FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH); #endif FLOAT_QUAT_NORMALIZE(imuFloat.body_to_imu_quat); FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat); FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat); struct FloatRates bias0 = { 0., 0., 0.}; rdyb_mahrs_init(imuFloat.body_to_imu_quat, bias0); return 0; }
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); }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; FLOAT_QUAT_INV_COMP(att_err, ahrs_float.ltp_to_body_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; RATES_DIFF(rate_err, stab_att_ref_rate, ahrs_float.body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &ahrs_float.body_rate_d, &stabilization_att_sum_err_quat); // FIXME: this is very dangerous! only works if this really includes all commands for (int i = COMMAND_ROLL; i <= COMMAND_YAW_SURFACE; i++) { stabilization_cmd[i] = stabilization_att_fb_cmd[i]+stabilization_att_ff_cmd[i]; } /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }
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); } }
static void send_att(void) { struct FloatEulers ltp_to_imu_euler; FLOAT_EULERS_OF_QUAT(ltp_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(); DOWNLINK_SEND_AHRS_EULER_INT(DefaultChannel, DefaultDevice, &euler_i.phi, &euler_i.theta, &euler_i.psi, &(eulers_body->phi), &(eulers_body->theta), &(eulers_body->psi)); }
void stabilization_attitude_ref_update(void) { /* integrate reference attitude */ #if STABILIZATION_ATTITUDE_REF_QUAT_INFINITESIMAL_STEP struct FloatQuat qdot; FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); QUAT_ADD(stab_att_ref_quat, qdot); #else // use finite step (involves trig) struct FloatQuat delta_q; FLOAT_QUAT_DIFFERENTIAL(delta_q, stab_att_ref_rate, DT_UPDATE); /* compose new ref_quat by quaternion multiplication of delta rotation and current ref_quat */ struct FloatQuat new_ref_quat; FLOAT_QUAT_COMP(new_ref_quat, delta_q, stab_att_ref_quat); QUAT_COPY(stab_att_ref_quat, new_ref_quat); #endif FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE); RATES_ADD(stab_att_ref_rate, delta_rate); /* compute reference angular accelerations */ struct FloatQuat err; /* compute reference attitude error */ FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(err); /* propagate the 2nd order linear model */ stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p - stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx; stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q - stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy; stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r - stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz; /* saturate acceleration */ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); /* saturate angular speed and trim accel accordingly */ SATURATE_SPEED_TRIM_ACCEL(); /* compute ref_euler */ FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat); }
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 FloatMat33 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 FloatMat33 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; }
void stabilization_attitude_ref_update() { /* integrate reference attitude */ struct FloatQuat qdot; FLOAT_QUAT_DERIVATIVE(qdot, stab_att_ref_rate, stab_att_ref_quat); QUAT_SMUL(qdot, qdot, DT_UPDATE); QUAT_ADD(stab_att_ref_quat, qdot); FLOAT_QUAT_NORMALIZE(stab_att_ref_quat); /* integrate reference rotational speeds */ struct FloatRates delta_rate; RATES_SMUL(delta_rate, stab_att_ref_accel, DT_UPDATE); RATES_ADD(stab_att_ref_rate, delta_rate); /* compute reference angular accelerations */ struct FloatQuat err; /* compute reference attitude error */ FLOAT_QUAT_INV_COMP(err, stab_att_sp_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(err); /* propagate the 2nd order linear model */ stab_att_ref_accel.p = -2.*stab_att_ref_model[ref_idx].zeta.p*stab_att_ref_model[ref_idx].omega.p*stab_att_ref_rate.p - stab_att_ref_model[ref_idx].omega.p*stab_att_ref_model[ref_idx].omega.p*err.qx; stab_att_ref_accel.q = -2.*stab_att_ref_model[ref_idx].zeta.q*stab_att_ref_model[ref_idx].omega.q*stab_att_ref_rate.q - stab_att_ref_model[ref_idx].omega.q*stab_att_ref_model[ref_idx].omega.q*err.qy; stab_att_ref_accel.r = -2.*stab_att_ref_model[ref_idx].zeta.r*stab_att_ref_model[ref_idx].omega.r*stab_att_ref_rate.r - stab_att_ref_model[ref_idx].omega.r*stab_att_ref_model[ref_idx].omega.r*err.qz; /* saturate acceleration */ const struct FloatRates MIN_ACCEL = { -REF_ACCEL_MAX_P, -REF_ACCEL_MAX_Q, -REF_ACCEL_MAX_R }; const struct FloatRates MAX_ACCEL = { REF_ACCEL_MAX_P, REF_ACCEL_MAX_Q, REF_ACCEL_MAX_R }; RATES_BOUND_BOX(stab_att_ref_accel, MIN_ACCEL, MAX_ACCEL); /* compute ref_euler */ FLOAT_EULERS_OF_QUAT(stab_att_ref_euler, stab_att_ref_quat); }
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 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; } }
static void print_estimator_state(double time) { #if FILTER_OUTPUT_IN_NED struct EcefCoor_d pos_ecef, cur_pos_ecef, cur_vel_ecef; struct NedCoor_d pos_ned, vel_ned; struct DoubleQuat q_ecef2body, q_ecef2enu, q_enu2body, q_ned2enu, q_ned2body; VECTOR_AS_VECT3(pos_ecef,pos_0_ecef); VECTOR_AS_VECT3(cur_pos_ecef,ins.avg_state.position); VECTOR_AS_VECT3(cur_vel_ecef,ins.avg_state.velocity); ned_of_ecef_point_d(&pos_ned, ¤t_ltp, &cur_pos_ecef); ned_of_ecef_vect_d(&vel_ned, ¤t_ltp, &cur_vel_ecef); int32_t xdd = 0; int32_t ydd = 0; int32_t zdd = 0; int32_t xd = vel_ned.x/0.0000019073; int32_t yd = vel_ned.y/0.0000019073; int32_t zd = vel_ned.z/0.0000019073; int32_t x = pos_ned.x/0.0039; int32_t y = pos_ned.y/0.0039; int32_t z = pos_ned.z/0.0039; fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); #if 0 QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), -ins.avg_state.orientation.x(), -ins.avg_state.orientation.y(), -ins.avg_state.orientation.z()); QUAT_ASSIGN(q_ned2enu, 0, M_SQRT1_2, M_SQRT1_2, 0); FLOAT_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); // q_enu2body = q_ecef2body * (q_ecef2enu)^* FLOAT_QUAT_COMP(q_ned2body, q_ned2enu, q_enu2body); // q_ned2body = q_enu2body * q_ned2enu #else /* if 0 */ QUATERNIOND_AS_DOUBLEQUAT(q_ecef2body, ins.avg_state.orientation); DOUBLE_QUAT_OF_RMAT(q_ecef2enu, current_ltp.ltp_of_ecef); FLOAT_QUAT_INV_COMP(q_enu2body, q_ecef2enu, q_ecef2body); QUAT_ENU_FROM_TO_NED(q_enu2body, q_ned2body); #endif /* if 0 */ struct FloatEulers e; FLOAT_EULERS_OF_QUAT(e, q_ned2body); #if PRINT_EULER_NED printf("EULER % 6.1f % 6.1f % 6.1f\n", e.phi*180*M_1_PI, e.theta*180*M_1_PI, e.psi*180*M_1_PI); #endif /* PRINT_EULER_NED */ fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e.phi, e.theta, e.psi); fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)), sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)), sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11))); fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2)); #else /* FILTER_OUTPUT_IN_ECEF */ int32_t xdd = 0; int32_t ydd = 0; int32_t zdd = 0; int32_t xd = ins.avg_state.velocity(0)/0.0000019073; int32_t yd = ins.avg_state.velocity(1)/0.0000019073; int32_t zd = ins.avg_state.velocity(2)/0.0000019073; int32_t x = ins.avg_state.position(0)/0.0039; int32_t y = ins.avg_state.position(1)/0.0039; int32_t z = ins.avg_state.position(2)/0.0039; fprintf(ins_logfile, "%f %d BOOZ2_INS2 %d %d %d %d %d %d %d %d %d\n", time, AC_ID, xdd, ydd, zdd, xd, yd, zd, x, y, z); struct FloatQuat q_ecef2body; QUAT_ASSIGN(q_ecef2body, ins.avg_state.orientation.w(), ins.avg_state.orientation.x(), ins.avg_state.orientation.y(), ins.avg_state.orientation.z()); struct FloatEulers e_ecef2body; FLOAT_EULERS_OF_QUAT(e_ecef2body, q_ecef2body); fprintf(ins_logfile, "%f %d AHRS_EULER %f %f %f\n", time, AC_ID, e_ecef2body.phi, e_ecef2body.theta, e_ecef2body.psi); fprintf(ins_logfile, "%f %d DEBUG_COVARIANCE %f %f %f %f %f %f %f %f %f %f %f %f\n", time, AC_ID, sqrt(ins.cov( 0, 0)), sqrt(ins.cov( 1, 1)), sqrt(ins.cov( 2, 2)), sqrt(ins.cov( 3, 3)), sqrt(ins.cov( 4, 4)), sqrt(ins.cov( 5, 5)), sqrt(ins.cov( 6, 6)), sqrt(ins.cov( 7, 7)), sqrt(ins.cov( 8, 8)), sqrt(ins.cov( 9, 9)), sqrt(ins.cov(10,10)), sqrt(ins.cov(11,11))); fprintf(ins_logfile, "%f %d BOOZ_SIM_GYRO_BIAS %f %f %f\n", time, AC_ID, ins.avg_state.gyro_bias(0), ins.avg_state.gyro_bias(1), ins.avg_state.gyro_bias(2)); #endif /* FILTER_OUTPUT_IN_NED / ECEF */ }
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 }
void stabilization_attitude_run(bool_t enable_integrator) { /* * Update reference */ stabilization_attitude_ref_update(); /* * Compute errors for feedback */ /* attitude error */ struct FloatQuat att_err; struct FloatQuat* att_quat = stateGetNedToBodyQuat_f(); FLOAT_QUAT_INV_COMP(att_err, *att_quat, stab_att_ref_quat); /* wrap it in the shortest direction */ FLOAT_QUAT_WRAP_SHORTEST(att_err); /* rate error */ struct FloatRates rate_err; struct FloatRates* body_rate = stateGetBodyRates_f(); RATES_DIFF(rate_err, stab_att_ref_rate, *body_rate); /* rate_d error */ struct FloatRates body_rate_d; RATES_DIFF(body_rate_d, *body_rate, last_body_rate); RATES_COPY(last_body_rate, *body_rate); /* integrated error */ if (enable_integrator) { struct FloatQuat new_sum_err, scaled_att_err; /* update accumulator */ scaled_att_err.qi = att_err.qi; scaled_att_err.qx = att_err.qx / IERROR_SCALE; scaled_att_err.qy = att_err.qy / IERROR_SCALE; scaled_att_err.qz = att_err.qz / IERROR_SCALE; FLOAT_QUAT_COMP(new_sum_err, stabilization_att_sum_err_quat, scaled_att_err); FLOAT_QUAT_NORMALIZE(new_sum_err); FLOAT_QUAT_COPY(stabilization_att_sum_err_quat, new_sum_err); FLOAT_EULERS_OF_QUAT(stabilization_att_sum_err_eulers, stabilization_att_sum_err_quat); } else { /* reset accumulator */ FLOAT_QUAT_ZERO( stabilization_att_sum_err_quat ); FLOAT_EULERS_ZERO( stabilization_att_sum_err_eulers ); } attitude_run_ff(stabilization_att_ff_cmd, &stabilization_gains[gain_idx], &stab_att_ref_accel); attitude_run_fb(stabilization_att_fb_cmd, &stabilization_gains[gain_idx], &att_err, &rate_err, &body_rate_d, &stabilization_att_sum_err_quat); stabilization_cmd[COMMAND_ROLL] = stabilization_att_fb_cmd[COMMAND_ROLL] + stabilization_att_ff_cmd[COMMAND_ROLL]; stabilization_cmd[COMMAND_PITCH] = stabilization_att_fb_cmd[COMMAND_PITCH] + stabilization_att_ff_cmd[COMMAND_PITCH]; stabilization_cmd[COMMAND_YAW] = stabilization_att_fb_cmd[COMMAND_YAW] + stabilization_att_ff_cmd[COMMAND_YAW]; #ifdef HAS_SURFACE_COMMANDS stabilization_cmd[COMMAND_ROLL_SURFACE] = stabilization_att_fb_cmd[COMMAND_ROLL_SURFACE] + stabilization_att_ff_cmd[COMMAND_ROLL_SURFACE]; stabilization_cmd[COMMAND_PITCH_SURFACE] = stabilization_att_fb_cmd[COMMAND_PITCH_SURFACE] + stabilization_att_ff_cmd[COMMAND_PITCH_SURFACE]; stabilization_cmd[COMMAND_YAW_SURFACE] = stabilization_att_fb_cmd[COMMAND_YAW_SURFACE] + stabilization_att_ff_cmd[COMMAND_YAW_SURFACE]; #endif /* bound the result */ BoundAbs(stabilization_cmd[COMMAND_ROLL], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_PITCH], MAX_PPRZ); BoundAbs(stabilization_cmd[COMMAND_YAW], MAX_PPRZ); }