void Replay::flush_and_exit() { flush_dataflash(); if (check_solution) { report_checks(); } exit(0); }
void Replay::flush_and_exit() { flush_dataflash(); if (check_solution) { report_checks(); } if (packet_counts) { show_packet_counts(); } exit(0); }
void Replay::loop() { while (true) { char type[5]; if (arm_time_ms >= 0 && AP_HAL::millis() > (uint32_t)arm_time_ms) { if (!hal.util->get_soft_armed()) { hal.util->set_soft_armed(true); ::printf("Arming at %u ms\n", (unsigned)AP_HAL::millis()); } } if (!logreader.update(type)) { ::printf("End of log at %.1f seconds\n", AP_HAL::millis()*0.001f); fclose(plotf); break; } read_sensors(type); if (streq(type,"ATT")) { Vector3f ekf_euler; Vector3f velNED; Vector3f posNED; Vector3f gyroBias; float accelWeighting; float accelZBias1; float accelZBias2; Vector3f windVel; Vector3f magNED; Vector3f magXYZ; Vector3f DCM_attitude; Vector3f ekf_relpos; Vector3f velInnov; Vector3f posInnov; Vector3f magInnov; float tasInnov; float velVar; float posVar; float hgtVar; Vector3f magVar; float tasVar; Vector2f offset; uint16_t faultStatus; const Matrix3f &dcm_matrix = _vehicle.ahrs.AP_AHRS_DCM::get_rotation_body_to_ned(); dcm_matrix.to_euler(&DCM_attitude.x, &DCM_attitude.y, &DCM_attitude.z); _vehicle.EKF.getEulerAngles(ekf_euler); _vehicle.EKF.getVelNED(velNED); _vehicle.EKF.getPosNED(posNED); _vehicle.EKF.getGyroBias(gyroBias); _vehicle.EKF.getIMU1Weighting(accelWeighting); _vehicle.EKF.getAccelZBias(accelZBias1, accelZBias2); _vehicle.EKF.getWind(windVel); _vehicle.EKF.getMagNED(magNED); _vehicle.EKF.getMagXYZ(magXYZ); _vehicle.EKF.getInnovations(velInnov, posInnov, magInnov, tasInnov); _vehicle.EKF.getVariances(velVar, posVar, hgtVar, magVar, tasVar, offset); _vehicle.EKF.getFilterFaults(faultStatus); _vehicle.EKF.getPosNED(ekf_relpos); Vector3f inav_pos = _vehicle.inertial_nav.get_position() * 0.01f; float temp = degrees(ekf_euler.z); if (temp < 0.0f) temp = temp + 360.0f; fprintf(plotf, "%.3f %.1f %.1f %.1f %.2f %.1f %.1f %.1f %.2f %.2f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.2f %.2f %.2f %.2f %.2f %.2f\n", AP_HAL::millis() * 0.001f, logreader.get_sim_attitude().x, logreader.get_sim_attitude().y, logreader.get_sim_attitude().z, _vehicle.barometer.get_altitude(), logreader.get_attitude().x, logreader.get_attitude().y, wrap_180_cd(logreader.get_attitude().z*100)*0.01f, logreader.get_inavpos().x, logreader.get_inavpos().y, logreader.get_ahr2_attitude().x, logreader.get_ahr2_attitude().y, wrap_180_cd(logreader.get_ahr2_attitude().z*100)*0.01f, degrees(DCM_attitude.x), degrees(DCM_attitude.y), degrees(DCM_attitude.z), degrees(ekf_euler.x), degrees(ekf_euler.y), degrees(ekf_euler.z), inav_pos.x, inav_pos.y, inav_pos.z, ekf_relpos.x, ekf_relpos.y, -ekf_relpos.z); fprintf(plotf2, "%.3f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f %.1f\n", AP_HAL::millis() * 0.001f, degrees(ekf_euler.x), degrees(ekf_euler.y), temp, velNED.x, velNED.y, velNED.z, posNED.x, posNED.y, posNED.z, 60*degrees(gyroBias.x), 60*degrees(gyroBias.y), 60*degrees(gyroBias.z), windVel.x, windVel.y, magNED.x, magNED.y, magNED.z, magXYZ.x, magXYZ.y, magXYZ.z, logreader.get_attitude().x, logreader.get_attitude().y, logreader.get_attitude().z); // define messages for EKF1 data packet int16_t roll = (int16_t)(100*degrees(ekf_euler.x)); // roll angle (centi-deg) int16_t pitch = (int16_t)(100*degrees(ekf_euler.y)); // pitch angle (centi-deg) uint16_t yaw = (uint16_t)wrap_360_cd(100*degrees(ekf_euler.z)); // yaw angle (centi-deg) float velN = (float)(velNED.x); // velocity North (m/s) float velE = (float)(velNED.y); // velocity East (m/s) float velD = (float)(velNED.z); // velocity Down (m/s) float posN = (float)(posNED.x); // metres North float posE = (float)(posNED.y); // metres East float posD = (float)(posNED.z); // metres Down float gyrX = (float)(6000*degrees(gyroBias.x)); // centi-deg/min float gyrY = (float)(6000*degrees(gyroBias.y)); // centi-deg/min float gyrZ = (float)(6000*degrees(gyroBias.z)); // centi-deg/min // print EKF1 data packet fprintf(ekf1f, "%.3f %u %d %d %u %.2f %.2f %.2f %.2f %.2f %.2f %.0f %.0f %.0f\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), roll, pitch, yaw, velN, velE, velD, posN, posE, posD, gyrX, gyrY, gyrZ); // define messages for EKF2 data packet int8_t accWeight = (int8_t)(100*accelWeighting); int8_t acc1 = (int8_t)(100*accelZBias1); int8_t acc2 = (int8_t)(100*accelZBias2); int16_t windN = (int16_t)(100*windVel.x); int16_t windE = (int16_t)(100*windVel.y); int16_t magN = (int16_t)(magNED.x); int16_t magE = (int16_t)(magNED.y); int16_t magD = (int16_t)(magNED.z); int16_t magX = (int16_t)(magXYZ.x); int16_t magY = (int16_t)(magXYZ.y); int16_t magZ = (int16_t)(magXYZ.z); // print EKF2 data packet fprintf(ekf2f, "%.3f %d %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), accWeight, acc1, acc2, windN, windE, magN, magE, magD, magX, magY, magZ); // define messages for EKF3 data packet int16_t innovVN = (int16_t)(100*velInnov.x); int16_t innovVE = (int16_t)(100*velInnov.y); int16_t innovVD = (int16_t)(100*velInnov.z); int16_t innovPN = (int16_t)(100*posInnov.x); int16_t innovPE = (int16_t)(100*posInnov.y); int16_t innovPD = (int16_t)(100*posInnov.z); int16_t innovMX = (int16_t)(magInnov.x); int16_t innovMY = (int16_t)(magInnov.y); int16_t innovMZ = (int16_t)(magInnov.z); int16_t innovVT = (int16_t)(100*tasInnov); // print EKF3 data packet fprintf(ekf3f, "%.3f %d %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, AP_HAL::millis(), innovVN, innovVE, innovVD, innovPN, innovPE, innovPD, innovMX, innovMY, innovMZ, innovVT); // define messages for EKF4 data packet int16_t sqrtvarV = (int16_t)(constrain_float(100*velVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarP = (int16_t)(constrain_float(100*posVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarH = (int16_t)(constrain_float(100*hgtVar,INT16_MIN,INT16_MAX)); int16_t sqrtvarMX = (int16_t)(constrain_float(100*magVar.x,INT16_MIN,INT16_MAX)); int16_t sqrtvarMY = (int16_t)(constrain_float(100*magVar.y,INT16_MIN,INT16_MAX)); int16_t sqrtvarMZ = (int16_t)(constrain_float(100*magVar.z,INT16_MIN,INT16_MAX)); int16_t sqrtvarVT = (int16_t)(constrain_float(100*tasVar,INT16_MIN,INT16_MAX)); int16_t offsetNorth = (int8_t)(constrain_float(offset.x,INT16_MIN,INT16_MAX)); int16_t offsetEast = (int8_t)(constrain_float(offset.y,INT16_MIN,INT16_MAX)); // print EKF4 data packet fprintf(ekf4f, "%.3f %u %d %d %d %d %d %d %d %d %d %d\n", AP_HAL::millis() * 0.001f, (unsigned)AP_HAL::millis(), (int)sqrtvarV, (int)sqrtvarP, (int)sqrtvarH, (int)sqrtvarMX, (int)sqrtvarMY, (int)sqrtvarMZ, (int)sqrtvarVT, (int)offsetNorth, (int)offsetEast, (int)faultStatus); } } flush_dataflash(); if (check_solution) { report_checks(); } exit(0); }