Skip to content

File pandar_qt128.hpp

File List > decoders > pandar_qt128.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>

namespace nebula::drivers
{

namespace hesai_packet
{

#pragma pack(push, 1)

struct TailQT128C2X
{
  uint8_t reserved1[5];
  uint8_t mode_flag;
  uint8_t reserved2[6];
  uint8_t return_mode;
  uint16_t motor_speed;
  DateTime<1900> date_time;
  uint32_t timestamp;
  uint8_t factory_information;
  uint32_t udp_sequence;
  uint32_t crc_tail;

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

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

  enum class LidarState : uint8_t {
    INITIALIZATION = 0,
    NORMAL = 1,
    WARNING = 2,
    PERFORMANCE_DEGRADATION = 3,
    OUTPUT_UNTRUSTED = 4,
  };

  enum class FaultCodeType : uint8_t {
    NO_FAULT = 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;
  uint8_t reserved1[8];
  uint32_t crc_fs;

  [[nodiscard]] bool valid() const
  {
    // fs_version is not included in the CRC check
    return crc<crc32_mpeg2_t>(&fs_version + 1, &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::PERFORMANCE_DEGRADATION:
        return FunctionalSafetySeverity::WARNING;
      case LidarState::OUTPUT_UNTRUSTED:
      default:
        return FunctionalSafetySeverity::ERROR;
    }
  }

  friend bool operator==(const FunctionalSafetyQT128C2X & lhs, const FunctionalSafetyQT128C2X & 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 FunctionalSafetyQT128C2X & lhs, const FunctionalSafetyQT128C2X & rhs)
  {
    return !(lhs == rhs);
  }
};

static_assert(sizeof(FunctionalSafetyQT128C2X) == 17);

struct PacketQT128C2X : public PacketBase<2, 128, 2, 100>
{
  using body_t = BodyWithCrc<Block<Unit4B, PacketQT128C2X::n_channels>, PacketQT128C2X::n_blocks>;
  Header12B header;
  body_t body;
  FunctionalSafetyQT128C2X fs;
  TailQT128C2X tail;

  /* Ignored optional fields */

  // uint8_t cyber_security[32];
};

#pragma pack(pop)

}  // namespace hesai_packet

class PandarQT128 : public HesaiSensor<hesai_packet::PacketQT128C2X>
{
private:
  // Channels 0-31 (starting at 0) do not fire, delay set to 0
  static constexpr int loop1[128] = {
    0,     0,     0,     0,     0,     0,     0,     0,     0,      0,     0,     0,     0,
    0,     0,     0,     0,     0,     0,     0,     0,     0,      0,     0,     0,     0,
    0,     0,     0,     0,     0,     0,     27656, 53000, 2312,   78344, 81512, 5480,  56168,
    30824, 33992, 59336, 8648,  84680, 87848, 11816, 62504, 37160,  40328, 65672, 14984, 91016,
    94184, 18152, 68840, 43496, 46664, 72008, 21320, 97352, 100520, 24488, 75176, 49832, 1456,
    77488, 26800, 52144, 55312, 29968, 80656, 4624,  7792,  83824,  33136, 58480, 61648, 36304,
    86992, 10960, 14128, 90160, 39472, 64816, 67984, 42640, 93328,  17296, 20464, 96496, 45808,
    71152, 74320, 48976, 99664, 23632, 25944, 51288, 600,   76632,  79800, 3768,  54456, 29112,
    32280, 57624, 6936,  82968, 86136, 10104, 60792, 35448, 38616,  63960, 13272, 89304, 92472,
    16440, 67128, 41784, 44952, 70296, 19608, 95640, 98808, 22776,  73464, 48120};

  // Channels 32-63 (starting at 0) do not fire, delay set to 0
  static constexpr int loop2[128] = {
    2312,  78344, 27656, 53000, 56168,  30824, 81512, 5480,  8648,  84680, 33992, 59336, 62504,
    37160, 87848, 11816, 14984, 91016,  40328, 65672, 68840, 43496, 94184, 18152, 21320, 97352,
    46664, 72008, 75176, 49832, 100520, 24488, 0,     0,     0,     0,     0,     0,     0,
    0,     0,     0,     0,     0,      0,     0,     0,     0,     0,     0,     0,     0,
    0,     0,     0,     0,     0,      0,     0,     0,     0,     0,     0,     0,     600,
    76632, 25944, 51288, 54456, 29112,  79800, 3768,  6936,  82968, 32280, 57624, 60792, 35448,
    86136, 10104, 13272, 89304, 38616,  63960, 67128, 41784, 92472, 16440, 19608, 95640, 44952,
    70296, 73464, 48120, 98808, 22776,  26800, 52144, 1456,  77488, 80656, 4624,  55312, 29968,
    33136, 58480, 7792,  83824, 86992,  10960, 61648, 36304, 39472, 64816, 14128, 90160, 93328,
    17296, 67984, 42640, 45808, 71152,  20464, 96496, 99664, 23632, 74320, 48976};

public:
  static constexpr float min_range = 0.05;
  static constexpr float max_range = 50.0;
  static constexpr size_t max_scan_buffer_points = 172800;
  static constexpr FieldOfView<int32_t, MilliDegrees> fov_mdeg{{0, 360'000}, {-52'630, 52'630}};
  static constexpr AnglePair<int32_t, MilliDegrees> peak_resolution_mdeg{400, 100};

  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 = 9000 + 111110 * (2 - block_id - 1) / n_returns;

    int channel_offset_ns = 0;
    if (n_returns == 1) {
      channel_offset_ns = block_id % 2 == 0 ? loop1[channel_id] : loop2[channel_id];
    } else {
      channel_offset_ns = packet.tail.mode_flag & 0x01 ? loop1[channel_id] : loop2[channel_id];
    }

    return block_offset_ns + channel_offset_ns;
  }
};

}  // namespace nebula::drivers