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/hesai_packet.hpp"
#include "nebula_decoders/nebula_decoders_hesai/decoders/hesai_sensor.hpp"

#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;
  }
};

struct Packet128E3X : public PacketBase<2, 128, 2, 100>
{
  using body_t = Body<Block<Unit3B, Packet128E3X::n_channels>, Packet128E3X::n_blocks>;
  Header12B header;
  body_t body;
  uint32_t crc_body;
  FunctionalSafety 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;

  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