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; }
float Duration::parse(const IMC::Elevator* 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; Position pos; extractPosition(maneuver, pos); float goto_dist = distance3D(pos, last_pos); float amplitude = std::fabs(last_pos.z - maneuver->end_z); float real_dist = amplitude / std::sin(c_rated_pitch); float travelled = goto_dist + real_dist; // compensate with path controller's eta factor travelled = compensate(travelled, speed); durations.push_back(travelled / speed + last_dur); return durations.back(); }
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; }
bool TimeProfile::parse(const IMC::Elevator* maneuver, Position& last_pos) { float speed = convertSpeed(maneuver); if (speed == 0.0) return false; Position pos; extractPosition(maneuver, pos); float goto_dist = distance3D(pos, last_pos); float amplitude = std::fabs(last_pos.z - maneuver->end_z); float real_dist = amplitude / std::sin(c_rated_pitch); float travelled = goto_dist + real_dist; // compensate with path controller's eta factor travelled = compensate(travelled, speed); float duration = travelled / speed; // Update speed profile m_speed_vec->push_back(SpeedProfile(maneuver, duration)); m_accum_dur->addDuration(duration); return true; }
bool TimeProfile::parse(const IMC::PopUp* maneuver, Position& last_pos) { float speed = convertSpeed(maneuver); if (speed == 0.0) return false; // Travel time float travel_time; if ((maneuver->flags & IMC::PopUp::FLG_CURR_POS) != 0) { Position pos; extractPosition(maneuver, pos); float travelled = distance3D(pos, last_pos); // compensate with path controller's eta factor travelled = compensate(travelled, speed); travel_time = travelled / speed; last_pos = pos; } else { travel_time = 0; } // Update speed profile m_speed_vec->push_back(SpeedProfile(maneuver, travel_time)); // Rising time and descending time float rising_time; float descending_time; if (maneuver->z_units == IMC::Z_DEPTH) { rising_time = (std::fabs(last_pos.z) / std::sin(c_rated_pitch)) / speed; descending_time = (std::fabs(maneuver->z) / std::sin(c_rated_pitch)) / speed; } else // altitude, assume zero { rising_time = 0.0; descending_time = 0.0; } // surface time float surface_time = c_fix_time; // Update speed profile m_speed_vec->push_back(SpeedProfile(maneuver, rising_time)); m_speed_vec->push_back(SpeedProfile(0.0, 0, surface_time)); m_speed_vec->push_back(SpeedProfile(maneuver, descending_time)); m_accum_dur->addDuration(travel_time + rising_time + surface_time + descending_time); return true; }
bool TimeProfile::parse(const IMC::FollowPath* maneuver, Position& last_pos) { float speed = convertSpeed(maneuver); if (speed == 0.0) return false; Position pos; pos.z = maneuver->z; pos.z_units = maneuver->z_units; IMC::MessageList<IMC::PathPoint>::const_iterator itr = maneuver->points.begin(); if (!maneuver->points.size()) { // Update speed profile m_speed_vec->push_back(SpeedProfile(0.0, 0)); // Update duration m_accum_dur->addDuration(0.0); } else { // Iterate point list for (; itr != maneuver->points.end(); itr++) { if ((*itr) == NULL) continue; pos.lat = maneuver->lat; pos.lon = maneuver->lon; Coordinates::WGS84::displace((*itr)->x, (*itr)->y, &pos.lat, &pos.lon); float travelled = distance3D(pos, last_pos); last_pos = pos; float duration = compensate(travelled, speed) / speed; // Update speed profile m_speed_vec->push_back(SpeedProfile(maneuver, duration)); // compensate with path controller's eta factor m_accum_dur->addDuration(duration); } } return true; }
float Duration::parse(const IMC::PopUp* 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; // Travel time float travel_time; if ((maneuver->flags & IMC::PopUp::FLG_CURR_POS) != 0) { Position pos; extractPosition(maneuver, pos); float travelled = distance3D(pos, last_pos); // compensate with path controller's eta factor travelled = compensate(travelled, speed); travel_time = travelled / speed; last_pos = pos; } else { travel_time = 0; } // Rising time float rising_time; if (maneuver->z_units == IMC::Z_DEPTH) rising_time = std::fabs(last_pos.z) / speed; else // altitude, assume zero rising_time = 0.0; // surface time bool wait = (maneuver->flags & IMC::PopUp::FLG_WAIT_AT_SURFACE) != 0; float surface_time = wait ? maneuver->duration : c_fix_time; durations.push_back(travel_time + rising_time + surface_time + last_dur); return durations.back(); }
float Duration::parse(const IMC::FollowPath* 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; Position pos; pos.z = maneuver->z; pos.z_units = maneuver->z_units; IMC::MessageList<IMC::PathPoint>::const_iterator itr = maneuver->points.begin(); double total_duration = last_dur; if (!maneuver->points.size()) { durations.push_back(0.0); } else { // Iterate point list for (; itr != maneuver->points.end(); itr++) { if ((*itr) == NULL) continue; pos.lat = maneuver->lat; pos.lon = maneuver->lon; Coordinates::WGS84::displace((*itr)->x, (*itr)->y, &pos.lat, &pos.lon); float travelled = distance3D(pos, last_pos); last_pos = pos; // compensate with path controller's eta factor total_duration += compensate(travelled, speed) / speed; durations.push_back(total_duration); } } return durations.back(); }
float Duration::parseSimple(const Type* maneuver, Position& last_pos) { float speed = convertSpeed(maneuver); if (speed == 0.0) return -1.0; Position pos; extractPosition(maneuver, pos); float travelled = distance3D(pos, last_pos); // compensate with path controller's eta factor travelled = compensate(travelled, speed); last_pos = pos; return travelled / speed; }
float Duration::parseSimple(const Type* 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; Position pos; extractPosition(maneuver, pos); float travelled = distance3D(pos, last_pos); // compensate with path controller's eta factor travelled = compensate(travelled, speed); last_pos = pos; durations.push_back(travelled / speed + last_dur); return durations[0]; }
float TimeProfile::parseSimple(const Type* maneuver, Position& last_pos) { float speed = convertSpeed(maneuver); if (speed == 0.0) return -1.0; Position pos; extractPosition(maneuver, pos); float travelled = distance3D(pos, last_pos); // compensate with path controller's eta factor travelled = compensate(travelled, speed); last_pos = pos; float duration = travelled / speed; // Update speed profile m_speed_vec->push_back(SpeedProfile(maneuver, duration)); return duration; }