Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-22 08:38:36 -04:00
Code Issues Packages Projects Releases Wiki Activity

Replay: remove infinite loop from loop() method

Browse Source 
Remove the while() loop inside Replay::loop() so the new signal handlers
correctly notify the mainloop to exit.

This makes SIGTERM/SIGINT work again.
This commit is contained in:
Lucas De Marchi 2016-11-02 10:29:58 -02:00
parent 2b5f9fdd6b
commit 3620529c22
2 changed files with 244 additions and 240 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

482
Tools/Replay/Replay.cpp
View File

@ -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)
{

2
Tools/Replay/Replay.h
View File

@ -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 {