cdd_v2-1-1_setters.h

Go to the documentation of this file.

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

 
#ifndef ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H
#define ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H
 
#include <etsi_its_msgs_utils/impl/cdd/cdd_setters_common.h>
#include <etsi_its_msgs_utils/impl/checks.h>
#include <GeographicLib/UTMUPS.hpp>
#include <cstring>

inline void setItsPduHeader(ItsPduHeader& header, const uint8_t message_id, const uint32_t station_id,
                            const uint8_t protocol_version = 0) {
  setStationId(header.station_id, station_id);
  throwIfOutOfRange(message_id, MessageId::MIN, MessageId::MAX, "MessageID");
  header.message_id.value = message_id;
  throwIfOutOfRange(protocol_version, OrdinalNumber1B::MIN, OrdinalNumber1B::MAX, "ProtocolVersion");
  header.protocol_version.value = protocol_version;
}

inline void setLongitudinalAccelerationValue(AccelerationValue& accel, const double value) {
  int64_t accel_val = (int64_t)std::round(value * 1e1);
  if (accel_val >= AccelerationValue::MIN && accel_val <= AccelerationValue::MAX) {
    accel.value = accel_val;
  } else if (accel_val < AccelerationValue::MIN) {
    accel.value = AccelerationValue::MIN;
  } else if (accel_val > AccelerationValue::MAX) {
    accel.value = AccelerationValue::MAX - 1;
  }
}

inline void setLongitudinalAcceleration(AccelerationComponent& accel, const double value, const double confidence) {
  setAccelerationConfidence(accel.confidence, confidence);
  setLongitudinalAccelerationValue(accel.value, value);
}

inline void setLateralAccelerationValue(AccelerationValue& accel, const double value) {
  int64_t accel_val = (int64_t)std::round(value * 1e1);
  if (accel_val >= AccelerationValue::MIN && accel_val <= AccelerationValue::MAX) {
    accel.value = accel_val;
  } else if (accel_val < AccelerationValue::MIN) {
    accel.value = AccelerationValue::MIN;
  } else if (accel_val > AccelerationValue::MAX) {
    accel.value = AccelerationValue::MAX - 1;
  }
}

inline void setLateralAcceleration(AccelerationComponent& accel, const double value, const double confidence) {
  setAccelerationConfidence(accel.confidence, confidence);
  setLateralAccelerationValue(accel.value, value);
}

template <typename PositionConfidenceEllipse, typename Wgs84AngleValue = decltype(PositionConfidenceEllipse::semi_major_axis_orientation)>
inline void setPositionConfidenceEllipse(PositionConfidenceEllipse& position_confidence_ellipse, const double semi_major_axis,
  const double semi_minor_axis, const double orientation) {
  setSemiAxis(position_confidence_ellipse.semi_major_axis_length, semi_major_axis);
  setSemiAxis(position_confidence_ellipse.semi_minor_axis_length, semi_minor_axis);
  setHeadingValue(position_confidence_ellipse.semi_major_axis_orientation, orientation);
}

template <typename PositionConfidenceEllipse>
inline void setPositionConfidenceEllipse(PositionConfidenceEllipse& position_confidence_ellipse, const std::array<double, 4>& covariance_matrix, const double object_heading){
  auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromCovMatrix(covariance_matrix, object_heading);
  setPositionConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
}

template <typename PositionConfidenceEllipse>
inline void setWGSPositionConfidenceEllipse(PositionConfidenceEllipse& position_confidence_ellipse, const std::array<double, 4>& covariance_matrix){
  auto [semi_major_axis, semi_minor_axis, orientation] = confidenceEllipseFromWGSCovMatrix(covariance_matrix);
  setPositionConfidenceEllipse(position_confidence_ellipse, semi_major_axis, semi_minor_axis, orientation);
}
 
#endif  // ETSI_ITS_MSGS_UTILS_IMPL_CDD_CDD_V2_1_1_SETTERS_H