/*************************************************************************************** *decode the gps data packet ***************************************************************************************/ static void decode_gpspacket(struct NpsFdm *fdm, byte *buffer) { /* gps velocity (1e2 m/s to m/s */ struct NedCoor_d vel; vel.x = (double)LongOfBuf(buffer, 3) * 1.0e-2; vel.y = (double)LongOfBuf(buffer, 7) * 1.0e-2; vel.z = (double)LongOfBuf(buffer, 11) * 1.0e-2; fdm->ltp_ecef_vel = vel; ecef_of_ned_vect_d(&fdm->ecef_ecef_vel, <pdef, &vel); /* No airspeed from CRRCSIM? * use ground speed for now, since we also don't know wind */ struct DoubleVect3 ltp_airspeed; VECT3_COPY(ltp_airspeed, vel); fdm.airspeed = double_vect3_norm(<p_airspeed); /* gps position (1e7 deg to rad and 1e3 m to m) */ struct LlaCoor_d pos; pos.lon = (double)LongOfBuf(buffer, 15) * 1.74533e-9; pos.lat = (double)LongOfBuf(buffer, 19) * 1.74533e-9; pos.alt = (double)LongOfBuf(buffer, 23) * 1.0e-3; pos.lat += ltpdef.lla.lat; pos.lon += ltpdef.lla.lon; pos.alt += ltpdef.lla.alt; fdm->lla_pos = pos; ecef_of_lla_d(&fdm->ecef_pos, &pos); fdm->hmsl = pos.alt - NAV_MSL0 / 1000.; fdm->pressure = pprz_isa_pressure_of_altitude(fdm->hmsl); /* gps time */ fdm->time = (double)UShortOfBuf(buffer, 27); /* in LTP pprz */ ned_of_ecef_point_d(&fdm->ltpprz_pos, <pdef, &fdm->ecef_pos); fdm->lla_pos_pprz = pos; ned_of_ecef_vect_d(&fdm->ltpprz_ecef_vel, <pdef, &fdm->ecef_ecef_vel); #if NPS_CRRCSIM_DEBUG printf("decode gps | pos %f %f %f | vel %f %f %f | time %f\n", 57.3 * fdm->lla_pos.lat, 57.3 * fdm->lla_pos.lon, fdm->lla_pos.alt, fdm->ltp_ecef_vel.x, fdm->ltp_ecef_vel.y, fdm->ltp_ecef_vel.z, fdm->time); #endif }
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 */ }