Skip to content

File pandar_128e3x.hpp

File List > decoders > pandar_128e3x.hpp

Go to the documentation of this file

// Copyright 2024 TIER IV, Inc.
//
// 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 "nebula_decoders/nebula_decoders_hesai/decoders/functional_safety.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_packet.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp"

#include <nebula_common/util/bitfield.hpp>
#include <nebula_common/util/crc.hpp>

#include <cstdint>
#include <iostream>
#include <ostream>
#include <vector>

namespace nebula::drivers
{

namespace hesai_packet
{

#pragma pack(push, 1)

struct Tail128E3X
{
  uint8_t reserved1[9];
  uint16_t azimuth_state;
  uint8_t operational_state;
  uint8_t return_mode;
  uint16_t motor_speed;
  DateTime<1900> date_time;
  uint32_t timestamp;
  uint8_t factory_information;

  /* Ignored optional fields */

  uint32_t udp_sequence;

  uint16_t imu_temperature;
  uint16_t imu_acceleration_unit;
  uint16_t imu_angular_velocity_unit;
  uint32_t imu_timestamp;
  uint16_t imu_x_axis_acceleration;
  uint16_t imu_y_axis_acceleration;
  uint16_t imu_z_axis_acceleration;
  uint16_t imu_x_axis_angular_velocity;
  uint16_t imu_y_axis_angular_velocity;
  uint16_t imu_z_axis_angular_velocity;

  uint32_t crc_tail;

  uint8_t get_azimuth_state(unsigned int block_id) const
  {
    return (azimuth_state >> (14 - block_id * 2)) & 0b11;
  }

  [[nodiscard]] bool is_crc_valid() const
  {
    return crc<crc32_mpeg2_t>(&reserved1, &crc_tail) == crc_tail;
  }
};

struct ChannelHealth128E3X
{
  uint16_t fault_info_num;
  uint32_t channel_health_status;
  uint8_t reserved[2];
};

struct FunctionalSafety128E3X
{
  static constexpr uint64_t update_cycle_ns = 5'000'000;

  enum class LidarState : uint8_t {
    INITIALIZATION = 0,
    NORMAL = 1,
    WARNING = 2,
    PRE_PERFORMANCE_DEGRADATION = 3,
    PERFORMANCE_DEGRADATION = 4,
    PRE_SHUTDOWN = 5,
    SHUTDOWN_OR_OUTPUT_UNTRUSTED = 6,
    STANDBY = 7,
  };

  enum class FaultCodeType : uint8_t {
    NONE = 0,
    CURRENT_FAULT = 1,
    PAST_FAULT = 2  
  };

  uint8_t fs_version;

  uint8_t bitfield1;
  BITFIELD_ACCESSOR(LidarState, lidar_state, 5, 7, bitfield1)
  BITFIELD_ACCESSOR(FaultCodeType, fault_code_type, 3, 4, bitfield1)
  BITFIELD_ACCESSOR(uint8_t, rolling_counter, 0, 2, bitfield1)

  uint8_t bitfield2;
  BITFIELD_ACCESSOR(uint8_t, total_fault_code_num, 4, 7, bitfield2)
  BITFIELD_ACCESSOR(uint8_t, fault_code_id, 0, 3, bitfield2)

  uint16_t fault_code;
  ChannelHealth128E3X channel_health;
  uint32_t crc_fs;

  [[nodiscard]] bool is_crc_valid() const
  {
    // FIXME(mojomex): OT128's CRC is broken, at least in B and C samples. Disable for now.
    return true;

    // fs_version is not included in the CRC check
    // return crc<crc32_mpeg2_t>(&bitfield1, &crc_fs) == crc_fs;
  }

  [[nodiscard]] FunctionalSafetySeverity severity() const
  {
    switch (lidar_state()) {
      case LidarState::INITIALIZATION:
      case LidarState::NORMAL:
      case LidarState::WARNING:
        return FunctionalSafetySeverity::OK;
      case LidarState::PRE_PERFORMANCE_DEGRADATION:
      case LidarState::PERFORMANCE_DEGRADATION:
      case LidarState::PRE_SHUTDOWN:
        return FunctionalSafetySeverity::WARNING;
      case LidarState::SHUTDOWN_OR_OUTPUT_UNTRUSTED:
      case LidarState::STANDBY:
      default:
        return FunctionalSafetySeverity::ERROR;
    }
  }

  friend bool operator==(const FunctionalSafety128E3X & lhs, const FunctionalSafety128E3X & rhs)
  {
    return lhs.lidar_state() == rhs.lidar_state() &&
           lhs.fault_code_type() == rhs.fault_code_type() &&
           lhs.rolling_counter() == rhs.rolling_counter() &&
           lhs.total_fault_code_num() == rhs.total_fault_code_num() &&
           lhs.fault_code == rhs.fault_code;
  }

  friend bool operator!=(const FunctionalSafety128E3X & lhs, const FunctionalSafety128E3X & rhs)
  {
    return !(lhs == rhs);
  }
};

struct Packet128E3X : public PacketBase<2, 128, 2, 100>
{
  using body_t = BodyWithCrc<Block<Unit3B, Packet128E3X::n_channels>, Packet128E3X::n_blocks>;
  Header12B header;
  body_t body;
  FunctionalSafety128E3X fs;
  Tail128E3X tail;

  /* Ignored optional fields */

  // uint8_t cyber_security[32];
};

#pragma pack(pop)

}  // namespace hesai_packet

class Pandar128E3X : public HesaiSensor<hesai_packet::Packet128E3X>
{
private:
  enum OperationalState { HIGH_RESOLUTION = 0, SHUTDOWN = 1, STANDARD = 2, ENERGY_SAVING = 3 };

  static constexpr int hires_as0_far_offset_ns[128] = {
    4436,  -1,    776,   2431,  4436,  -1,    6441,  -1,    -1,    776,   2431,  6441,  -1,
    -1,    -1,    -1,    -1,    6441,  -1,    776,   2431,  -1,    -1,    -1,    4436,  10381,
    14951, 12666, 14951, 19521, 19521, 8096,  12666, 12666, 10381, 24091, 17236, 24091, 14951,
    14951, 19521, 17236, 12666, 21806, 8096,  21806, 10381, 10381, 21806, 8096,  8096,  19521,
    12666, 12666, 24091, 24091, 17236, 21806, 17236, 14951, 10381, 14951, 17236, 17236, 8096,
    19521, 19521, 10381, 24091, 10381, 21806, 12666, 10381, 14951, 21806, 8096,  19521, 17236,
    8096,  19521, 24091, 24091, 24091, 17236, 21806, 8096,  12666, 21806, 14951, 2431,  776,
    4436,  6441,  -1,    -1,    776,   -1,    2431,  2431,  4436,  -1,    -1,    -1,    6441,
    4436,  -1,    -1,    -1,    6441,  -1,    -1,    -1,    776,   4436,  -1,    2431,  -1,
    -1,    776,   -1,    4436,  6441,  -1,    -1,    2431,  776,   6441,  -1};

  static constexpr int hires_as0_near_offset_ns[128] = {
    5201, -1,    1541, -1, -1, -1, -1, -1, -1, -1,   -1, -1, -1, -1,   -1, -1, -1, 7206,  -1,
    -1,   3196,  -1,   -1, -1, -1, -1, -1, -1, -1,   -1, -1, -1, -1,   -1, -1, -1, -1,    -1,
    -1,   27056, -1,   -1, -1, -1, -1, -1, -1, -1,   -1, -1, -1, -1,   -1, -1, -1, -1,    -1,
    -1,   -1,    -1,   -1, -1, -1, -1, -1, -1, -1,   -1, -1, -1, -1,   -1, -1, -1, 23201, -1,
    -1,   -1,    -1,   -1, -1, -1, -1, -1, -1, -1,   -1, -1, -1, 3676, -1, -1, -1, -1,    -1,
    -1,   -1,    -1,   -1, -1, -1, -1, -1, -1, 5681, -1, -1, -1, -1,   -1, -1, -1, -1,    -1,
    -1,   -1,    -1,   -1, -1, -1, -1, -1, -1, -1,   -1, -1, -1, -1};

  static constexpr int hires_as1_far_offset_ns[128] = {
    -1,    776,   -1,    -1,    -1,    2781,  -1,    4786,  6441,  -1,    -1,    -1,    776,
    6441,  2781,  776,   4786,  -1,    4786,  -1,    -1,    2781,  6441,  4786,  -1,    10731,
    15301, 13016, 15301, 19871, 19871, 8446,  13016, 13016, 10731, 24441, 17586, 24441, 15301,
    15301, 19871, 17586, 13016, 22156, 8446,  22156, 10731, 10731, 22156, 8446,  8446,  19871,
    13016, 13016, 24441, 24441, 17586, 22156, 17586, 15301, 10731, 15301, 17586, 17586, 8446,
    19871, 19871, 10731, 24441, 10731, 22156, 13016, 10731, 15301, 22156, 8446,  19871, 17586,
    8446,  19871, 24441, 24441, 24441, 17586, 22156, 8446,  13016, 22156, 15301, -1,    -1,
    -1,    -1,    6441,  2781,  -1,    776,   -1,    -1,    -1,    4786,  776,   2781,  -1,
    -1,    2781,  776,   4786,  -1,    6441,  6441,  4786,  -1,    -1,    4786,  -1,    2781,
    6441,  -1,    776,   -1,    -1,    6441,  2781,  -1,    -1,    -1,    776};

  static constexpr int hires_as1_near_offset_ns[128] = {
    -1, -1, -1, -1, -1, 4026, -1,    -1, 7206,  -1, -1, -1, -1, -1, 3546, -1,   -1, -1, -1,
    -1, -1, -1, -1, -1, -1,   12126, -1, -1,    -1, -1, -1, -1, -1, -1,   -1,   -1, -1, -1,
    -1, -1, -1, -1, -1, -1,   -1,    -1, 27406, -1, -1, -1, -1, -1, -1,   -1,   -1, -1, -1,
    -1, -1, -1, -1, -1, -1,   -1,    -1, -1,    -1, -1, -1, -1, -1, -1,   -1,   -1, -1, -1,
    -1, -1, -1, -1, -1, -1,   -1,    -1, -1,    -1, -1, -1, -1, -1, -1,   -1,   -1, -1, -1,
    -1, -1, -1, -1, -1, -1,   2021,  -1, -1,    -1, -1, -1, -1, -1, -1,   7686, -1, -1, -1,
    -1, -1, -1, -1, -1, 1541, -1,    -1, -1,    -1, -1, -1, -1, -1};

  static constexpr int hires_as2_far_offset_ns[128] = {
    4436,  -1,    776,   2781,  4436,  -1,    6 - 191, -1,    -1,    776,   2781,  6091,  -1,
    -1,    -1,    -1,    -1,    6091,  -1,    776,     2781,  -1,    -1,    -1,    4436,  10381,
    14951, 12666, 14951, 19521, 19521, 8096,  12666,   12666, 10381, 24091, 17236, 24091, 14951,
    14951, 19521, 17236, 12666, 21806, 8096,  21806,   10381, 10381, 21806, 8096,  8096,  19521,
    12666, 12666, 24091, 24091, 17236, 21806, 17236,   14951, 10381, 14951, 17236, 17236, 8096,
    19521, 19521, 10381, 24091, 10381, 21806, 12666,   10381, 14951, 21806, 8096,  19521, 17236,
    8096,  19521, 24091, 24091, 24091, 17236, 21806,   8096,  12666, 21806, 14951, 2781,  776,
    4436,  6091,  -1,    -1,    776,   -1,    2781,    2781,  4436,  -1,    -1,    -1,    6091,
    4436,  -1,    -1,    -1,    6091,  -1,    -1,      -1,    776,   4436,  -1,    2781,  -1,
    -1,    776,   -1,    4436,  6091,  -1,    -1,      2781,  776,   6091,  -1};

  static constexpr int hires_as2_near_offset_ns[128] = {
    -1,   -1, -1, -1,   -1, -1, -1, -1, -1, -1, -1, 7336, -1, -1,    -1, -1,    -1,   -1, -1,
    -1,   -1, -1, -1,   -1, -1, -1, -1, -1, -1, -1, -1,   -1, 14061, -1, -1,    -1,   -1, -1,
    -1,   -1, -1, -1,   -1, -1, -1, -1, -1, -1, -1, -1,   -1, -1,    -1, 27056, -1,   -1, -1,
    -1,   -1, -1, -1,   -1, -1, -1, -1, -1, -1, -1, -1,   -1, -1,    -1, -1,    -1,   -1, -1,
    -1,   -1, -1, -1,   -1, -1, -1, -1, -1, -1, -1, -1,   -1, -1,    -1, -1,    6856, -1, -1,
    2021, -1, -1, 3546, -1, -1, -1, -1, -1, -1, -1, -1,   -1, -1,    -1, -1,    -1,   -1, 5201,
    -1,   -1, -1, -1,   -1, -1, -1, -1, -1, -1, -1, 1541, -1, -1};

  static constexpr int hires_as3_far_offset_ns[128] = {
    -1,    776,   -1,    -1,    -1,    2431,  -1,    4086,  6091,  -1,    -1,    -1,    776,
    6091,  2431,  776,   4086,  -1,    4086,  -1,    -1,    2431,  6091,  4086,  -1,    10031,
    14601, 12316, 14601, 19171, 19171, 7746,  12316, 12316, 10031, 23741, 16886, 23741, 14601,
    14601, 19171, 16886, 12316, 21456, 7746,  21456, 10031, 10031, 21456, 7746,  7746,  19171,
    12316, 12316, 23741, 23741, 16886, 21456, 16886, 14601, 10031, 14601, 16886, 16886, 7746,
    19171, 19171, 10031, 23741, 10031, 21456, 12316, 10031, 14601, 21456, 7746,  19171, 16886,
    7746,  19171, 23741, 23741, 23741, 16886, 21456, 7746,  12316, 21456, 14601, -1,    -1,
    -1,    -1,    6091,  2431,  -1,    776,   -1,    -1,    -1,    4086,  776,   2431,  -1,
    -1,    2431,  776,   4086,  -1,    6091,  6091,  4086,  -1,    -1,    4086,  -1,    2431,
    6091,  -1,    776,   -1,    -1,    6091,  2431,  -1,    -1,    -1,    776};

  static constexpr int hires_as3_near_offset_ns[128] = {
    -1, -1, -1,   -1,    -1,   -1,    -1, -1, -1,   -1, -1,    -1, -1,   -1,  -1, -1, -1, -1, -1,
    -1, -1, -1,   -1,    4851, -1,    -1, -1, -1,   -1, -1,    -1, -1,   -1,  -1, -1, -1, -1, -1,
    -1, -1, -1,   -1,    -1,   -1,    -1, -1, -1,   -1, -1,    -1, -1,   -1,  -1, -1, -1, -1, -1,
    -1, -1, -1,   26706, -1,   -1,    -1, -1, -1,   -1, 11426, -1, -1,   -1,  -1, -1, -1, -1, -1,
    -1, -1, -1,   -1,    -1,   25136, -1, -1, -1,   -1, -1,    -1, -1,   -1,  -1, -1, -1, -1, -1,
    -1, -1, -1,   -1,    -1,   -1,    -1, -1, -1,   -1, -1,    -1, 5331, -1,  -1, -1, -1, -1, -1,
    -1, -1, 3196, -1,    -1,   -1,    -1, -1, 6856, -1, -1,    -1, -1,   1541};

  static constexpr int standard_as0_far_offset_ns[128] = {
    4436,  28554, 776,   2431,  4436,  30559, 6441,  32564, 34219, 776,   2431,  6441,  28554,
    34219, 30559, 28554, 32564, 6441,  32564, 776,   2431,  30559, 34219, 32564, 4436,  38509,
    43079, 12666, 43079, 19521, 19521, 36224, 12666, 12666, 38509, 52219, 17236, 52219, 43079,
    43079, 19521, 17236, 12666, 21806, 36224, 21806, 38509, 38509, 21806, 36224, 36224, 19521,
    12666, 12666, 52219, 52219, 17236, 21806, 17236, 43079, 38509, 43079, 17236, 17236, 36224,
    19521, 19521, 38509, 52219, 38509, 21806, 12666, 38509, 43079, 21806, 36224, 19521, 17236,
    36224, 19521, 52219, 52219, 52219, 17236, 21806, 36224, 12666, 21806, 43079, 2431,  776,
    4436,  6441,  34219, 30559, 776,   28554, 2431,  2431,  4436,  32564, 28554, 30559, 6441,
    4436,  30559, 28554, 32564, 6441,  34219, 34219, 32564, 776,   4436,  32564, 2431,  30559,
    34219, 776,   28554, 4436,  6441,  34219, 30559, 2431,  776,   6441,  28554};

  static constexpr int standard_as0_near_offset_ns[128] = {
    5201, -1,   1541, -1, -1,   31804, -1, -1,    34984, -1,    -1,    -1, -1, -1, 31324, -1,
    -1,   7206, -1,   -1, 3196, -1,    -1, -1,    -1,    39904, -1,    -1, -1, -1, -1,    -1,
    -1,   -1,   -1,   -1, -1,   -1,    -1, 27056, -1,    -1,    -1,    -1, -1, -1, 55184, -1,
    -1,   -1,   -1,   -1, -1,   -1,    -1, -1,    -1,    -1,    -1,    -1, -1, -1, -1,    -1,
    -1,   -1,   -1,   -1, -1,   -1,    -1, -1,    -1,    -1,    23201, -1, -1, -1, -1,    -1,
    -1,   -1,   -1,   -1, -1,   -1,    -1, -1,    -1,    3676,  -1,    -1, -1, -1, -1,    -1,
    -1,   -1,   -1,   -1, -1,   29799, -1, -1,    5681,  -1,    -1,    -1, -1, -1, 35464, -1,
    -1,   -1,   -1,   -1, -1,   -1,    -1, 29319, -1,    -1,    -1,    -1, -1, -1, -1,    -1};

  static constexpr int standard_as1_far_offset_ns[128] = {
    4436,  28554, 776,   2781,  4436,  30209, 6091,  31864, 33869, 776,   2781,  6091,  28554,
    33869, 30209, 28554, 31864, 6091,  31864, 776,   2781,  30209, 33869, 31864, 4436,  37809,
    42379, 12666, 42379, 19521, 19521, 35524, 12666, 12666, 37809, 51519, 17236, 51519, 42379,
    42379, 19521, 17236, 12666, 21806, 35524, 21806, 37809, 37809, 21806, 35524, 35524, 19521,
    12666, 12666, 51519, 51519, 17236, 21806, 17236, 42379, 37809, 42379, 17236, 17236, 35524,
    19521, 19521, 37809, 51519, 37809, 21806, 12666, 37809, 42379, 21806, 35524, 19521, 17236,
    35524, 19521, 51519, 51519, 51519, 17236, 21806, 35524, 12666, 21806, 42379, 2781,  776,
    4436,  6091,  33869, 30209, 776,   28554, 2781,  2781,  4436,  31864, 28554, 30209, 6091,
    4436,  30209, 28554, 31864, 6091,  33869, 33869, 31864, 776,   4436,  31864, 2781,  30209,
    33869, 776,   28554, 4436,  6091,  33869, 30209, 2781,  776,   6091,  28554};

  static constexpr int standard_as1_near_offset_ns[128] = {
    -1,    -1, -1,    -1, -1, -1,   -1,    -1,    -1,    -1, -1, 7336,  -1, -1, -1,
    -1,    -1, -1,    -1, -1, -1,   -1,    -1,    32629, -1, -1, -1,    -1, -1, -1,
    -1,    -1, 14061, -1, -1, -1,   -1,    -1,    -1,    -1, -1, -1,    -1, -1, -1,
    -1,    -1, -1,    -1, -1, -1,   -1,    -1,    27056, -1, -1, -1,    -1, -1, -1,
    54484, -1, -1,    -1, -1, -1,   -1,    39204, -1,    -1, -1, -1,    -1, -1, -1,
    -1,    -1, -1,    -1, -1, -1,   52914, -1,    -1,    -1, -1, -1,    -1, -1, -1,
    -1,    -1, 6856,  -1, -1, 2021, -1,    -1,    3546,  -1, -1, -1,    -1, -1, -1,
    -1,    -1, 33109, -1, -1, -1,   -1,    -1,    5201,  -1, -1, 30974, -1, -1, -1,
    -1,    -1, 34634, -1, -1, 1541, -1,    29319};

public:
  static constexpr float min_range = 0.1;
  static constexpr float max_range = 230.0;
  static constexpr size_t max_scan_buffer_points = 691200;
  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-25'000, 14'400}};
  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{100, 125};

  int get_packet_relative_point_time_offset(
    uint32_t block_id, uint32_t channel_id, const packet_t & packet) override
  {
    auto n_returns = hesai_packet::get_n_returns(packet.tail.return_mode);
    int block_offset_ns = 3148 - 27778 * 2 * (2 - block_id - 1) / n_returns;

    int channel_offset_ns = 0;
    bool is_hires_mode = packet.tail.operational_state == OperationalState::HIGH_RESOLUTION;
    bool is_nearfield = (hesai_packet::get_dis_unit(packet) *
                         packet.body.blocks[block_id].units[channel_id].distance) <= 2.85f;
    auto azimuth_state = packet.tail.get_azimuth_state(block_id);

    if (is_hires_mode && azimuth_state == 0 && !is_nearfield)
      channel_offset_ns = hires_as0_far_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 0 && is_nearfield)
      channel_offset_ns = hires_as0_near_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 1 && !is_nearfield)
      channel_offset_ns = hires_as1_far_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 1 && is_nearfield)
      channel_offset_ns = hires_as1_near_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 2 && !is_nearfield)
      channel_offset_ns = hires_as2_far_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 2 && is_nearfield)
      channel_offset_ns = hires_as2_near_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 3 && !is_nearfield)
      channel_offset_ns = hires_as3_far_offset_ns[channel_id];
    else if (is_hires_mode && azimuth_state == 3 && is_nearfield)
      channel_offset_ns = hires_as3_near_offset_ns[channel_id];
    else if (!is_hires_mode && azimuth_state == 0 && !is_nearfield)
      channel_offset_ns = standard_as0_far_offset_ns[channel_id];
    else if (!is_hires_mode && azimuth_state == 0 && is_nearfield)
      channel_offset_ns = standard_as0_near_offset_ns[channel_id];
    else if (!is_hires_mode && azimuth_state == 1 && !is_nearfield)
      channel_offset_ns = standard_as1_far_offset_ns[channel_id];
    else if (!is_hires_mode && azimuth_state == 1 && is_nearfield)
      channel_offset_ns = standard_as1_near_offset_ns[channel_id];
    else
      throw std::runtime_error(
        "Invalid combination of operational state and azimuth state and nearfield firing");

    return block_offset_ns + channel_offset_ns;
  }

  ReturnType get_return_type(
    hesai_packet::return_mode::ReturnMode return_mode, unsigned int return_idx,
    const std::vector<const typename packet_t::body_t::block_t::unit_t *> & return_units) override
  {
    auto return_type =
      HesaiSensor<packet_t>::get_return_type(return_mode, return_idx, return_units);
    if (return_type == ReturnType::IDENTICAL) {
      return return_type;
    }

    // This sensor orders returns in the opposite order, so the return_type needs to be flipped too
    if (return_mode == hesai_packet::return_mode::DUAL_FIRST_LAST) {
      if (return_type == ReturnType::FIRST)
        return_type = ReturnType::LAST;
      else if (return_type == ReturnType::LAST)
        return_type = ReturnType::FIRST;
    }

    return return_type;
  }
};

}  // namespace nebula::drivers