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