diff --git a/Tools/Replay/Replay.cpp b/Tools/Replay/Replay.cpp index 778e53bbd3..01dfba63cf 100644 --- a/Tools/Replay/Replay.cpp +++ b/Tools/Replay/Replay.cpp @@ -773,255 +773,257 @@ void Replay::log_check_solution(void) check_result.max_pos_error = MAX(check_result.max_pos_error, pos_error); } - -void Replay::loop() +void Replay::flush_and_exit() { - uint64_t last_timestamp = 0; - 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; - } - - if (last_timestamp != 0) { - uint64_t gap = AP_HAL::micros64() - last_timestamp; - if (gap > 40000) { - ::printf("Gap in log at timestamp=%lu of length %luus\n", - last_timestamp, gap); - } - } - last_timestamp = AP_HAL::micros64(); - - read_sensors(type); - - if (streq(type,"ATT")) { - Vector3f ekf_euler; - Vector3f velNED; - Vector2f posNE; - float posD; - Vector3f gyroBias; - float accelWeighting; - float accelZBias1; - float accelZBias2; - Vector3f windVel; - Vector3f magNED; - Vector3f magXYZ; - Vector3f DCM_attitude; - 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.getPosNE(posNE); - _vehicle.EKF.getPosD(posD); - _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); - 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, - posNE.x, - posNE.y, - -posD); - 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, - posNE.x, - posNE.y, - posD, - 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)(posNE.x); // metres North - float posE = (float)(posNE.y); // metres East - 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); } +void Replay::loop() +{ + 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); + flush_and_exit(); + } + + if (last_timestamp != 0) { + uint64_t gap = AP_HAL::micros64() - last_timestamp; + if (gap > 40000) { + ::printf("Gap in log at timestamp=%lu of length %luus\n", + last_timestamp, gap); + } + } + last_timestamp = AP_HAL::micros64(); + + read_sensors(type); + + if (!streq(type,"ATT")) { + return; + } + + Vector3f ekf_euler; + Vector3f velNED; + Vector2f posNE; + float posD; + Vector3f gyroBias; + float accelWeighting; + float accelZBias1; + float accelZBias2; + Vector3f windVel; + Vector3f magNED; + Vector3f magXYZ; + Vector3f DCM_attitude; + 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.getPosNE(posNE); + _vehicle.EKF.getPosD(posD); + _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); + 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, + posNE.x, + posNE.y, + -posD); + 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, + posNE.x, + posNE.y, + posD, + 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)(posNE.x); // metres North + float posE = (float)(posNE.y); // metres East + 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); +} + bool Replay::show_error(const char *text, float max_error, float tolerance) { diff --git a/Tools/Replay/Replay.h b/Tools/Replay/Replay.h index bb0a5bf646..73113379fe 100644 --- a/Tools/Replay/Replay.h +++ b/Tools/Replay/Replay.h @@ -142,6 +142,7 @@ private: uint16_t downsample = 0; bool logmatch = false; uint32_t output_counter = 0; + uint64_t last_timestamp = 0; struct { float max_roll_error; @@ -176,6 +177,7 @@ private: bool parse_param_line(char *line, char **vname, float &value); void load_param_file(const char *filename); void set_signal_handlers(void); + void flush_and_exit(); }; enum {