Add lidar PWM auto-tuning and optimize binary sizes
Lidar auto-tuning: - Implement adaptive binary-search algorithm to find max stable PWM - Start at 100% PWM, ramp down with 20% steps, halve on direction reversal - Converge when step reaches 2% and scans are stable (typically ~75%) - Add settling detection via timeout (250ms) or health packet count (3) - Share scan timestamps and health counts between GD32/Delta2D drivers - Add recovery mode to handle unexpected scan loss - Remove manual PWM configuration (Configure action now ignored) - Document auto-tuning algorithm in COMMANDS.md Binary size optimization: - Disable default features for serde, prost, rand, image, env_logger - Replace toml crate with basic-toml (no toml_edit/winnow) - Disable serialport libudev feature (not needed on embedded Linux) - Add size-optimized release profiles (opt-level=z, LTO, panic=abort) - Remove NEON rustflags from .cargo/config.toml (caused issues)
This commit is contained in:
codetiger
+17
-7
@@ -7,20 +7,20 @@ license = "MIT"
|
||||
|
||||
[dependencies]
|
||||
base64 = "0.22"
|
||||
clap = { version = "4.5", features = ["derive"] } # CLI parsing for binaries
|
||||
clap = { version = "4.5", default-features = false, features = ["derive", "std", "help", "usage", "error-context"] }
|
||||
ctrlc = "3.5"
|
||||
env_logger = "0.11"
|
||||
env_logger = { version = "0.11", default-features = false, features = ["humantime"] }
|
||||
kiddo = "4.2" # K-d tree for nearest neighbor queries
|
||||
log = "0.4"
|
||||
postcard = { version = "1.1", features = ["alloc"] } # For bag file format (internal)
|
||||
prost = "0.13"
|
||||
rand = "0.9" # Random number generation for benchmark sampling
|
||||
serde = { version = "1.0", features = ["derive"] }
|
||||
prost = { version = "0.13", default-features = false, features = ["std", "derive"] }
|
||||
rand = { version = "0.9", default-features = false, features = ["std", "std_rng", "os_rng"] }
|
||||
serde = { version = "1.0", default-features = false, features = ["derive", "std"] }
|
||||
serde_json = "1.0" # JSON output for benchmarks
|
||||
sysinfo = "0.32" # Memory monitoring for benchmarks
|
||||
image = "0.25" # PNG export for map visualization
|
||||
image = { version = "0.25", default-features = false, features = ["png"] } # Only PNG export
|
||||
thiserror = "2.0"
|
||||
toml = "0.8"
|
||||
basic-toml = "0.1" # Minimal TOML parser
|
||||
|
||||
[build-dependencies]
|
||||
prost-build = "0.13"
|
||||
@@ -30,6 +30,16 @@ approx = "0.5"
|
||||
criterion = { version = "0.5", features = ["html_reports"] }
|
||||
tempfile = "3.23"
|
||||
|
||||
[profile.release]
|
||||
opt-level = "z" # Optimize for size
|
||||
lto = true # Link-time optimization
|
||||
codegen-units = 1 # Single codegen unit for better optimization
|
||||
panic = "abort" # Abort on panic (smaller than unwinding)
|
||||
strip = true # Strip symbols from binary
|
||||
|
||||
[profile.release.build-override]
|
||||
opt-level = "z"
|
||||
|
||||
[[example]]
|
||||
name = "dhruva_slam_node"
|
||||
path = "examples/dhruva_slam_node.rs"
|
||||
|
||||
@@ -207,7 +207,7 @@ fn print_help() {
|
||||
fn load_config(args: &Args) -> Config {
|
||||
let config = match &args.config_path {
|
||||
Some(path) => match fs::read_to_string(path) {
|
||||
Ok(contents) => match toml::from_str(&contents) {
|
||||
Ok(contents) => match basic_toml::from_str(&contents) {
|
||||
Ok(cfg) => {
|
||||
eprintln!("Loaded config from {}", path);
|
||||
cfg
|
||||
@@ -226,7 +226,7 @@ fn load_config(args: &Args) -> Config {
|
||||
// Try default paths
|
||||
for path in &["dhruva-slam.toml", "/etc/dhruva-slam.toml"] {
|
||||
if let Ok(contents) = fs::read_to_string(path)
|
||||
&& let Ok(cfg) = toml::from_str(&contents)
|
||||
&& let Ok(cfg) = basic_toml::from_str(&contents)
|
||||
{
|
||||
eprintln!("Loaded config from {}", path);
|
||||
return apply_overrides(cfg, args);
|
||||
|
||||
@@ -7,5 +7,15 @@ license = "Apache-2.0"
|
||||
repository = "https://github.com/codetiger/VacuumTiger"
|
||||
|
||||
[dependencies]
|
||||
chrono = "0.4"
|
||||
chrono = { version = "0.4", default-features = false, features = ["std", "clock"] }
|
||||
libc = "0.2"
|
||||
|
||||
[profile.release]
|
||||
opt-level = "z" # Optimize for size
|
||||
lto = true # Link-time optimization
|
||||
codegen-units = 1 # Single codegen unit for better optimization
|
||||
panic = "abort" # Abort on panic (smaller than unwinding)
|
||||
strip = true # Strip symbols from binary
|
||||
|
||||
[profile.release.build-override]
|
||||
opt-level = "z"
|
||||
|
||||
@@ -3,5 +3,4 @@ target = "armv7-unknown-linux-musleabihf"
|
||||
|
||||
[target.armv7-unknown-linux-musleabihf]
|
||||
linker = "armv7-unknown-linux-musleabihf-gcc"
|
||||
# Enable NEON SIMD for auto-vectorization on Cortex-A8 (Allwinner A33)
|
||||
rustflags = ["-C", "target-feature=+neon", "-C", "target-cpu=cortex-a7"]
|
||||
rustflags = ["-C", "target-cpu=cortex-a7"]
|
||||
|
||||
@@ -131,3 +131,101 @@ Values 0-18 have distinct behaviors; values 19+ default to Orange Stable.
|
||||
|
||||
**Usage**: `ComponentControl { id: "led", action: Configure { config: { "state": U8(N) } } }` where N is 0-18
|
||||
|
||||
---
|
||||
|
||||
## Lidar Auto-Tuning
|
||||
|
||||
The lidar motor requires PWM control (0x71) to spin at the correct speed for scanning. SangamIO implements **automatic PWM discovery** that finds the maximum stable PWM value - no user configuration needed.
|
||||
|
||||
### How It Works
|
||||
|
||||
When lidar is enabled via `ComponentControl { id: "lidar", action: Enable }`:
|
||||
|
||||
1. **Start at 100% PWM** - Motor begins at full speed
|
||||
2. **Adaptive step ramping** - PWM decreases with large steps (20%) initially
|
||||
3. **Direction reversal on state change** - When scans start/stop, direction reverses and step halves
|
||||
4. **Convergence** - Settles at maximum stable PWM when step size reaches 2%
|
||||
5. **Stop sending commands** - Once stable, no more PWM commands are sent
|
||||
|
||||
### Settling Detection
|
||||
|
||||
After each PWM change, the driver waits before evaluating success:
|
||||
- **250ms timeout** OR **3 consecutive health packets (0xAE)** - whichever comes first
|
||||
- This allows the motor to stabilize before checking if measurement scans (0xAD) arrive
|
||||
|
||||
### State Machine
|
||||
|
||||
```
|
||||
┌─────────────────┐
|
||||
│ RAMPING │ PWM starts at 100%, step starts at 20%
|
||||
│ (finding range) │ Step halves on each direction reversal
|
||||
└────────┬────────┘
|
||||
│ step_size <= 2% and scans stable
|
||||
▼
|
||||
┌─────────────────┐
|
||||
│ STABLE │ Maximum stable PWM found
|
||||
│ (optimal found) │ Stop sending PWM commands (0x71)
|
||||
└────────┬────────┘
|
||||
│ scans stop unexpectedly
|
||||
▼
|
||||
┌─────────────────┐
|
||||
│ RECOVERY │ Increase PWM by 5% until scans resume
|
||||
└─────────────────┘ Return to STABLE when recovered
|
||||
```
|
||||
|
||||
### Example Tuning Sequence
|
||||
|
||||
Starting at 100% with 20% step size:
|
||||
|
||||
```
|
||||
100% → 80% (step=20, no scans yet, decrease)
|
||||
80% → 60% (step=20, no scans, decrease)
|
||||
60% (scans start! reverse direction, step halves to 10%)
|
||||
60% → 70% (step=10, scans working, increase)
|
||||
70% → 80% (step=10, scans working, increase)
|
||||
80% (scans stop! reverse direction, step halves to 5%)
|
||||
80% → 75% (step=5, no scans, decrease)
|
||||
75% (scans start! reverse direction, step halves to 2%)
|
||||
75% → 73% (step=2, scans stable, step <= MIN_STEP)
|
||||
→ STABLE at 73% (stop sending PWM commands)
|
||||
```
|
||||
|
||||
Total tuning time: ~400-800ms (depends on motor characteristics)
|
||||
|
||||
### Constants
|
||||
|
||||
| Constant | Value | Description |
|
||||
|----------|-------|-------------|
|
||||
| `SCAN_TIMEOUT_MS` | 200 | No scan within this time = "not receiving" |
|
||||
| `SETTLING_TIMEOUT_MS` | 250 | Wait after PWM change before evaluating |
|
||||
| `SETTLING_HEALTH_COUNT` | 3 | OR wait for this many health packets |
|
||||
| `INITIAL_STEP` | 20% | Starting step size |
|
||||
| `MIN_STEP` | 2% | Convergence threshold |
|
||||
| `MIN_PWM` | 30% | Never go below this |
|
||||
| `MAX_PWM` | 100% | Never exceed this |
|
||||
| `RECOVERY_STEP` | 5% | Fixed step for recovery mode |
|
||||
|
||||
### TCP API
|
||||
|
||||
**Enable lidar (starts auto-tuning):**
|
||||
```protobuf
|
||||
RobotCommand {
|
||||
command: ComponentControl {
|
||||
id: "lidar",
|
||||
action: Enable {}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
**Disable lidar (stops motor, resets tuning state):**
|
||||
```protobuf
|
||||
RobotCommand {
|
||||
command: ComponentControl {
|
||||
id: "lidar",
|
||||
action: Disable {}
|
||||
}
|
||||
}
|
||||
```
|
||||
|
||||
**Note:** Manual PWM configuration is no longer supported. The `Configure` action for lidar is ignored.
|
||||
|
||||
|
||||
+18
-8
@@ -7,20 +7,20 @@ authors = ["CodeTiger"]
|
||||
|
||||
[dependencies]
|
||||
# Serialization
|
||||
serde = { version = "1.0", features = ["derive"] }
|
||||
toml = "0.8"
|
||||
prost = "0.13"
|
||||
serde = { version = "1.0", default-features = false, features = ["derive", "std"] }
|
||||
basic-toml = "0.1" # Minimal TOML parser (no toml_edit/winnow)
|
||||
prost = { version = "0.13", default-features = false, features = ["std", "derive"] }
|
||||
|
||||
# Serial port
|
||||
serialport = "4.8"
|
||||
# Serial port (libudev not needed on embedded Linux)
|
||||
serialport = { version = "4.8", default-features = false }
|
||||
|
||||
# Logging
|
||||
log = "0.4"
|
||||
env_logger = "0.10"
|
||||
env_logger = { version = "0.10", default-features = false, features = ["humantime"] }
|
||||
|
||||
# Concurrency
|
||||
crossbeam-queue = "0.3"
|
||||
crossbeam-channel = "0.5"
|
||||
crossbeam-queue = { version = "0.3", default-features = false, features = ["std"] }
|
||||
crossbeam-channel = { version = "0.5", default-features = false, features = ["std"] }
|
||||
|
||||
# Error handling
|
||||
thiserror = "1.0"
|
||||
@@ -30,3 +30,13 @@ ctrlc = "3.5"
|
||||
|
||||
[build-dependencies]
|
||||
prost-build = "0.13"
|
||||
|
||||
[profile.release]
|
||||
opt-level = "z" # Optimize for size
|
||||
lto = true # Link-time optimization
|
||||
codegen-units = 1 # Single codegen unit for better optimization
|
||||
panic = "abort" # Abort on panic (smaller than unwinding)
|
||||
strip = true # Strip symbols from binary
|
||||
|
||||
[profile.release.build-override]
|
||||
opt-level = "z"
|
||||
|
||||
@@ -109,7 +109,7 @@ impl Config {
|
||||
let content = fs::read_to_string(&path)
|
||||
.map_err(|e| Error::Config(format!("Failed to read config: {}", e)))?;
|
||||
|
||||
let config: Config = toml::from_str(&content)
|
||||
let config: Config = basic_toml::from_str(&content)
|
||||
.map_err(|e| Error::Config(format!("Failed to parse config: {}", e)))?;
|
||||
|
||||
Ok(config)
|
||||
|
||||
@@ -54,16 +54,24 @@ pub mod protocol;
|
||||
|
||||
use crate::core::types::{SensorGroupData, SensorValue};
|
||||
use crate::error::{Error, Result};
|
||||
use protocol::Delta2DPacketReader;
|
||||
use protocol::{Delta2DPacketReader, ParseResult};
|
||||
use serialport::SerialPort;
|
||||
use std::sync::atomic::{AtomicBool, AtomicU64, Ordering};
|
||||
use std::sync::atomic::{AtomicBool, AtomicU64, AtomicU8, Ordering};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::thread::{self, JoinHandle};
|
||||
use std::time::{Duration, Instant};
|
||||
use std::time::{Duration, Instant, SystemTime, UNIX_EPOCH};
|
||||
|
||||
const LIDAR_BAUD_RATE: u32 = 115200;
|
||||
const LIDAR_READ_TIMEOUT_MS: u64 = 100;
|
||||
|
||||
/// Get current time in milliseconds since epoch
|
||||
fn current_time_ms() -> u64 {
|
||||
SystemTime::now()
|
||||
.duration_since(UNIX_EPOCH)
|
||||
.map(|d| d.as_millis() as u64)
|
||||
.unwrap_or(0)
|
||||
}
|
||||
|
||||
/// Minimum points required before accepting a scan as complete
|
||||
/// This prevents false scan boundaries from angle noise or single bad readings
|
||||
const MIN_SCAN_POINTS: usize = 50;
|
||||
@@ -79,17 +87,31 @@ pub struct Delta2DDriver {
|
||||
reader_handle: Option<JoinHandle<()>>,
|
||||
scan_count: Arc<AtomicU64>,
|
||||
error_count: Arc<AtomicU64>,
|
||||
// Shared state with GD32 for lidar PWM auto-tuning
|
||||
scan_timestamp: Arc<AtomicU64>,
|
||||
health_count: Arc<AtomicU8>,
|
||||
}
|
||||
|
||||
impl Delta2DDriver {
|
||||
/// Create a new Delta-2D driver
|
||||
pub fn new(port_path: &str) -> Self {
|
||||
///
|
||||
/// # Arguments
|
||||
/// - `port_path`: Serial port path (e.g., "/dev/ttyS1")
|
||||
/// - `scan_timestamp`: Shared timestamp updated when measurement scans arrive (for GD32 PWM tuning)
|
||||
/// - `health_count`: Shared counter incremented on each health packet (for GD32 PWM settling)
|
||||
pub fn new(
|
||||
port_path: &str,
|
||||
scan_timestamp: Arc<AtomicU64>,
|
||||
health_count: Arc<AtomicU8>,
|
||||
) -> Self {
|
||||
Self {
|
||||
port_path: port_path.to_string(),
|
||||
shutdown: Arc::new(AtomicBool::new(false)),
|
||||
reader_handle: None,
|
||||
scan_count: Arc::new(AtomicU64::new(0)),
|
||||
error_count: Arc::new(AtomicU64::new(0)),
|
||||
scan_timestamp,
|
||||
health_count,
|
||||
}
|
||||
}
|
||||
|
||||
@@ -112,12 +134,22 @@ impl Delta2DDriver {
|
||||
let shutdown = Arc::clone(&self.shutdown);
|
||||
let scan_count = Arc::clone(&self.scan_count);
|
||||
let error_count = Arc::clone(&self.error_count);
|
||||
let scan_timestamp = Arc::clone(&self.scan_timestamp);
|
||||
let health_count = Arc::clone(&self.health_count);
|
||||
|
||||
self.reader_handle = Some(
|
||||
thread::Builder::new()
|
||||
.name("delta2d-reader".to_string())
|
||||
.spawn(move || {
|
||||
Self::reader_loop(port, shutdown, sensor_data, scan_count, error_count);
|
||||
Self::reader_loop(
|
||||
port,
|
||||
shutdown,
|
||||
sensor_data,
|
||||
scan_count,
|
||||
error_count,
|
||||
scan_timestamp,
|
||||
health_count,
|
||||
);
|
||||
})
|
||||
.map_err(|e| Error::Other(format!("Failed to spawn lidar thread: {}", e)))?,
|
||||
);
|
||||
@@ -133,6 +165,8 @@ impl Delta2DDriver {
|
||||
sensor_data: Arc<Mutex<SensorGroupData>>,
|
||||
scan_count: Arc<AtomicU64>,
|
||||
error_count: Arc<AtomicU64>,
|
||||
scan_timestamp: Arc<AtomicU64>,
|
||||
health_count: Arc<AtomicU8>,
|
||||
) {
|
||||
let mut reader = Delta2DPacketReader::new();
|
||||
let mut accumulated_points: Vec<(f32, f32, u8)> = Vec::with_capacity(400);
|
||||
@@ -150,7 +184,7 @@ impl Delta2DDriver {
|
||||
// Step 2: Drain ALL complete packets from buffer
|
||||
loop {
|
||||
match reader.parse_next() {
|
||||
Ok(Some(scan)) => {
|
||||
Ok(ParseResult::Scan(scan)) => {
|
||||
let count = scan_count.fetch_add(1, Ordering::Relaxed) + 1;
|
||||
|
||||
// Accumulate points for a complete 360° scan
|
||||
@@ -172,7 +206,11 @@ impl Delta2DDriver {
|
||||
last_angle.to_degrees(),
|
||||
first_point.angle.to_degrees()
|
||||
);
|
||||
Self::publish_scan(&sensor_data, &accumulated_points);
|
||||
Self::publish_scan(
|
||||
&sensor_data,
|
||||
&accumulated_points,
|
||||
&scan_timestamp,
|
||||
);
|
||||
accumulated_points.clear();
|
||||
last_scan_time = Instant::now();
|
||||
}
|
||||
@@ -199,7 +237,11 @@ impl Delta2DDriver {
|
||||
elapsed,
|
||||
accumulated_points.len()
|
||||
);
|
||||
Self::publish_scan(&sensor_data, &accumulated_points);
|
||||
Self::publish_scan(
|
||||
&sensor_data,
|
||||
&accumulated_points,
|
||||
&scan_timestamp,
|
||||
);
|
||||
accumulated_points.clear();
|
||||
last_scan_time = Instant::now();
|
||||
}
|
||||
@@ -227,7 +269,11 @@ impl Delta2DDriver {
|
||||
last_diagnostic_time = Instant::now();
|
||||
}
|
||||
}
|
||||
Ok(None) => {
|
||||
Ok(ParseResult::Health) => {
|
||||
// Health packet received - increment counter for PWM settling detection
|
||||
health_count.fetch_add(1, Ordering::Relaxed);
|
||||
}
|
||||
Ok(ParseResult::None) => {
|
||||
// No more complete packets in buffer - break inner loop
|
||||
break;
|
||||
}
|
||||
@@ -247,7 +293,7 @@ impl Delta2DDriver {
|
||||
elapsed,
|
||||
accumulated_points.len()
|
||||
);
|
||||
Self::publish_scan(&sensor_data, &accumulated_points);
|
||||
Self::publish_scan(&sensor_data, &accumulated_points, &scan_timestamp);
|
||||
accumulated_points.clear();
|
||||
last_scan_time = Instant::now();
|
||||
}
|
||||
@@ -266,8 +312,12 @@ impl Delta2DDriver {
|
||||
log::info!("Delta-2D reader thread exiting");
|
||||
}
|
||||
|
||||
/// Publish accumulated scan to sensor data
|
||||
fn publish_scan(sensor_data: &Arc<Mutex<SensorGroupData>>, points: &[(f32, f32, u8)]) {
|
||||
/// Publish accumulated scan to sensor data and update timestamp for PWM tuning
|
||||
fn publish_scan(
|
||||
sensor_data: &Arc<Mutex<SensorGroupData>>,
|
||||
points: &[(f32, f32, u8)],
|
||||
scan_timestamp: &Arc<AtomicU64>,
|
||||
) {
|
||||
let Ok(mut data) = sensor_data.lock() else {
|
||||
log::error!("Failed to lock sensor data for lidar scan");
|
||||
return;
|
||||
@@ -275,6 +325,9 @@ impl Delta2DDriver {
|
||||
data.touch();
|
||||
data.set("scan", SensorValue::PointCloud2D(points.to_vec()));
|
||||
|
||||
// Update shared timestamp for GD32 PWM auto-tuning
|
||||
scan_timestamp.store(current_time_ms(), Ordering::Relaxed);
|
||||
|
||||
log::trace!("Published lidar scan with {} points", points.len());
|
||||
}
|
||||
|
||||
|
||||
@@ -46,6 +46,17 @@ pub enum CommandType {
|
||||
Unknown = 0xFF,
|
||||
}
|
||||
|
||||
/// Result of parsing a lidar packet
|
||||
#[derive(Debug, Clone)]
|
||||
pub enum ParseResult {
|
||||
/// Measurement scan data
|
||||
Scan(LidarScan),
|
||||
/// Health packet (motor is spinning, RPM logged)
|
||||
Health,
|
||||
/// No complete packet available
|
||||
None,
|
||||
}
|
||||
|
||||
impl From<u8> for CommandType {
|
||||
fn from(value: u8) -> Self {
|
||||
match value {
|
||||
@@ -109,9 +120,14 @@ impl Delta2DPacketReader {
|
||||
|
||||
/// Parse and return the next complete packet from the buffer
|
||||
///
|
||||
/// Call this in a loop after `read_bytes()` until it returns `Ok(None)`
|
||||
/// Call this in a loop after `read_bytes()` until it returns `Ok(ParseResult::None)`
|
||||
/// to drain all available packets from the buffer.
|
||||
pub fn parse_next(&mut self) -> Result<Option<LidarScan>> {
|
||||
///
|
||||
/// Returns:
|
||||
/// - `ParseResult::Scan(scan)` for measurement packets (0xAD)
|
||||
/// - `ParseResult::Health { rpm }` for health packets (0xAE)
|
||||
/// - `ParseResult::None` when no complete packet is available
|
||||
pub fn parse_next(&mut self) -> Result<ParseResult> {
|
||||
// Safety check: if buffer is too large, something is wrong
|
||||
if self.buffer.len() > MAX_BUFFER_SIZE {
|
||||
log::warn!(
|
||||
@@ -122,7 +138,7 @@ impl Delta2DPacketReader {
|
||||
);
|
||||
self.bytes_discarded += self.buffer.len() as u64;
|
||||
self.buffer.clear();
|
||||
return Ok(None);
|
||||
return Ok(ParseResult::None);
|
||||
}
|
||||
|
||||
// Try to parse one packet using loop-based resync
|
||||
@@ -133,16 +149,19 @@ impl Delta2DPacketReader {
|
||||
#[allow(dead_code)]
|
||||
pub fn read_scan<R: Read>(&mut self, port: &mut R) -> Result<Option<LidarScan>> {
|
||||
self.read_bytes(port)?;
|
||||
self.parse_next()
|
||||
match self.parse_next()? {
|
||||
ParseResult::Scan(scan) => Ok(Some(scan)),
|
||||
_ => Ok(None),
|
||||
}
|
||||
}
|
||||
|
||||
/// Parse scan with loop-based resync on failure
|
||||
/// On parse failure, drains 1 byte and retries instead of clearing buffer
|
||||
fn try_parse_scan_with_resync(&mut self) -> Result<Option<LidarScan>> {
|
||||
fn try_parse_scan_with_resync(&mut self) -> Result<ParseResult> {
|
||||
loop {
|
||||
// Need at least header (8 bytes)
|
||||
if self.buffer.len() < 8 {
|
||||
return Ok(None);
|
||||
return Ok(ParseResult::None);
|
||||
}
|
||||
|
||||
// Find sync byte 0xAA
|
||||
@@ -155,7 +174,7 @@ impl Delta2DPacketReader {
|
||||
log::trace!("Lidar: no sync found, discarding {} bytes", discard);
|
||||
self.buffer.drain(0..discard);
|
||||
}
|
||||
return Ok(None);
|
||||
return Ok(ParseResult::None);
|
||||
};
|
||||
|
||||
// Discard bytes before sync
|
||||
@@ -167,7 +186,7 @@ impl Delta2DPacketReader {
|
||||
|
||||
// Check if we have complete header
|
||||
if self.buffer.len() < 8 {
|
||||
return Ok(None);
|
||||
return Ok(ParseResult::None);
|
||||
}
|
||||
|
||||
// Validate header before proceeding
|
||||
@@ -198,7 +217,7 @@ impl Delta2DPacketReader {
|
||||
|
||||
// Wait for complete packet
|
||||
if self.buffer.len() < total_len {
|
||||
return Ok(None);
|
||||
return Ok(ParseResult::None);
|
||||
}
|
||||
|
||||
// NOTE: CRC validation disabled for now as the Delta-2D protocol's
|
||||
@@ -228,22 +247,22 @@ impl Delta2DPacketReader {
|
||||
let payload = &self.buffer[8..8 + payload_len as usize];
|
||||
|
||||
// Parse based on command type
|
||||
let scan = if cmd_type == CommandType::Measurement && payload_len > 5 {
|
||||
Some(self.parse_measurement(payload)?)
|
||||
let result = if cmd_type == CommandType::Measurement && payload_len > 5 {
|
||||
ParseResult::Scan(self.parse_measurement(payload)?)
|
||||
} else if cmd_type == CommandType::Health && payload_len >= 1 {
|
||||
let rps_raw = payload[0];
|
||||
let rpm = rps_raw as u16 * 3;
|
||||
let rps = rps_raw as f32 * 0.05;
|
||||
log::info!("Lidar health: RPM={}, RPS={:.2}", rpm, rps);
|
||||
None
|
||||
ParseResult::Health
|
||||
} else {
|
||||
None
|
||||
ParseResult::None
|
||||
};
|
||||
|
||||
// Remove processed packet
|
||||
self.buffer.drain(0..total_len);
|
||||
|
||||
return Ok(scan);
|
||||
return Ok(result);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -467,19 +467,21 @@ fn handle_lidar(
|
||||
SensorValue::U32(p) => Some(*p as u8),
|
||||
_ => None,
|
||||
})
|
||||
.unwrap_or(60);
|
||||
.unwrap_or(100); // Default to 100% (auto-tuning will find optimal)
|
||||
|
||||
log::info!("Lidar enable (PWM: {}%)", pwm);
|
||||
log::info!("Lidar enable (auto-tuning from {}%)", pwm);
|
||||
|
||||
// Power on first, then set PWM
|
||||
// Power on first
|
||||
pkt.set_lidar_power(true);
|
||||
send_packet(port, pkt)?;
|
||||
pkt.set_lidar_pwm(pwm);
|
||||
send_packet(port, pkt)?;
|
||||
|
||||
// Update state for heartbeat
|
||||
// Initialize auto-tuning state and send initial PWM
|
||||
component_state.lidar_enabled.store(true, Ordering::Relaxed);
|
||||
component_state.lidar_pwm.store(pwm, Ordering::Relaxed);
|
||||
component_state.init_lidar_tuning();
|
||||
|
||||
// Send initial PWM command
|
||||
pkt.set_lidar_pwm(component_state.get_lidar_pwm());
|
||||
send_packet(port, pkt)?;
|
||||
|
||||
Ok(())
|
||||
}
|
||||
@@ -490,7 +492,6 @@ fn handle_lidar(
|
||||
component_state
|
||||
.lidar_enabled
|
||||
.store(false, Ordering::Relaxed);
|
||||
component_state.lidar_pwm.store(0, Ordering::Relaxed);
|
||||
|
||||
// PWM to 0 first, then power off
|
||||
pkt.set_lidar_pwm(0);
|
||||
@@ -498,19 +499,9 @@ fn handle_lidar(
|
||||
pkt.set_lidar_power(false);
|
||||
send_packet(port, pkt)
|
||||
}
|
||||
ComponentAction::Configure { ref config } => {
|
||||
// Handle both U8 and U32 (protobuf sends U8 as U32)
|
||||
let pwm = match config.get("pwm") {
|
||||
Some(SensorValue::U8(p)) => Some(*p),
|
||||
Some(SensorValue::U32(p)) => Some(*p as u8),
|
||||
_ => None,
|
||||
};
|
||||
if let Some(pwm) = pwm {
|
||||
log::info!("Lidar PWM: {}%", pwm);
|
||||
component_state.lidar_pwm.store(pwm, Ordering::Relaxed);
|
||||
pkt.set_lidar_pwm(pwm);
|
||||
send_packet(port, pkt)?;
|
||||
}
|
||||
ComponentAction::Configure { .. } => {
|
||||
// PWM is now auto-tuned - Configure action is no longer supported
|
||||
log::info!("Lidar PWM is auto-tuned, Configure action ignored");
|
||||
Ok(())
|
||||
}
|
||||
_ => Err(Error::NotImplemented(format!(
|
||||
|
||||
@@ -170,11 +170,13 @@ pub(super) fn heartbeat_loop(
|
||||
let _ = pkt.send_to(&mut *port);
|
||||
}
|
||||
|
||||
// Send lidar PWM if enabled
|
||||
// Send lidar PWM if enabled and auto-tuning needs it
|
||||
if lidar_enabled {
|
||||
let lidar_pwm = component_state.lidar_pwm.load(Ordering::Relaxed);
|
||||
pkt.set_lidar_pwm(lidar_pwm);
|
||||
let _ = pkt.send_to(&mut *port);
|
||||
if let Some(pwm) = component_state.update_lidar_tuning() {
|
||||
pkt.set_lidar_pwm(pwm);
|
||||
let _ = pkt.send_to(&mut *port);
|
||||
}
|
||||
// None = stable or settling, don't send PWM command
|
||||
}
|
||||
} else {
|
||||
// No components active - send regular heartbeat
|
||||
|
||||
@@ -72,7 +72,7 @@ use crate::devices::crl200s::constants::{INIT_RETRY_DELAY_MS, SERIAL_READ_TIMEOU
|
||||
use crate::error::{Error, Result};
|
||||
use packet::{initialize_packet, TxPacket};
|
||||
use serialport::SerialPort;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::sync::atomic::{AtomicBool, AtomicU64, AtomicU8, Ordering};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::thread::{self, JoinHandle};
|
||||
use std::time::Duration;
|
||||
@@ -116,6 +116,40 @@ impl GD32Driver {
|
||||
})
|
||||
}
|
||||
|
||||
/// Set shared lidar scan timestamp reference (for PWM auto-tuning)
|
||||
///
|
||||
/// This must be called before `start()` to share state with the Delta2D driver.
|
||||
/// The timestamp is updated by Delta2D when measurement scans arrive.
|
||||
pub fn set_lidar_scan_timestamp(&mut self, ts: Arc<AtomicU64>) {
|
||||
// We need to modify the Arc-wrapped ComponentState's field
|
||||
// Since ComponentState has public Arc fields, we can swap them
|
||||
// But Arc<ComponentState> means we can't mutate directly...
|
||||
// The solution is to use interior mutability - the Arc fields themselves are Arc<AtomicU64>
|
||||
// which can be cloned into place by recreating the ComponentState.
|
||||
//
|
||||
// Actually, since ComponentState fields are public and we have &mut self,
|
||||
// we need to recreate the Arc<ComponentState> with the new shared reference.
|
||||
// This is only called before start(), so no threads are using it yet.
|
||||
let mut new_state = ComponentState::default();
|
||||
new_state.lidar_last_scan_ms = ts;
|
||||
// Preserve any other state from the old one (though it should be default)
|
||||
self.component_state = Arc::new(new_state);
|
||||
}
|
||||
|
||||
/// Set shared lidar health count reference (for PWM auto-tuning settling detection)
|
||||
///
|
||||
/// This must be called before `start()` to share state with the Delta2D driver.
|
||||
/// The count is incremented by Delta2D when health packets (0xAE) arrive.
|
||||
pub fn set_lidar_health_count(&mut self, count: Arc<AtomicU8>) {
|
||||
// Similar to above - we need to update the Arc<ComponentState>
|
||||
// Get existing scan timestamp to preserve it
|
||||
let existing_ts = Arc::clone(&self.component_state.lidar_last_scan_ms);
|
||||
let mut new_state = ComponentState::default();
|
||||
new_state.lidar_last_scan_ms = existing_ts;
|
||||
new_state.lidar_health_count = count;
|
||||
self.component_state = Arc::new(new_state);
|
||||
}
|
||||
|
||||
/// Initialize the GD32 device by sending init commands
|
||||
///
|
||||
/// Version is requested by reader thread after first packet received.
|
||||
|
||||
@@ -3,7 +3,32 @@
|
||||
//! This module defines the shared state used by the heartbeat thread to refresh
|
||||
//! component commands every 20ms. All fields use atomic types to allow lockless reads.
|
||||
|
||||
use std::sync::atomic::{AtomicBool, AtomicI16, AtomicU8, Ordering};
|
||||
use std::sync::atomic::{AtomicBool, AtomicI16, AtomicI8, AtomicU64, AtomicU8, Ordering};
|
||||
use std::sync::Arc;
|
||||
use std::time::{SystemTime, UNIX_EPOCH};
|
||||
|
||||
// Lidar auto-tuning constants
|
||||
const SCAN_TIMEOUT_MS: u64 = 200; // Consider "not receiving" if no scan for 200ms
|
||||
const SETTLING_TIMEOUT_MS: u64 = 250; // Wait after PWM change before evaluating
|
||||
const SETTLING_HEALTH_COUNT: u8 = 3; // OR wait for this many health packets
|
||||
const INITIAL_STEP: u8 = 20; // Start with 20% step size
|
||||
const MIN_STEP: u8 = 2; // Converge when step reaches 2%
|
||||
const MIN_PWM: u8 = 30; // Never go below 30%
|
||||
const MAX_PWM: u8 = 100; // Never exceed 100%
|
||||
const RECOVERY_STEP: u8 = 5; // Fixed step for recovery mode
|
||||
|
||||
// Lidar tuning phases
|
||||
const PHASE_RAMPING: u8 = 0; // Adaptive stepping to find max stable PWM
|
||||
const PHASE_STABLE: u8 = 1; // Optimal found, not sending PWM
|
||||
const PHASE_RECOVERY: u8 = 2; // Lost scans, recovering
|
||||
|
||||
/// Get current time in milliseconds since epoch
|
||||
fn current_time_ms() -> u64 {
|
||||
SystemTime::now()
|
||||
.duration_since(UNIX_EPOCH)
|
||||
.map(|d| d.as_millis() as u64)
|
||||
.unwrap_or(0)
|
||||
}
|
||||
|
||||
/// Shared component state for periodic refresh
|
||||
///
|
||||
@@ -18,11 +43,17 @@ use std::sync::atomic::{AtomicBool, AtomicI16, AtomicU8, Ordering};
|
||||
/// - `water_pump`: Water pump speed for 2-in-1 mop box (0-100)
|
||||
/// - `motor_mode_set`: Whether motor mode 0x02 (navigation) is currently active
|
||||
/// - `lidar_enabled`: Whether lidar motor should be spinning
|
||||
/// - `lidar_pwm`: Lidar motor PWM percentage (0-100)
|
||||
/// - `linear_velocity`: Forward/backward velocity in mm/s (signed)
|
||||
/// - `angular_velocity`: Rotation velocity in mrad/s (signed)
|
||||
/// - `wheel_motor_enabled`: Explicit flag to keep mode 0x02 active even without motion
|
||||
#[derive(Default)]
|
||||
///
|
||||
/// ## Lidar Auto-Tuning State
|
||||
///
|
||||
/// The lidar PWM is automatically discovered using an adaptive binary-search algorithm:
|
||||
/// - Starts at 100% PWM and ramps down with 20% steps
|
||||
/// - Halves step size on each direction reversal (scan state changes)
|
||||
/// - Converges when step size reaches 2% and scans are stable
|
||||
/// - Stops sending PWM commands once stable
|
||||
pub struct ComponentState {
|
||||
pub vacuum: AtomicU8,
|
||||
pub main_brush: AtomicU8,
|
||||
@@ -30,10 +61,47 @@ pub struct ComponentState {
|
||||
pub water_pump: AtomicU8,
|
||||
pub motor_mode_set: AtomicBool,
|
||||
pub lidar_enabled: AtomicBool,
|
||||
pub lidar_pwm: AtomicU8,
|
||||
pub linear_velocity: AtomicI16,
|
||||
pub angular_velocity: AtomicI16,
|
||||
pub wheel_motor_enabled: AtomicBool,
|
||||
|
||||
// Lidar auto-tuning state
|
||||
pub lidar_current_pwm: AtomicU8,
|
||||
pub lidar_step_size: AtomicU8,
|
||||
pub lidar_direction: AtomicI8, // +1 = increasing, -1 = decreasing
|
||||
pub lidar_tuning_phase: AtomicU8,
|
||||
pub lidar_last_pwm_change_ms: AtomicU64,
|
||||
pub lidar_last_had_scans: AtomicBool,
|
||||
|
||||
// Shared with Delta2D driver (set via setters after construction)
|
||||
pub lidar_last_scan_ms: Arc<AtomicU64>,
|
||||
pub lidar_health_count: Arc<AtomicU8>,
|
||||
}
|
||||
|
||||
impl Default for ComponentState {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
vacuum: AtomicU8::new(0),
|
||||
main_brush: AtomicU8::new(0),
|
||||
side_brush: AtomicU8::new(0),
|
||||
water_pump: AtomicU8::new(0),
|
||||
motor_mode_set: AtomicBool::new(false),
|
||||
lidar_enabled: AtomicBool::new(false),
|
||||
linear_velocity: AtomicI16::new(0),
|
||||
angular_velocity: AtomicI16::new(0),
|
||||
wheel_motor_enabled: AtomicBool::new(false),
|
||||
// Lidar auto-tuning defaults
|
||||
lidar_current_pwm: AtomicU8::new(MAX_PWM),
|
||||
lidar_step_size: AtomicU8::new(INITIAL_STEP),
|
||||
lidar_direction: AtomicI8::new(-1), // Start decreasing
|
||||
lidar_tuning_phase: AtomicU8::new(PHASE_RAMPING),
|
||||
lidar_last_pwm_change_ms: AtomicU64::new(0),
|
||||
lidar_last_had_scans: AtomicBool::new(false),
|
||||
// Shared state (will be replaced via setters)
|
||||
lidar_last_scan_ms: Arc::new(AtomicU64::new(0)),
|
||||
lidar_health_count: Arc::new(AtomicU8::new(0)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl ComponentState {
|
||||
@@ -44,11 +112,17 @@ impl ComponentState {
|
||||
self.side_brush.store(0, Ordering::Relaxed);
|
||||
self.water_pump.store(0, Ordering::Relaxed);
|
||||
self.lidar_enabled.store(false, Ordering::Relaxed);
|
||||
self.lidar_pwm.store(0, Ordering::Relaxed);
|
||||
self.linear_velocity.store(0, Ordering::Relaxed);
|
||||
self.angular_velocity.store(0, Ordering::Relaxed);
|
||||
self.wheel_motor_enabled.store(false, Ordering::Relaxed);
|
||||
self.motor_mode_set.store(false, Ordering::Relaxed);
|
||||
// Reset lidar tuning state
|
||||
self.lidar_current_pwm.store(MAX_PWM, Ordering::Relaxed);
|
||||
self.lidar_step_size.store(INITIAL_STEP, Ordering::Relaxed);
|
||||
self.lidar_direction.store(-1, Ordering::Relaxed);
|
||||
self.lidar_tuning_phase.store(PHASE_RAMPING, Ordering::Relaxed);
|
||||
self.lidar_last_pwm_change_ms.store(0, Ordering::Relaxed);
|
||||
self.lidar_last_had_scans.store(false, Ordering::Relaxed);
|
||||
}
|
||||
|
||||
/// Check if any component is active (determines if motor mode 0x02 is needed)
|
||||
@@ -78,4 +152,140 @@ impl ComponentState {
|
||||
self.water_pump.load(Ordering::Relaxed),
|
||||
)
|
||||
}
|
||||
|
||||
/// Initialize lidar auto-tuning state for a new enable cycle
|
||||
pub fn init_lidar_tuning(&self) {
|
||||
let now = current_time_ms();
|
||||
self.lidar_current_pwm.store(MAX_PWM, Ordering::Relaxed);
|
||||
self.lidar_step_size.store(INITIAL_STEP, Ordering::Relaxed);
|
||||
self.lidar_direction.store(-1, Ordering::Relaxed); // Start decreasing
|
||||
self.lidar_tuning_phase.store(PHASE_RAMPING, Ordering::Relaxed);
|
||||
self.lidar_last_pwm_change_ms.store(now, Ordering::Relaxed);
|
||||
self.lidar_last_had_scans.store(false, Ordering::Relaxed);
|
||||
self.lidar_health_count.store(0, Ordering::Relaxed);
|
||||
}
|
||||
|
||||
/// Update lidar PWM tuning state machine.
|
||||
///
|
||||
/// Returns `Some(pwm)` if a PWM command should be sent, `None` if stable/settling.
|
||||
///
|
||||
/// This implements an adaptive binary-search algorithm:
|
||||
/// - Starts at 100% PWM and ramps down with large steps (20%)
|
||||
/// - Halves step size on each direction reversal (when scan state changes)
|
||||
/// - Converges when step size reaches MIN_STEP and scans are stable
|
||||
/// - Waits for settling period (250ms or 3 health packets) after each PWM change
|
||||
pub fn update_lidar_tuning(&self) -> Option<u8> {
|
||||
let now = current_time_ms();
|
||||
|
||||
// Check if still settling after PWM change
|
||||
let time_since_change = now.saturating_sub(self.lidar_last_pwm_change_ms.load(Ordering::Relaxed));
|
||||
let health_packets = self.lidar_health_count.load(Ordering::Relaxed);
|
||||
let is_settling =
|
||||
time_since_change < SETTLING_TIMEOUT_MS && health_packets < SETTLING_HEALTH_COUNT;
|
||||
|
||||
if is_settling {
|
||||
return None; // Don't change PWM while settling
|
||||
}
|
||||
|
||||
// Evaluate scan success (only after settling)
|
||||
let last_scan = self.lidar_last_scan_ms.load(Ordering::Relaxed);
|
||||
let has_scans = now.saturating_sub(last_scan) < SCAN_TIMEOUT_MS;
|
||||
|
||||
let phase = self.lidar_tuning_phase.load(Ordering::Relaxed);
|
||||
|
||||
match phase {
|
||||
PHASE_RAMPING => {
|
||||
let prev_had_scans = self.lidar_last_had_scans.swap(has_scans, Ordering::Relaxed);
|
||||
|
||||
// Detect direction change (scan state flipped)
|
||||
if has_scans != prev_had_scans {
|
||||
// Direction reversal - halve step size
|
||||
let mut step = self.lidar_step_size.load(Ordering::Relaxed);
|
||||
step = (step / 2).max(MIN_STEP);
|
||||
self.lidar_step_size.store(step, Ordering::Relaxed);
|
||||
|
||||
// Flip direction
|
||||
let dir = self.lidar_direction.load(Ordering::Relaxed);
|
||||
self.lidar_direction.store(-dir, Ordering::Relaxed);
|
||||
|
||||
let pwm = self.lidar_current_pwm.load(Ordering::Relaxed);
|
||||
log::info!(
|
||||
"Lidar PWM direction reversal at {}%, new step={}%",
|
||||
pwm,
|
||||
step
|
||||
);
|
||||
}
|
||||
|
||||
let step = self.lidar_step_size.load(Ordering::Relaxed);
|
||||
|
||||
// Check if converged (small step + stable scans)
|
||||
if step <= MIN_STEP && has_scans {
|
||||
self.lidar_tuning_phase.store(PHASE_STABLE, Ordering::Relaxed);
|
||||
let pwm = self.lidar_current_pwm.load(Ordering::Relaxed);
|
||||
log::info!("Lidar stable at PWM {}%", pwm);
|
||||
return None; // Stop sending PWM
|
||||
}
|
||||
|
||||
// Apply step in current direction
|
||||
let dir = self.lidar_direction.load(Ordering::Relaxed);
|
||||
let current_pwm = self.lidar_current_pwm.load(Ordering::Relaxed);
|
||||
let new_pwm = if dir < 0 {
|
||||
current_pwm.saturating_sub(step).max(MIN_PWM)
|
||||
} else {
|
||||
current_pwm.saturating_add(step).min(MAX_PWM)
|
||||
};
|
||||
|
||||
self.lidar_current_pwm.store(new_pwm, Ordering::Relaxed);
|
||||
self.lidar_last_pwm_change_ms.store(now, Ordering::Relaxed);
|
||||
self.lidar_health_count.store(0, Ordering::Relaxed);
|
||||
|
||||
log::debug!(
|
||||
"Lidar PWM: {} -> {}% (step={}%, dir={})",
|
||||
current_pwm,
|
||||
new_pwm,
|
||||
step,
|
||||
if dir < 0 { "down" } else { "up" }
|
||||
);
|
||||
|
||||
Some(new_pwm)
|
||||
}
|
||||
PHASE_STABLE => {
|
||||
if has_scans {
|
||||
None // All good, don't send PWM
|
||||
} else {
|
||||
// Lost scans unexpectedly
|
||||
self.lidar_tuning_phase.store(PHASE_RECOVERY, Ordering::Relaxed);
|
||||
let pwm = self.lidar_current_pwm.load(Ordering::Relaxed);
|
||||
log::warn!("Lidar lost scans at PWM {}%, entering recovery", pwm);
|
||||
self.lidar_last_pwm_change_ms.store(now, Ordering::Relaxed);
|
||||
self.lidar_health_count.store(0, Ordering::Relaxed);
|
||||
Some(pwm)
|
||||
}
|
||||
}
|
||||
PHASE_RECOVERY => {
|
||||
if has_scans {
|
||||
// Recovered! Return to stable
|
||||
self.lidar_tuning_phase.store(PHASE_STABLE, Ordering::Relaxed);
|
||||
let pwm = self.lidar_current_pwm.load(Ordering::Relaxed);
|
||||
log::info!("Lidar recovered at PWM {}%", pwm);
|
||||
None
|
||||
} else {
|
||||
// Still no scans, increase PWM
|
||||
let current_pwm = self.lidar_current_pwm.load(Ordering::Relaxed);
|
||||
let new_pwm = current_pwm.saturating_add(RECOVERY_STEP).min(MAX_PWM);
|
||||
self.lidar_current_pwm.store(new_pwm, Ordering::Relaxed);
|
||||
self.lidar_last_pwm_change_ms.store(now, Ordering::Relaxed);
|
||||
self.lidar_health_count.store(0, Ordering::Relaxed);
|
||||
log::debug!("Lidar recovery: {} -> {}%", current_pwm, new_pwm);
|
||||
Some(new_pwm)
|
||||
}
|
||||
}
|
||||
_ => None,
|
||||
}
|
||||
}
|
||||
|
||||
/// Get current lidar PWM value (for initial enable command)
|
||||
pub fn get_lidar_pwm(&self) -> u8 {
|
||||
self.lidar_current_pwm.load(Ordering::Relaxed)
|
||||
}
|
||||
}
|
||||
|
||||
@@ -36,6 +36,7 @@ use crate::error::Result;
|
||||
use delta2d::Delta2DDriver;
|
||||
use gd32::GD32Driver;
|
||||
use std::collections::HashMap;
|
||||
use std::sync::atomic::{AtomicU64, AtomicU8};
|
||||
use std::sync::{Arc, Mutex};
|
||||
|
||||
/// CRL-200S device driver coordinating GD32 motor controller and Delta2D lidar.
|
||||
@@ -103,12 +104,21 @@ impl DeviceDriver for CRL200SDriver {
|
||||
sensor_data.insert("lidar".to_string(), lidar_data.clone());
|
||||
log::info!("Created sensor group 'lidar' (360° point cloud @ 5Hz)");
|
||||
|
||||
// Create shared state for lidar PWM auto-tuning
|
||||
// These are shared between GD32 (for PWM decisions) and Delta2D (updates on scan/health)
|
||||
let lidar_scan_timestamp = Arc::new(AtomicU64::new(0));
|
||||
let lidar_health_count = Arc::new(AtomicU8::new(0));
|
||||
|
||||
// Initialize GD32 motor controller
|
||||
let mut gd32 = GD32Driver::new(
|
||||
&self.config.hardware.gd32_port,
|
||||
self.config.hardware.heartbeat_interval_ms,
|
||||
)?;
|
||||
|
||||
// Set shared lidar state references before initialization
|
||||
gd32.set_lidar_scan_timestamp(Arc::clone(&lidar_scan_timestamp));
|
||||
gd32.set_lidar_health_count(Arc::clone(&lidar_health_count));
|
||||
|
||||
// Send initialization sequence
|
||||
gd32.initialize()?;
|
||||
|
||||
@@ -117,12 +127,16 @@ impl DeviceDriver for CRL200SDriver {
|
||||
|
||||
self.gd32 = Some(gd32);
|
||||
|
||||
// Initialize lidar driver
|
||||
// Initialize lidar driver with shared state
|
||||
// Driver starts but lidar motor is OFF - will be enabled via command
|
||||
let mut lidar = Delta2DDriver::new(&self.config.hardware.lidar_port);
|
||||
let mut lidar = Delta2DDriver::new(
|
||||
&self.config.hardware.lidar_port,
|
||||
Arc::clone(&lidar_scan_timestamp),
|
||||
Arc::clone(&lidar_health_count),
|
||||
);
|
||||
lidar.start(lidar_data)?;
|
||||
self.lidar = Some(lidar);
|
||||
log::info!("Lidar driver started (motor OFF - use ComponentControl to enable)");
|
||||
log::info!("Lidar driver started (motor OFF - PWM auto-tuned when enabled)");
|
||||
|
||||
log::info!("CRL-200S device initialized");
|
||||
Ok(DriverInitResult {
|
||||
|
||||
Reference in New Issue
Block a user