spatem_ts_utils.h

Go to the documentation of this file.

// SPDX-License-Identifier: MIT
// Copyright Institute for Automotive Engineering (ika), RWTH Aachen University

 
#include <ctime>
 
#pragma once
 
namespace etsi_its_spatem_ts_msgs {
 
namespace access {
 
static constexpr std::array<float, 4> COLOR_GREY{0.5, 0.5, 0.5, 1.0};
static constexpr std::array<float, 4> COLOR_GREEN{0.18, 0.79, 0.21, 1.0};
static constexpr std::array<float, 4> COLOR_ORANGE{0.9, 0.7, 0.09, 1.0};
static constexpr std::array<float, 4> COLOR_RED{0.8, 0.2, 0.2, 1.0};
 
static constexpr int HOUR_IN_SECONDS = 3600;
static constexpr int64_t FACTOR_SECONDS_TO_NANOSECONDS = 1000000000LL;
 
// ETSI Timemark value equals 0.1 seconds and 10e8 nanoseconds
static constexpr int64_t FACTOR_ETSI_TIMEMARK_TO_NANOSECONDS = 100000000LL;
static constexpr float FACTOR_ETSI_TIMEMARK_TO_SECONDS = 0.1f;
 
enum time_mark_value_interpretation { normal, undefined, over_an_hour, leap_second };

inline uint64_t getUnixSecondsOfYear(const uint64_t unixSecond) {
  // Get current time as a time_point
  time_t ts = static_cast<time_t>(unixSecond);  // Convert uint64_t to time_t
 
  struct tm* timeinfo;
  timeinfo = gmtime(&ts);
 
  // Set the timeinfo to the beginning of the year
  timeinfo->tm_sec = 0;
  timeinfo->tm_min = 0;
  timeinfo->tm_hour = 0;
  timeinfo->tm_mday = 1;
  timeinfo->tm_mon = 0;
 
  return timegm(timeinfo);  // Convert struct tm back to Unix timestamp
}

inline uint64_t getUnixNanosecondsFromMinuteOfTheYear(const MinuteOfTheYear& moy,
                                                      const uint64_t unix_nanoseconds_estimate) {
  return ((uint64_t)(moy.value * 60) + getUnixSecondsOfYear(unix_nanoseconds_estimate * 1e-9)) * 1e9;
}

inline float interpretTimeIntervalConfidenceAsFloat(const uint16_t encoded_probability) {
  float probability = 0;
 
  switch (encoded_probability) {
    case 0:
      probability = 0.21;
      break;
    case 1:
      probability = 0.36;
      break;
    case 2:
      probability = 0.47;
      break;
    case 3:
      probability = 0.56;
      break;
    case 4:
      probability = 0.62;
      break;
    case 5:
      probability = 0.68;
      break;
    case 6:
      probability = 0.73;
      break;
    case 7:
      probability = 0.77;
      break;
    case 8:
      probability = 0.81;
      break;
    case 9:
      probability = 0.85;
      break;
    case 10:
      probability = 0.88;
      break;
    case 11:
      probability = 0.91;
      break;
    case 12:
      probability = 0.94;
      break;
    case 13:
      probability = 0.96;
      break;
    case 14:
      probability = 0.98;
      break;
    case 15:
      probability = 1.0;
      break;
  }
 
  return probability;
}

inline std::array<float, 4> interpretMovementPhaseStateAsColor(const uint8_t value) {
  std::array<float, 4> color;
 
  switch (value) {
    case MovementPhaseState::UNAVAILABLE:
      color = COLOR_GREY;
      break;
 
    case MovementPhaseState::DARK:
      color = COLOR_GREY;
      break;
    case MovementPhaseState::STOP_THEN_PROCEED:
      color = COLOR_RED;
      break;
    case MovementPhaseState::STOP_AND_REMAIN:
      color = COLOR_RED;
      break;
    case MovementPhaseState::PRE_MOVEMENT:
      color = COLOR_ORANGE;
      break;
    case MovementPhaseState::PERMISSIVE_MOVEMENT_ALLOWED:
      color = COLOR_GREEN;
      break;
    case MovementPhaseState::PROTECTED_MOVEMENT_ALLOWED:
      color = COLOR_GREEN;
      break;
    case MovementPhaseState::PERMISSIVE_CLEARANCE:
      color = COLOR_ORANGE;
      break;
    case MovementPhaseState::PROTECTED_CLEARANCE:
      color = COLOR_ORANGE;
      break;
    case MovementPhaseState::CAUTION_CONFLICTING_TRAFFIC:
      color = COLOR_ORANGE;
      break;
    default:
      color = COLOR_GREY;
      break;
  }
 
  return color;
}

inline time_mark_value_interpretation interpretTimeMarkValueType(const uint16_t time) {
  time_mark_value_interpretation type;
 
  if (time == 36001) {
    // value is undefined or unknown
    type = time_mark_value_interpretation::undefined;
  } else if (time == 36000) {
    // used to indicate time >3600 seconds
    type = time_mark_value_interpretation::over_an_hour;
  } else if (time >= 35991 && time <= 35999) {
    // leap second
    type = time_mark_value_interpretation::leap_second;
  } else {  // time >= 0 && time <= 36000
    type = time_mark_value_interpretation::normal;
  }
 
  return type;
}

inline int64_t interpretTimeMarkDeltaTimeAsNanoSeconds(const uint16_t time, const uint64_t nanosec) {
  // calculate elapsed nanoseconds since the start of the last full hour
  int64_t timestamp_hour_nanosec = ((nanosec) % (HOUR_IN_SECONDS * FACTOR_SECONDS_TO_NANOSECONDS));
  // convert TimeMark value to nanoseconds
  int64_t timemark_in_nanosec =
      static_cast<int64_t>(time) * FACTOR_ETSI_TIMEMARK_TO_SECONDS * FACTOR_SECONDS_TO_NANOSECONDS;
  // calculate relative time until the next change occurs
  int64_t rel_time_until_change = timemark_in_nanosec - timestamp_hour_nanosec;
 
  // adjust relative time if a jump to the next hour occurs (relative time should be inside the interval [0, 3600e9) )
  if (rel_time_until_change < 0) {
    rel_time_until_change += HOUR_IN_SECONDS * FACTOR_SECONDS_TO_NANOSECONDS;
  }
 
  return rel_time_until_change;
}

inline float interpretTimeMarkDeltaTimeValueAsSeconds(const uint16_t time, const int32_t seconds,
                                                      const uint32_t nanosec) {
  uint64_t timestamp_in_nanoseconds = seconds * FACTOR_SECONDS_TO_NANOSECONDS + nanosec;
  return interpretTimeMarkDeltaTimeAsNanoSeconds(time, timestamp_in_nanoseconds) /
         static_cast<float>(FACTOR_SECONDS_TO_NANOSECONDS);
}

inline std::string parseTimeMarkValueToString(const uint16_t time, const int32_t seconds, const uint32_t nanosec) {
  time_mark_value_interpretation time_type = interpretTimeMarkValueType(time);
 
  std::string text_content;
 
  switch (time_type) {
    case time_mark_value_interpretation::undefined:
      text_content = "undefined";
      break;
    case time_mark_value_interpretation::over_an_hour:
      text_content = ">36000s";
      break;
    case time_mark_value_interpretation::leap_second:
      text_content = "leap second";
      break;
    case time_mark_value_interpretation::normal:
      float rel_time_until_change = interpretTimeMarkDeltaTimeValueAsSeconds(time, seconds, nanosec);
 
      // set displayed precision to 0.1
      std::stringstream ss;
      ss << std::fixed << std::setprecision(1) << rel_time_until_change << "s";
      text_content = ss.str();
      break;
  }
 
  return text_content;
}
 
}  // namespace access
 
}  // namespace etsi_its_spatem_ts_msgs