cam_setters.h

Go to the documentation of this file.

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

 
#pragma once
 
namespace etsi_its_cam_msgs::access {
 
#include <etsi_its_msgs_utils/impl/cdd/cdd_v1-3-1_setters.h>

 */
inline void setItsPduHeader(CAM &cam, const uint32_t station_id, const uint8_t protocol_version = 0) {
  setItsPduHeader(cam.header, ItsPduHeader::MESSAGE_ID_CAM, station_id, protocol_version);
}

 *
 * @param accel object to set
 * @param value LongitudinalAccelerationValue in m/s^2 as decimal number (braking is negative)
 */
inline void setLongitudinalAccelerationValue(LongitudinalAccelerationValue& accel, const double value) {
  auto accel_val = std::round(value * 1e1);
  if (accel_val >= LongitudinalAccelerationValue::MIN && accel_val <= LongitudinalAccelerationValue::MAX) {
    accel.value = static_cast<int16_t>(accel_val);
  } else if (accel_val < LongitudinalAccelerationValue::MIN) {
    accel.value = LongitudinalAccelerationValue::MIN;
  } else if (accel_val > LongitudinalAccelerationValue::MAX) {
    accel.value = LongitudinalAccelerationValue::MAX - 1;
  }
}


 */
inline void setLongitudinalAcceleration(LongitudinalAcceleration& accel, const double value, const double confidence = std::numeric_limits<double>::infinity()) {
  setAccelerationConfidence(accel.longitudinal_acceleration_confidence, confidence);
  setLongitudinalAccelerationValue(accel.longitudinal_acceleration_value, value);
}

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

 *
 * @param accel object to set
 * @param value LaterallAccelerationValue in m/s^2 as decimal number (left is positive)
 * @param confidence standard deviation of the lateral acceleration in m/s^2 as decimal number
 *                   Default is infinity, mapping to AccelerationConfidence::UNAVAILABLE
 */
inline void setLateralAcceleration(LateralAcceleration& accel, const double value, const double confidence = std::numeric_limits<double>::infinity()) {
  setAccelerationConfidence(accel.lateral_acceleration_confidence, confidence);
  setLateralAccelerationValue(accel.lateral_acceleration_value, value);
}
 
#include <etsi_its_msgs_utils/impl/cam/cam_setters_common.h>

inline void setRefPosConfidence(CAM& cam, const std::array<double, 4>& covariance_matrix, const double object_heading) {
  // First ensure, that the object has the correct heading by setting its value
  double orientation = object_heading * 180 / M_PI; // Convert to degrees
  // Normalize to [0, 360)
  orientation = std::fmod(orientation + 360, 360);
  while (orientation < 0) {
    orientation += 360;
  }
  while (orientation >= 360) {
    orientation -= 360;
  }
  setHeading(cam, orientation);
  setPosConfidenceEllipse(cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse,
                          covariance_matrix, object_heading);
}

/**
 * @brief Set the confidence of the reference position
 *
 * @param cam CAM-Message to set the confidence
 * @param covariance_matrix The four values of the covariance matrix in the order: cov_xx, cov_xy, cov_yx, cov_yy
 *                          The matrix has to be SPD, otherwise a std::invalid_argument exception is thrown.
 *                          Its coordinate system is WGS84 (x = North, y = East)
 */
inline void setWGSRefPosConfidence(CAM& cam, const std::array<double, 4>& covariance_matrix) {
  setWGSPosConfidenceEllipse(cam.cam.cam_parameters.basic_container.reference_position.position_confidence_ellipse,
                             covariance_matrix);
}
 
}  // namespace etsi_its_cam_msgs::access