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; }