QUYVN/DriverLIdar
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update Diagnostics

Browse Source
This commit is contained in:
QUYVN
2026-07-07 08:59:36 +07:00
parent 397b9ab3c5
commit 1c347a4918
8 changed files with 446 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

108
include/lidarlib/diagnostics.hpp Normal file
View File

@@ -0,0 +1,108 @@
#pragma once
#include <cstdint>
#include <cstdio>
#include <optional>
#include <string>
namespace lidarlib {
struct ExtraInfo; // lidar.hpp
// Family A (0xFAF0) error_status bits, header byte [5]. Bits 3-7 are reserved
// on the wire; a nonzero reserved bit is still reported as a fault.
inline constexpr uint8_t kFaultMonitor = 1u << 0; // monitor / motor abnormal
inline constexpr uint8_t kFaultVoltage = 1u << 1; // supply voltage out of range
inline constexpr uint8_t kFaultTemperature = 1u << 2; // internal temperature abnormal
// SICK TiM LMDscandata device status (low word; Telegram Listing).
inline constexpr uint16_t kSickStatusError = 1u << 0;
inline constexpr uint16_t kSickStatusPollutionWarning = 1u << 1;
inline constexpr uint16_t kSickStatusPollutionError = 1u << 2;
// SICK nanoScan3 General System State byte 0 (layout from sick_safetyscanners;
// NOT verified on real hardware).
inline constexpr uint8_t kNanoStateRunMode = 1u << 0;
inline constexpr uint8_t kNanoStateStandby = 1u << 1;
inline constexpr uint8_t kNanoStateContaminationWarning = 1u << 2;
inline constexpr uint8_t kNanoStateContaminationError = 1u << 3;
inline constexpr uint8_t kNanoStateReferenceContour = 1u << 4;
inline constexpr uint8_t kNanoStateManipulation = 1u << 5;
// Device self-diagnostics decoded from the data stream. Fields the family
// doesn't carry stay std::nullopt (see docs/diagnostics.md for the per-family
// wire layout). valid stays false until the driver has decoded one full scan.
struct Diagnostics {
bool valid = false;
std::string model = "AUTO";
uint32_t device_timestamp_ms = 0; // device clock; 0 if not on the wire
// Family A error byte (0 = no fault; Family B/C don't carry it)
uint8_t error_status = 0;
bool monitor_fault() const { return (error_status & kFaultMonitor) != 0; }
bool voltage_fault() const { return (error_status & kFaultVoltage) != 0; }
bool temperature_fault() const { return (error_status & kFaultTemperature) != 0; }
// Family A only — raw motor speed field, unit unverified
std::optional<uint16_t> rotation_raw;
// Family C / V3 (GS1-5) only — raw passthroughs, bit meanings unverified
std::optional<uint16_t> scan_frequency_raw;
std::optional<uint16_t> input_status;
std::optional<uint16_t> output_status;
std::optional<uint32_t> field_status;
std::optional<uint32_t> status_flags;
// SICK TiM — LMDscandata status pair (word0<<8)|word1
std::optional<uint16_t> sick_device_status;
bool sick_error() const { return sick_device_status && (*sick_device_status & kSickStatusError); }
bool pollution_warning() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionWarning); }
bool pollution_error() const { return sick_device_status && (*sick_device_status & kSickStatusPollutionError); }
// SICK nanoScan3 — General System State byte 0
std::optional<uint8_t> nano_general_state;
bool contamination_warning() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationWarning); }
bool contamination_error() const { return nano_general_state && (*nano_general_state & kNanoStateContaminationError); }
bool manipulation() const { return nano_general_state && (*nano_general_state & kNanoStateManipulation); }
// Fault = device says something is wrong now; warning = degraded but
// still measuring (dirty optics) — schedule cleaning/service.
bool has_fault() const {
return error_status != 0 || sick_error() || pollution_error()
|| contamination_error() || manipulation();
}
bool has_warning() const { return pollution_warning() || contamination_warning(); }
bool healthy() const { return valid && !has_fault(); }
};
// Decode the diagnostic fields of one scan; sets valid = true.
Diagnostics decode_diagnostics(const ExtraInfo& info);
// One-line log summary: "no data" / "ok" / "WARN: pollution" /
// "FAULT: voltage temperature".
inline std::string to_string(const Diagnostics& d) {
if (!d.valid) return "no data";
if (!d.has_fault()) return d.has_warning()
? std::string("WARN:") + (d.pollution_warning() ? " pollution" : "")
+ (d.contamination_warning() ? " contamination" : "")
: "ok";
std::string s = "FAULT:";
if (d.monitor_fault()) s += " monitor";
if (d.voltage_fault()) s += " voltage";
if (d.temperature_fault()) s += " temperature";
if (d.sick_error()) s += " device";
if (d.pollution_error()) s += " pollution";
if (d.contamination_error()) s += " contamination";
if (d.manipulation()) s += " manipulation";
if (uint8_t rest = d.error_status & ~(kFaultMonitor | kFaultVoltage | kFaultTemperature)) {
char buf[24];
std::snprintf(buf, sizeof(buf), " reserved(0x%02X)", rest);
s += buf;
}
return s;
}
} // namespace lidarlib

19
include/lidarlib/lidar.hpp
View File

@@ -1,4 +1,5 @@
#pragma once
#include "lidarlib/diagnostics.hpp"
#include "lidarlib/error.hpp"
#include <cstdint>
#include <vector>
@@ -39,6 +40,13 @@ struct ExtraInfo {
std::optional<uint16_t> output_status;
std::optional<uint32_t> field_status;
std::optional<uint32_t> status_flags;
// SICK TiM only — LMDscandata status pair (word0<<8)|word1:
// 0 ok, 1 error, 2 pollution warning, 4 pollution error.
std::optional<uint16_t> sick_device_status;
// SICK nanoScan3 only — General System State byte 0 (see kNanoState* bits).
std::optional<uint8_t> nano_general_state;
};
struct ScanResult {
@@ -101,6 +109,12 @@ public:
// Status of the most recent open()/recv_scan()/spin_once() call.
ErrorCode last_error() const { return last_error_; }
// Device self-diagnostics from the newest fully decoded scan. valid stays
// false until one scan has been seen — and permanently on drivers whose
// wire format carries no diagnostic fields (SICK, for now). Updated by
// recv_scan()/spin_once(); call from the same thread that pumps them.
virtual Diagnostics get_diagnostics() const { return {}; }
protected:
ErrorCode set_error(ErrorCode e) { last_error_ = e; return e; }
@@ -134,6 +148,8 @@ public:
// Model name read from the Family B/C header; "AUTO" until one is seen.
const char* detected_model() const override { return detected_model_name_.c_str(); }
Diagnostics get_diagnostics() const override { return latest_diag_; }
private:
bool parse_family_a(const uint8_t* buf, int len); // ID=0xFAF0
bool parse_family_b(const uint8_t* buf, int len); // ID=0xFEF0
@@ -159,6 +175,9 @@ private:
ExtraInfo pending_info_;
// Snapshot for get_diagnostics(); refreshed by flush_scan().
Diagnostics latest_diag_;
ScanResult ready_result_;
bool scan_ready_ = false;

10
include/lidarlib/sick_lidar.hpp
View File

@@ -39,6 +39,8 @@ public:
// No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); }
Diagnostics get_diagnostics() const override { return latest_diag_; }
private:
bool send_telegram(const std::string& body);
bool read_telegram(std::string& out, int timeout_ms);
@@ -51,6 +53,9 @@ private:
int sock_fd_ = -1;
ScanCallback cb_;
// Snapshot for get_diagnostics(); refreshed by parse_lmdscandata().
Diagnostics latest_diag_;
// Leftover TCP bytes carried across telegram boundaries; per-instance.
std::string recv_buf_;
};
@@ -84,6 +89,8 @@ public:
// No model string on the wire — returns the configured name.
const char* detected_model() const override { return detected_model_name_.c_str(); }
Diagnostics get_diagnostics() const override { return latest_diag_; }
private:
int recv_datagram(int timeout_ms);
bool parse_packet(const uint8_t* buf, int len, ScanResult& out);
@@ -95,6 +102,9 @@ private:
int sock_fd_ = -1;
ScanCallback cb_;
// Snapshot for get_diagnostics(); refreshed by parse_packet().
Diagnostics latest_diag_;
// Per-instance; sized for a full safety-data packet (max ~2751 beams).
std::vector<uint8_t> recv_buf_;
};