激光雷达高吞吐数据处理 Pipeline: 模块化架构与 NEON 向量化

// Handle: 仅 6 字节，在 SPSC 中传递
struct FrameHandle {
  uint16_t pool_index;    // ObjectPool 中的槽位
  uint32_t frame_id;      // 帧号 (全链路追踪)
};

// 数据本体: 留在 Pool 中，各 stage 原地读写
using FramePool = osp::ObjectPool<PointCloudSoA, 32>;  // 32 帧预分配
using StageRing = osp::SpscRingbuffer<FrameHandle, 32>; // 每 slot 仅 6B

// stage 间传递 Handle，不拷贝数据
StageRing acq_to_precalc;
StageRing precalc_to_xtalk;
StageRing xtalk_to_ghost;
// ...

// AoS: 每个点的 x/y/z/intensity 连续存储
struct Point { float x, y, z, intensity; };
struct RawFrame { uint32_t id; Point points[1024]; };  // 16 KB

// SoA: 同一分量连续存储，NEON 友好
struct PointCloudSoA {
  uint32_t frame_id;
  uint32_t count;                       // 有效点数
  alignas(16) float x[1024];            // NEON vld1q_f32 连续加载 4 个 x
  alignas(16) float y[1024];
  alignas(16) float z[1024];
  alignas(16) float intensity[1024];
  uint64_t timestamp_ns;                // osp::SteadyNowNs()
};
// sizeof = 16400B, alignas(16) 确保 NEON 对齐加载

#include <arm_neon.h>

void ComputeDistance(PointCloudSoA& cloud) {
  for (uint32_t i = 0; i < cloud.count; i += 4) {
    float32x4_t vx = vld1q_f32(&cloud.x[i]);
    float32x4_t vy = vld1q_f32(&cloud.y[i]);
    float32x4_t vz = vld1q_f32(&cloud.z[i]);

    // d^2 = x*x + y*y + z*z
    float32x4_t d2 = vmlaq_f32(vmlaq_f32(vmulq_f32(vx, vx),
                                           vy, vy), vz, vz);
    // d = 1/sqrt(d^2) * d^2 = sqrt(d^2), 用 vrsqrteq 近似
    float32x4_t inv = vrsqrteq_f32(d2);
    float32x4_t dist = vmulq_f32(d2, inv);

    vst1q_f32(&cloud.intensity[i], dist);  // in-place 覆写
  }
}

// 串扰去除: 原地修改 intensity
void RemoveCrosstalk(PointCloudSoA& cloud) {
  for (uint32_t i = 0; i < cloud.count; ++i) {
    cloud.intensity[i] -= crosstalk_table[i];  // in-place
  }
}

// 噪声滤波: 原地修改 x/y/z
void FilterNoise(PointCloudSoA& cloud) {
  // median filter on x[], y[], z[] -- in-place
}

// stage 间 SPSC 直连，每 slot 仅 6B
using StageRing = osp::SpscRingbuffer<FrameHandle, 32, /*FakeTSO=*/false>;

// 生产者 stage
void PreCalcStage::Process() {
  if (auto* handle = input_.Peek()) {      // 零拷贝读取 Handle
    auto& cloud = pool_[handle->pool_index];
    ComputeDistance(cloud);                 // 原地处理
    output_.Push(*handle);                 // 6B Handle 传递
    input_.Discard();                      // 释放上游 slot
  }
}

// 消费者 stage
void XTalkStage::Process() {
  if (auto* handle = input_.Peek()) {
    auto& cloud = pool_[handle->pool_index];
    RemoveCrosstalk(cloud);
    output_.Push(*handle);
    input_.Discard();
  }
}

enum class XTalkState : uint8_t {
  kIdle, kProcessing, kCalibrating, kError
};
enum class XTalkEvent : uint8_t {
  kFrameIn, kCalibReq, kCalibDone, kFail, kRetry
};

using XTalkHsm = osp::HsmStateMachine<XTalkState, XTalkEvent, 8>;
XTalkHsm hsm;

// 声明式状态层次
hsm.SetParent(XTalkState::kCalibrating, XTalkState::kProcessing);

// Guard 条件: 重试不超过 3 次
hsm.AddTransition(XTalkState::kError, XTalkEvent::kRetry,
                   XTalkState::kProcessing,
                   [](auto& ctx) { return ctx.retry_count < 3; });

// Entry/Exit 自动性能计时
hsm.OnEntry(XTalkState::kProcessing, [](auto&) { start_perf_timer(); });
hsm.OnExit(XTalkState::kProcessing, [](auto&) { record_perf_metrics(); });

// 预分配 32 帧 PointCloudSoA，零堆分配
using FramePool = osp::ObjectPool<PointCloudSoA, 32>;
FramePool pool;

// 采集 stage: 分配帧
auto* cloud = pool.Create();  // O(1), placement new
uint16_t idx = pool.IndexOf(cloud);

// DMA 直接写入 pool 槽位
dma_transfer(sensor_fd, cloud->x, cloud->count * sizeof(float));
// ...

// 构造 Handle 送入 SPSC
FrameHandle handle{idx, frame_id};
acq_to_precalc.Push(handle);

// 最后一个 stage: 释放帧
pool.Destroy(&pool[handle.pool_index]);  // O(1), placement delete

// Stage 线程分配 (4 核 Cortex-A53)
// CPU0: 采集 (DMA 中断亲和)
// CPU1: PreCalc + XTalk (融合，计算密集)
// CPU2: Ghost + FilterSeg (融合，内存密集)
// CPU3: 封装输出 + Shell 诊断

// 看门狗监控每个 stage 线程
osp::ThreadWatchdog<8> watchdog;
watchdog.SetTimeout(50);  // 50ms 超时 (100Hz 帧率的半周期)
watchdog.OnTimeout([](uint32_t thread_id) {
    osp::FaultCollector::Report(0x01010001, thread_id);
});

// 一行代码注册诊断命令
osp::shell_cmd::RegisterWatchdog(watchdog);
osp::shell_cmd::RegisterPool(pool, "frame_pool");

// Console/UART/Telnet 中执行
// osp> osp_watchdog
// [osp_watchdog] 4 threads monitored, 0 timeouts
// osp> osp_pool
// [osp_pool] frame_pool: 28/32 available, 4 in-use

struct StageMetrics {
  uint32_t processed_count = 0;
  uint64_t total_ns = 0;
  uint64_t max_ns = 0;

  void Record(uint64_t elapsed_ns) {
    ++processed_count;
    total_ns += elapsed_ns;
    if (elapsed_ns > max_ns) max_ns = elapsed_ns;
  }

  uint64_t AvgNs() const {
    return processed_count > 0 ? total_ns / processed_count : 0;
  }
};

#include "osp/spsc_ringbuffer.hpp"
#include "osp/mem_pool.hpp"
#include "osp/hsm.hpp"
#include "osp/bus.hpp"
#include "osp/watchdog.hpp"
#include "osp/shell.hpp"
#include "osp/shell_commands.hpp"
#include "osp/platform.hpp"

// SoA 点云数据
struct PointCloudSoA {
  uint32_t frame_id;
  uint32_t count;
  alignas(16) float x[1024];
  alignas(16) float y[1024];
  alignas(16) float z[1024];
  alignas(16) float intensity[1024];
  uint64_t timestamp_ns;
};

// Handle: 仅 6 字节在 SPSC 中流转
struct FrameHandle {
  uint16_t pool_index;
  uint32_t frame_id;
};

// 基础设施
using FramePool = osp::ObjectPool<PointCloudSoA, 32>;
using StageRing = osp::SpscRingbuffer<FrameHandle, 32>;

// Pipeline stage (示例: 距离计算)
class PreCalcStage {
 public:
  PreCalcStage(FramePool& pool, StageRing& in, StageRing& out)
      : pool_(pool), input_(in), output_(out) {}

  void Run() {
    while (running_.load(std::memory_order_relaxed)) {
      heartbeat_.Beat();
      if (auto* h = input_.Peek()) {
        uint64_t t0 = osp::SteadyNowNs();
        auto& cloud = pool_[h->pool_index];
        ComputeDistance(cloud);              // SoA + NEON, in-place
        output_.Push(*h);                   // 6B Handle
        input_.Discard();
        metrics_.Record(osp::SteadyNowNs() - t0);
      } else {
        std::this_thread::yield();
      }
    }
  }

 private:
  FramePool& pool_;
  StageRing& input_;
  StageRing& output_;
  std::atomic<bool> running_{true};
  osp::ThreadHeartbeat heartbeat_;
  StageMetrics metrics_;
};

int main() {
  // 1. 基础设施
  FramePool pool;
  StageRing acq_to_precalc, precalc_to_xtalk, xtalk_to_ghost,
            ghost_to_filtseg, filtseg_to_pkg;

  // 2. 创建各 stage (省略完整实现)
  PreCalcStage precalc(pool, acq_to_precalc, precalc_to_xtalk);

  // 3. 看门狗
  osp::ThreadWatchdog<8> watchdog;
  watchdog.SetTimeout(50);

  // 4. Shell 诊断
  osp::ConsoleShell console;
  osp::shell_cmd::RegisterWatchdog(watchdog);
  console.Start();

  // 5. 启动 stage 线程 (每 stage 绑核)
  std::thread t1([&] {
    // CPU1 affinity + SCHED_FIFO
    precalc.Run();
  });

  // ... 其他 stage 线程 ...

  t1.join();
  return 0;
}