コード例 #1
0
ファイル: Duration.cpp プロジェクト: retlaw84/dune
    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;
    }
コード例 #2
0
ファイル: Duration.cpp プロジェクト: krisklau/dune
    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;
    }