void Analyzer_Position_Estimate_Divergence::evaluate()
{
const std::map<const std::string, AnalyzerVehicle::PositionEstimate*> &estimates =
_vehicle->position_estimates();
AnalyzerVehicle::Position pos = _vehicle->pos();
// ::fprintf(stderr, "Position: %.20f/%.20f\n", pos.lat(), pos.lon());
if (pos.lat_modtime() == 0) {
// No craft position yet
return;
}
for (std::map<const std::string, AnalyzerVehicle::PositionEstimate*>::const_iterator it = estimates.begin();
it != estimates.end();
++it) {
AnalyzerVehicle::PositionEstimate *est = (*it).second;
AnalyzerVehicle::Position estimate = est->position();
evaluate_estimate((*it).first, pos, estimate);
}
if (!prevpos.is_zero_zero() &&
!pos.is_zero_zero()) {
_total_distance_travelled += prevpos.horizontal_distance_to(pos);
const double dfo = _vehicle->distance_from_origin();
if (!is_equal(dfo, -1)) {
if (dfo > _maximum_distance_from_origin) {
_maximum_distance_from_origin = dfo;
}
}
}
prevpos = pos;
}
void Analyzer_Attitude_Estimate_Divergence::evaluate()
{
const std::map<const std::string, AnalyzerVehicle::AttitudeEstimate*> &estimates =
_vehicle->attitude_estimates();
AnalyzerVehicle::Attitude att = _vehicle->att();
// ::fprintf(stderr, "Attitude: %.20f/%.20f\n", att.lat(), att.lon());
if (att.roll_modtime() == 0) {
// No craft attitude yet
return;
}
for (std::map<const std::string, AnalyzerVehicle::AttitudeEstimate*>::const_iterator it = estimates.begin();
it != estimates.end();
++it) {
AnalyzerVehicle::AttitudeEstimate *est = (*it).second;
AnalyzerVehicle::Attitude estimate = est->attitude();
evaluate_estimate((*it).first, att, estimate);
}
}
void Analyzer_Velocity_Estimate_Divergence::evaluate()
{
const std::map<const std::string, AnalyzerVehicle::VelocityEstimate*> &estimates =
_vehicle->velocity_estimates();
AnalyzerVehicle::Velocity &vel = _vehicle->vel();
// ::fprintf(stderr, "Velocity: %.20f", vel);
if (vel.velocity_modtime() == 0) {
// No craft velocity yet
return;
}
for (std::map<const std::string, AnalyzerVehicle::VelocityEstimate*>::const_iterator it = estimates.begin();
it != estimates.end();
++it) {
AnalyzerVehicle::VelocityEstimate *est = (*it).second;
AnalyzerVehicle::Velocity &estimate = est->velocity();
evaluate_estimate((*it).first, vel, estimate);
}
if (vel.size() > _maximum_velocity) {
_maximum_velocity = vel.size();
}
}
