void Analyzer_Position_Estimate_Divergence::update_result(std::string name,
double delta)
{
if (delta > _result[name]->max_delta()) {
_result[name]->set_max_delta(delta);
if (delta >= delta_fail()) {
_result[name]->set_delta_threshold(delta_fail());
_result[name]->set_status(analyzer_status_fail);
_result[name]->set_severity_score(20);
} else if (delta >= delta_warn()) {
_result[name]->set_status(analyzer_status_warn);
_result[name]->set_delta_threshold(delta_warn());
_result[name]->set_severity_score(10);
}
}
}
void Analyzer_Position_Estimate_Divergence::evaluate_estimate(
const std::string name,
AnalyzerVehicle::Position position,
AnalyzerVehicle::Position estimate)
{
if (estimate.lat_modtime() == 0) {
// No estimate for this yet
return;
}
double delta = estimate.horizontal_distance_to(position);
bool failing = (delta > delta_warn());
// ::fprintf(stderr, "%s: delta=%f delta_warn=%f: %s\n", name.c_str(), delta, delta_warn(), failing ? "FAILING" : "");
if (_result.count(name) == 0 ||
_result[name] == NULL) {
// no current problem
if (failing) {
open_result(name, delta);
}
} else {
// problem currently underway
if (failing) {
update_result(name, delta);
} else {
close_result(name);
}
}
}
void Analyzer_Attitude_Estimate_Divergence::evaluate_estimate(
std::string name,
AnalyzerVehicle::Attitude attitude,
AnalyzerVehicle::Attitude estimate)
{
if (estimate.roll_modtime() == 0) {
// No estimate for this yet
return;
}
double delta_roll = fabs(angle_delta(estimate.roll(), attitude.roll()));
double delta_pitch = fabs(angle_delta(estimate.pitch(), attitude.pitch()));
// double delta_yaw = estimate.yaw() - attitude.yaw();
double delta = delta_roll > delta_pitch ? delta_roll : delta_pitch;
bool failing = (delta >= delta_warn());
if (_result[name] == NULL) {
if (failing) {
open_result(name, delta);
}
} else {
if (failing) {
update_result(name, delta);
} else {
close_result(name);
}
}
}
void Analyzer_Estimate_Divergence::update_result_set_status(Analyzer_Estimate_Divergence_Result *result)
{
if (fabs(result->max_delta()) >= delta_fail()) {
result->set_status(analyzer_status_fail);
} else if (fabs(result->max_delta()) >= delta_warn()) {
result->set_status(analyzer_status_warn);
}
}
void Analyzer_Estimate_Divergence::update_result(std::string name, double delta)
{
Analyzer_Estimate_Divergence_Result *result = _result[name];
if (fabs(delta) > fabs(result->max_delta())) {
result->set_max_delta(delta);
update_result_set_status(result);
if (result->status() == analyzer_status_fail) {
result->set_delta_threshold(delta_fail());
result->set_severity_score(20);
} else if (result->status() == analyzer_status_warn) {
result->set_delta_threshold(delta_warn());
result->set_severity_score(10);
}
}
}
void Analyzer_Velocity_Estimate_Divergence::evaluate_estimate(
const std::string name,
AnalyzerVehicle::Velocity &velocity,
AnalyzerVehicle::Velocity &estimate)
{
if (estimate.velocity_modtime() == 0) {
// No estimate for this yet
return;
}
// make sure we're comparing apples with apples:
double velocity_size;
if (estimate.is_2d()) {
velocity_size = velocity.size_2d();
} else {
velocity_size = velocity.size();
}
double delta = estimate.size() - velocity_size;
// ::fprintf(stderr, "delta: %f\n", delta);
bool failing = (delta > delta_warn());
// ::fprintf(stderr, "%s: delta=%f delta_warn=%f: %s\n", name.c_str(), delta, delta_warn(), failing ? "FAILING" : "");
if (_result.count(name) == 0 ||
_result[name] == NULL) {
// no current problem
if (failing) {
open_result(name, delta);
}
} else {
// problem currently underway
if (failing) {
update_result(name, delta);
} else {
close_result(name);
}
}
}