43 lines
1.4 KiB
C++
43 lines
1.4 KiB
C++
// Internal helpers shared by the driver TUs — not part of the public API.
|
|
#pragma once
|
|
#include "lidarlib/lidar.hpp"
|
|
#include <cstdint>
|
|
#include <cstring>
|
|
|
|
namespace lidarlib {
|
|
|
|
inline constexpr float kDeg2Rad = 3.14159265358979323846f / 180.f;
|
|
|
|
// Remap a finished scan's angular window onto [min_deg, max_deg]. Only
|
|
// angle_min/angle_max/angle_increment are rewritten; points are untouched.
|
|
inline void remap_scan_window(LaserScan& scan, float min_deg, float max_deg) {
|
|
const float new_min = min_deg * kDeg2Rad;
|
|
const float new_max = max_deg * kDeg2Rad;
|
|
const float old_span = scan.angle_max - scan.angle_min;
|
|
if (old_span > 0.f)
|
|
scan.angle_increment *= (new_max - new_min) / old_span;
|
|
scan.angle_min = new_min;
|
|
scan.angle_max = new_max;
|
|
}
|
|
|
|
// Little-endian readers (bounds are the caller's responsibility).
|
|
inline uint8_t le_u8 (const uint8_t* p) { return p[0]; }
|
|
inline uint16_t le16(const uint8_t* p) {
|
|
return static_cast<uint16_t>(p[0] | (p[1] << 8));
|
|
}
|
|
inline uint32_t le32(const uint8_t* p) {
|
|
return static_cast<uint32_t>(p[0])
|
|
| (static_cast<uint32_t>(p[1]) << 8)
|
|
| (static_cast<uint32_t>(p[2]) << 16)
|
|
| (static_cast<uint32_t>(p[3]) << 24);
|
|
}
|
|
inline int32_t le_i32(const uint8_t* p) { return static_cast<int32_t>(le32(p)); }
|
|
|
|
inline float bits_to_float(uint32_t bits) {
|
|
float f;
|
|
std::memcpy(&f, &bits, sizeof(f));
|
|
return f;
|
|
}
|
|
|
|
} // namespace lidarlib
|