float
Duration::parse(const IMC::Rows* maneuver, Position& last_pos, float last_dur,
std::vector<float>& durations, const SpeedConversion& speed_conv)
{
float speed = convertSpeed(maneuver, speed_conv);
if (speed == 0.0)
return -1.0;
Maneuvers::RowsStages rstages = Maneuvers::RowsStages(maneuver, NULL);
Position pos;
pos.z = maneuver->z;
pos.z_units = maneuver->z_units;
rstages.getFirstPoint(&pos.lat, &pos.lon);
float distance = distance3D(pos, last_pos);
durations.push_back(distance / speed + last_dur);
last_pos = pos;
distance += rstages.getDistance(&last_pos.lat, &last_pos.lon);
std::vector<float>::const_iterator itr = rstages.getDistancesBegin();
for (; itr != rstages.getDistancesEnd(); ++itr)
{
// compensate with path controller's eta factor
float travelled = compensate(*itr, speed);
durations.push_back(travelled / speed + durations.back());
}
return distance / speed;
}
bool
Duration::parse(const IMC::Rows* maneuver, Position& last_pos)
{
float speed = convertSpeed(maneuver);
if (speed == 0.0)
return false;
Maneuvers::RowsStages rstages = Maneuvers::RowsStages(maneuver, NULL);
Position pos;
pos.z = maneuver->z;
pos.z_units = maneuver->z_units;
rstages.getFirstPoint(&pos.lat, &pos.lon);
float distance = distance3D(pos, last_pos);
m_accum_dur->addDuration(distance / speed);
last_pos = pos;
distance += rstages.getDistance(&last_pos.lat, &last_pos.lon);
std::vector<float>::const_iterator itr = rstages.getDistancesBegin();
for (; itr != rstages.getDistancesEnd(); ++itr)
{
// compensate with path controller's eta factor
float travelled = compensate(*itr, speed);
m_accum_dur->addDuration(travelled / speed);
}
return true;
}