Program Listing for File pdu_definitions.hpp

↰ Return to documentation for file (/tmp/ws/src/off_highway_sensor_drivers/off_highway_premium_radar/include/off_highway_premium_radar/pdu_definitions.hpp)

// Copyright 2023 Robert Bosch GmbH and its subsidiaries
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <array>
#include <cassert>
#include <cmath>
#include <limits>
#include <vector>

#include "off_highway_premium_radar/range.hpp"

namespace off_highway_premium_radar
{

static constexpr uint8_t kPduHeaderLength{8U};
static constexpr uint8_t kPduIdOffset{0U};
static constexpr uint8_t kPduPayloadLengthOffset{4U};

struct E2E_Header
{
  uint16_t E2E_length;
  uint16_t E2E_Counter;
  uint32_t E2E_DataId;
  uint32_t E2E_Crc;
} __attribute__((packed));

struct LocData_Header_i
{
  void betoh();

  uint32_t LocData_LgpVer_i;
  uint8_t LocData_BlockCounter_i;
  uint32_t LocData_TimeSts_i;
  uint32_t LocData_TimeStns_i;
  uint8_t LocData_OpMode;
  uint8_t LocData_DataMeas;
  uint16_t LocData_NumLoc;
  uint8_t LocData_MaxLocPerPdu;
  std::array<uint8_t, 8> LocData_Reserved_i;
} __attribute__((packed));

struct LocData_Packet_i_j
{
  static constexpr Range<float> r_LocData_RadDist_i_j{0.0F, 302.0F, NAN};
  static constexpr Range<float> r_LocData_RadRelVel_i_j{-110.0F, 55.0F, NAN};
  static constexpr Range<float> r_LocData_AziAng_i_j{-1.5708F, 1.5708F, NAN};
  static constexpr Range<float> r_LocData_EleAng_i_j{-0.785398F, 0.785398F, NAN};
  static constexpr Range<float> r_LocData_Rcs_i_j{-50.0F, 70.0F, NAN};
  static constexpr Range<float> r_LocData_Snr_i_j{0.0F, 31.0F, NAN};
  static constexpr Range<float> r_LocData_RadDistVar_i_j{0.0F, 0.05F, NAN};
  static constexpr Range<float> r_LocData_RadRelVelVar_i_j{0.0F, 0.1F, NAN};
  static constexpr Range<float> r_LocData_VarAzi_i_j{0.0F, 0.0174533F, NAN};
  static constexpr Range<float> r_LocData_VarEle_i_j{0.0F, 0.0174533F, NAN};
  static constexpr Range<float> r_LocData_DistVelCov_i_j{-0.1F, 0.1F, NAN};
  // Probability ranges are wrongly stated in interface documentation V11.0 as [0, 1]...
  static constexpr Range<float> r_LocData_ProVelRes_i_j{0.0F, 255.0F, NAN};
  static constexpr Range<float> r_LocData_ProAziAng_i_j{0.0F, 255.0F, NAN};
  static constexpr Range<float> r_LocData_ProEleAng_i_j{0.0F, 255.0F, NAN};
  static constexpr Range<uint16_t> r_LocData_MeasStat_i_j{0u, 255u, 0xFFFF};
  static constexpr Range<uint16_t> r_LocData_IdAngAmb_i_j{0u, 1023u, 0xFFFF};

  void betoh();

  void check();

  float LocData_RadDist_i_j;
  float LocData_RadRelVel_i_j;
  float LocData_AziAng_i_j;
  float LocData_EleAng_i_j;
  float LocData_Rcs_i_j;
  float LocData_Snr_i_j;
  float LocData_RadDistVar_i_j;
  float LocData_RadRelVelVar_i_j;
  float LocData_VarAzi_i_j;
  float LocData_VarEle_i_j;
  float LocData_DistVelCov_i_j;
  float LocData_ProVelRes_i_j;
  float LocData_ProAziAng_i_j;
  float LocData_ProEleAng_i_j;
  uint16_t LocData_MeasStat_i_j;
  uint16_t LocData_IdAngAmb_i_j;
  std::array<uint8_t, 12> LocData_Reserved_i_j;
} __attribute__((packed));

struct LocationDataPdu
{
  static constexpr uint32_t kPacketIdFirst = 0x13370001UL;
  static constexpr uint32_t kPacketIdLast = 0x13370040UL;
  static constexpr uint32_t kPduPayloadLength{1190u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};
  static constexpr uint8_t kMaxNumLocDataPacketsPerPdu{16U};

  explicit LocationDataPdu(const std::array<uint8_t, kPduSize> & buffer);

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct E2E_Header e2e_header;
  struct LocData_Header_i loc_data_header;
  std::array<LocData_Packet_i_j, kMaxNumLocDataPacketsPerPdu> loc_data_packets;
} __attribute__((packed));

static_assert(
  sizeof(LocationDataPdu) == LocationDataPdu::kPduSize,
  "Wrong LocationDataPdu struct size!");

struct LocationData
{
  E2E_Header e2e_header;
  LocData_Header_i header;
  std::vector<LocData_Packet_i_j> locations;
};

struct MeasurementCycleSyncData
{
  uint8_t FeedBack_SyncType;
  uint32_t FeedBack_SenTimeOff;
} __attribute__((packed));

struct EgoVehicleData
{
  void betoh();

  float FeedBack_RelYawRate;
  float FeedBack_VehSpd;
  float FeedBack_VehSpdStdDev;
  float FeedBack_LogAcc;
} __attribute__((packed));

struct SensorFeedback
{
  static constexpr uint32_t kPduId{0x133ADDCF};
  static constexpr uint32_t kPduPayloadLength{100u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  explicit SensorFeedback(const std::array<uint8_t, kPduSize> & buffer);

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct E2E_Header e2e_header;
  uint32_t FeedBack_LgpVer;
  uint32_t FeedBack_TimeS;
  uint32_t FeedBack_TimeNs;
  struct MeasurementCycleSyncData measurement_cycle_sync_data;
  uint8_t FeedBack_TimeSynSta;
  struct EgoVehicleData ego_vehicle_data;
  std::array<uint8_t, 54> FeedBack_Unassigned;
} __attribute__((packed));

static_assert(
  sizeof(SensorFeedback) == SensorFeedback::kPduSize,
  "Wrong SensorFeedback struct size!");

struct SenStInfo_SwNu_Int
{
  std::array<uint8_t, 5> CommitId;
} __attribute__((packed));

struct SensorStateData
{
  std::array<uint8_t, 9> SenStInfo_Unassigned1;
  uint8_t SenStInfo_SenSt;
  uint32_t SenStInfo_SwNu_Cust;
  struct SenStInfo_SwNu_Int sen_st_info_sw_nu_int;
} __attribute__((packed));

struct SensorStateInformation
{
  static constexpr uint32_t kPduId{0x1338DDCF};
  static constexpr uint32_t kPduPayloadLength{64u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  explicit SensorStateInformation(const std::array<uint8_t, kPduSize> & buffer);

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct E2E_Header e2e_header;
  uint32_t SenStInfo_LgpVer;
  struct SensorStateData sensor_state_data;
  std::array<uint8_t, 29> SenStInfo_Unassigned;
} __attribute__((packed));

static_assert(
  sizeof(SensorStateInformation) == SensorStateInformation::kPduSize,
  "Wrong SensorStateInformation struct size!");

struct SensorEthernetConfigurationInformation
{
  void betoh();

  uint32_t BroadCast_SenIpAdd;
  uint32_t BroadCast_DestIpAdd;
  uint32_t BroadCast_SenNetmask;
  uint16_t BroadCast_SenVlan;
  uint16_t BroadCast_SouPort;
  uint32_t BroadCast_SouPortUnassigned;
  uint16_t BroadCast_DestPort;
  uint32_t BroadCast_DestPortUnassigned;
} __attribute__((packed));

struct DignosticsEthernetConfigurationInformation
{
  void betoh();

  uint32_t BroadCast_DiagSouIpAdd;
  uint32_t BroadCast_DiagNetmask;
  uint16_t BroadCast_DiagVlan;
  uint16_t BroadCast_DiagPort;
} __attribute__((packed));

struct DoIPInformation
{
  void betoh();

  uint16_t BroadCast_SenDoIPPhyAdd;
  uint16_t BroadCast_SenDoIPFuncAdd;
  uint16_t BroadCast_DoIPTarAdd;
} __attribute__((packed));

struct SensorBroadcastData
{
  void betoh();

  uint32_t BroadCast_SwCust;
  struct SensorEthernetConfigurationInformation sensor_ethernet_configuration_information;
  struct DignosticsEthernetConfigurationInformation dignostics_ethernet_configuration_information;
  uint64_t BroadCast_SenMacAd;
  struct DoIPInformation doip_information;
} __attribute__((packed));

struct SensorBroadcast
{
  static constexpr uint32_t kPduId{0x133CDDCF};
  static constexpr uint32_t kPduPayloadLength{160u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  explicit SensorBroadcast(const std::array<uint8_t, kPduSize> & buffer);

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  uint32_t BroadCast_LgpVer;
  struct SensorBroadcastData sensor_broadcast_data;
  std::array<uint8_t, 100> BroadCast_Unassigned;
} __attribute__((packed));

static_assert(
  sizeof(SensorBroadcast) == SensorBroadcast::kPduSize,
  "Wrong SensorBroadcast struct size!");

struct LocAtr_Header_i
{
  void betoh();

  uint32_t LocAtr_LgpVer;
  uint8_t LocAtr_BlockCounter;
  uint32_t LocAtr_TimeSts;
  uint32_t LocAtr_TimeStns;
  uint8_t LocAtr_OpMode;
  uint8_t LocAtr_DataMeas;
  uint8_t LocAtr_Reserved[8];
} __attribute__((packed));

struct SensorModulationPerformance
{
  static constexpr Range<uint8_t> r_LocAtr_DmpID{0u, 254u, 0xFF};
  static constexpr Range<uint16_t> r_LocAtr_ModID{0u, 1023u, 0xFFFF};
  static constexpr Range<float> r_LocAtr_DistRangScalFact{0.0F, 1.0F, NAN};
  static constexpr Range<float> r_LocAtr_SepRadDist{0.0F, 300.0F, NAN};
  static constexpr Range<float> r_LocAtr_SepRadVelo{0.0F, 10.0F, NAN};
  static constexpr Range<float> r_LocAtr_PrecRadDist{0.0F, 10.0F, NAN};
  static constexpr Range<float> r_LocAtr_PrecRadVelo{0.0F, 10.0F, NAN};
  static constexpr Range<float> r_LocAtr_RadDistVeloCovVar{-0.1F, 0.1F, NAN};
  static constexpr Range<float> r_LocAtr_MinRadDist{0.0F, 10.0F, NAN};
  static constexpr Range<float> r_LocAtr_MaxRadDist{0.0F, 300.0F, NAN};
  static constexpr Range<float> r_LocAtr_MinRadVelo{-150.0F, 0.0F, NAN};
  static constexpr Range<float> r_LocAtr_MaxRadVelo{0.0F, 150.0F, NAN};

  void betoh();

  void check();

  uint8_t LocAtr_DmpID;
  uint16_t LocAtr_ModID;
  float LocAtr_DistRangScalFact;
  float LocAtr_SepRadDist;
  float LocAtr_SepRadVelo;
  float LocAtr_PrecRadDist;
  float LocAtr_PrecRadVelo;
  float LocAtr_RadDistVeloCovVar;
  float LocAtr_MinRadDist;
  float LocAtr_MaxRadDist;
  float LocAtr_MinRadVelo;
  float LocAtr_MaxRadVelo;
} __attribute__((packed));

struct Misalignment
{
  static constexpr Range<float> r_LocAtr_ThetaMalAng{-3.141592654F, 3.141592654F, NAN};
  static constexpr Range<float> r_LocAtr_ThetaMalAngVar{std::numeric_limits<float>::min(),
    std::numeric_limits<float>::max(), NAN};
  static constexpr Range<float> r_LocAtr_PhiMalAng{-0.785398163F, 0.785398163F, NAN};
  static constexpr Range<float> r_LocAtr_PhiMalAngVar{std::numeric_limits<float>::min(),
    std::numeric_limits<float>::max(), NAN};
  static constexpr Range<float> r_LocAtr_PhiMalAngEme{-0.785398163F, 0.785398163F, NAN};
  static constexpr Range<float> r_LocAtr_PhiMalAngEmeVar{std::numeric_limits<float>::min(),
    std::numeric_limits<float>::max(), NAN};
  static constexpr Range<uint16_t> r_LocAtr_MalStatus{0u, 255u, 0xFFFF};
  static constexpr Range<uint16_t> r_LocAtr_MalStatusEme{0u, 255u, 0xFFFF};
  static constexpr Range<float> r_LocAtr_PercNegativeTheta{0.0F, 100.0F, NAN};
  static constexpr Range<float> r_LocAtr_MinThetaMalSOs{-3.141592654F, 3.141592654F, NAN};
  static constexpr Range<float> r_LocAtr_MaxThetaMalSOs{-3.141592654F, 3.141592654F, NAN};
  static constexpr Range<float> r_LocAtr_VarThetaMalSOs{std::numeric_limits<float>::min(),
    std::numeric_limits<float>::max(), NAN};
  static constexpr Range<float> r_LocAtr_MeanThetaMalSOs{-3.141592654F, 3.141592654F, NAN};
  static constexpr Range<float> r_LocAtr_MinPhiMalSOs{-0.785398163F, 0.785398163F, NAN};
  static constexpr Range<float> r_LocAtr_MaxPhiMalSOs{-0.785398163F, 0.785398163F, NAN};
  static constexpr Range<float> r_LocAtr_VarPhiMalSOs{std::numeric_limits<float>::min(),
    std::numeric_limits<float>::max(), NAN};
  static constexpr Range<float> r_LocAtr_MeanPhiMalSOs{-0.785398163F, 0.785398163F, NAN};
  static constexpr Range<float> r_LocAtr_SpreadPhiMalSOs{-0.785398163F, 0.785398163F, NAN};
  static constexpr Range<uint16_t> r_LocAtr_NumSOs{0u, 1023u, 0xFFFF};
  static constexpr Range<uint16_t> r_LocAtr_NumEmeLocs{0u, 1023u, 0xFFFF};

  void betoh();

  void check();

  float LocAtr_ThetaMalAng;
  float LocAtr_ThetaMalAngVar;
  float LocAtr_PhiMalAng;
  float LocAtr_PhiMalAngVar;
  float LocAtr_PhiMalAngEme;
  float LocAtr_PhiMalAngEmeVar;
  uint16_t LocAtr_MalStatus;
  uint16_t LocAtr_MalStatusEme;
  float LocAtr_PercNegativeTheta;
  float LocAtr_MinThetaMalSOs;
  float LocAtr_MaxThetaMalSOs;
  float LocAtr_VarThetaMalSOs;
  float LocAtr_MeanThetaMalSOs;
  float LocAtr_MinPhiMalSOs;
  float LocAtr_MaxPhiMalSOs;
  float LocAtr_VarPhiMalSOs;
  float LocAtr_MeanPhiMalSOs;
  float LocAtr_SpreadPhiMalSOs;
  uint16_t LocAtr_NumSOs;
  uint16_t LocAtr_NumEmeLocs;
} __attribute__((packed));

struct InterferenceIndicator
{
  static constexpr Range<float> r_LocAtr_FovRedInt{0.0F, 1.0F, NAN};
  static constexpr Range<uint8_t> r_LocAtr_IntStat{0u, 2u, 0xFF};

  void betoh();

  void check();

  float LocAtr_FovRedInt;
  uint8_t LocAtr_IntStat;
} __attribute__((packed));

struct SensorFieldOfView
{
  static constexpr Range<float> r_LocAtr_FoVRange{0.0F, 511.0F, NAN};
  static constexpr Range<float> r_LocAtr_AziAngArr{-1.5708F, 1.5708F, NAN};
  static constexpr Range<float> r_LocAtr_RangScaEle{0.0F, 1.0F, NAN};
  static constexpr Range<float> r_LocAtr_EleAngArr{-0.785398F, 0.785398F, NAN};

  void betoh();

  void check();

  std::array<float, 25> LocAtr_FoVRange;
  std::array<float, 25> LocAtr_AziAngArr;
  std::array<float, 11> LocAtr_RangScaEle;
  std::array<float, 11> LocAtr_EleAngArr;
} __attribute__((packed));

struct LocAttributes_Packet
{
  void betoh();

  void check();

  struct SensorModulationPerformance sensor_modulation_performance;
  struct Misalignment misalignment;
  struct InterferenceIndicator interference_indicator;
  struct SensorFieldOfView sensor_field_of_view;
  std::array<uint8_t, 50> LocAtr_CoatingIndicationRes;   // 0xFF (all 50 Bytes)
} __attribute__((packed));

struct LocAtr_MountingPosition
{
  void betoh();

  float LocAtr_SenPosX;
  float LocAtr_SenPosY;
  float LocAtr_SenPosZ;
  float LocAtr_SenPosAzi;
  float LocAtr_SenPosEle;
  int8_t LocAtr_SenOrient;
} __attribute__((packed));

struct LocationAttributes
{
  static constexpr uint32_t kPduId{0x133BDDCF};
  static constexpr uint32_t kPduPayloadLength{514u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  explicit LocationAttributes(const std::array<uint8_t, kPduSize> & buffer);

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct E2E_Header e2e_header;
  struct LocAtr_Header_i loc_atr_header;
  struct LocAttributes_Packet loc_atr_packet;
  struct LocAtr_MountingPosition loc_atr_mounting_position;
} __attribute__((packed));

static_assert(
  sizeof(LocationAttributes) == LocationAttributes::kPduSize,
  "Wrong LocationAttributes struct size!");


struct DTCInformationData
{
  std::array<uint32_t, 10> SensorDtc_Dtc;
} __attribute__((packed));

struct SensorDTCInformation
{
  static constexpr uint32_t kPduId{0x133DDDCF};
  static constexpr uint32_t kPduPayloadLength{100u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  explicit SensorDTCInformation(const std::array<uint8_t, kPduSize> & buffer);

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct E2E_Header e2e_header;
  uint32_t SensorDtc_LgpVer;
  DTCInformationData dtc_information_data;
  std::array<uint8_t, 44> BroadCast_Unassigned;
} __attribute__((packed));

static_assert(
  sizeof(SensorDTCInformation) == SensorDTCInformation::kPduSize,
  "Wrong SensorDTCInformation struct size!");


struct VehicleData
{
  float EgoData_RelYawRate;
  float EgoData_VehSpd;
  float EgoData_VehSpdStdDev;
  float EgoData_LogAcc;
} __attribute__((packed));

struct EgoVehicleInput
{
  static constexpr uint32_t kPduId{0x13370050};
  static constexpr uint32_t kPduPayloadLength{16u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  std::vector<uint8_t> serialize();

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct VehicleData vehicle_data;
} __attribute__((packed));

static_assert(
  sizeof(EgoVehicleInput) == EgoVehicleInput::kPduSize,
  "Wrong EgoVehicleInput struct size!");


struct MCSData
{
  uint8_t MCS_SyncType;
  uint32_t MCS_SenTimeOff;
} __attribute__((packed));

struct MeasurementCycleSynchronisation
{
  std::vector<uint8_t> serialize();

  static constexpr uint32_t kPduId{0x13370051};
  static constexpr uint32_t kPduPayloadLength{5u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct MCSData mcs_data;
} __attribute__((packed));

static_assert(
  sizeof(MeasurementCycleSynchronisation) == MeasurementCycleSynchronisation::kPduSize,
  "Wrong MeasurementCycleSynchronisation(MCS) struct size!");


struct SensorModeData
{
  uint8_t SenModReq_RadMod;
  std::array<uint8_t, 63> SenModReq_Unassigned;
} __attribute__((packed));

struct SensorModeRequest
{
  std::vector<uint8_t> serialize();

  static constexpr uint32_t kPduId{0x13370052};
  static constexpr uint32_t kPduPayloadLength{64u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct SensorModeData sensor_mode_data;
} __attribute__((packed));

static_assert(
  sizeof(SensorModeRequest) == SensorModeRequest::kPduSize,
  "Wrong SensorModeRequest struct size!");

// +-------------------------+------------+--------------------+-------------------+-------------+
// | LocAtr_DmpID            | 01 (DMP01) |     02 (DMP02)     | 04 (DMP04)        | 00 (DMP00)  |
// | (Measurement Program)   |            |                    |                   |             |
// |-------------------------+------------+--------------------+-------------------+-------------|
// | Detection range         |    150     |         200        |     302           |     na      |
// | (d_max in m)            |            |                    |                   |             |
// |-------------------------+------------+--------------------+-------------------+-------------|
// | Velocity of the vehicle | <=Velocity | >Velocity range 1  | >Velocity range 2 |     na      |
// |                         |   range 1  | <=Velocity range 2 | < = 360KMPH       |             |
// |-------------------------+------------+--------------------+-------------------+-------------|
// | FoV                     | ±60°/±15°  |       ±60°/±15°    |     ±60°/±15°     | ±60°/±15°   |
// |-------------------------+------------+--------------------+-------------------+-------------|
// | LocAtr_ModID            | 448U,449U, |       452U,453U,   |   460U,461U,      |     na      |
// | (active modulation ID)  | 450U,451U  |       454U,455U    |   462U,463U       |             |
// +-------------------------+------------+--------------------+-------------------+-------------+
// Velocity of the vehicle
//   Velocity range 1 : 65KMPH
//   Velocity range 2 : 115KMPH
// The Velocity ranges can be changed using the DID 0x60C using DMP05
// (Refer Bosch_Variant_Handling.pdf chapter 3.6. Measurement program)
// When sensor is using DMP00, the signals LocAtr_DmpID and LocAtr_ModID are updated based on
// velocity of the vehicle
// For velocity of the vehicle >360KMPH the sensor Modulation stops

struct MeasurementProgramData
{
  uint16_t MeasPgm_ID;
  std::array<uint8_t, 10> MeasPgm_Unassigned;
} __attribute__((packed));

struct MeasurementProgram
{
  std::vector<uint8_t> serialize();

  static constexpr uint32_t kPduId{0x13370053};
  static constexpr uint32_t kPduPayloadLength{12u};
  static constexpr uint32_t kPduSize{kPduPayloadLength + kPduHeaderLength};

  uint32_t pdu_id;
  uint32_t pdu_payload_length;
  struct MeasurementProgramData measurement_program_data;
} __attribute__((packed));

static_assert(
  sizeof(MeasurementProgram) == MeasurementProgram::kPduSize,
  "Wrong MeasurementProgram struct size!");

}  // namespace off_highway_premium_radar