Add comprehensive documentation and optimize protobuf serialization
Improve code documentation across SangamIO for better maintainability: - Add detailed proto comments explaining field semantics and valid values - Document component IDs and action types in commands.rs - Expand config comments in sangamio.toml and build.rs - Add thread model docs in reader.rs and types.rs Performance optimization: - Add from_ref() method to avoid cloning primitive SensorValues in hot path - Pre-allocate HashMap in sensor serialization (~500Hz path) - Remove unused raw_packet debug output from status handler
This commit is contained in:
codetiger
@@ -3,3 +3,5 @@ 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"]
|
||||
|
||||
@@ -5,12 +5,6 @@ edition = "2021"
|
||||
description = "Hardware abstraction daemon for robot vacuum"
|
||||
authors = ["CodeTiger"]
|
||||
|
||||
[features]
|
||||
default = []
|
||||
# Enable raw packet capture in sensor data (adds ~48KB/sec allocations at 500Hz)
|
||||
# Useful for debugging and reverse engineering, disable for production
|
||||
protocol_debug = []
|
||||
|
||||
[dependencies]
|
||||
# Serialization
|
||||
serde = { version = "1.0", features = ["derive"] }
|
||||
|
||||
+3
-3
@@ -48,7 +48,7 @@ cargo build --release
|
||||
cargo run --release
|
||||
|
||||
# Run with custom config
|
||||
cargo run --release -- my_config.json
|
||||
cargo run --release -- sangamio.toml
|
||||
|
||||
# Enable verbose logging
|
||||
RUST_LOG=debug cargo run --release
|
||||
@@ -81,7 +81,7 @@ cat target/armv7-unknown-linux-musleabihf/release/sangam-io | \
|
||||
ssh root@vacuum "cat > /usr/sbin/sangamio && chmod +x /usr/sbin/sangamio"
|
||||
|
||||
# Deploy configuration
|
||||
ssh root@vacuum "cat > /etc/hardware.json" < hardware.json
|
||||
ssh root@vacuum "cat > /etc/sangamio.toml" < sangamio.toml
|
||||
|
||||
# Disable original firmware (rename to prevent auto-restart)
|
||||
ssh root@vacuum "mv /usr/sbin/AuxCtrl /usr/sbin/AuxCtrl.bak && killall -9 AuxCtrl"
|
||||
@@ -174,7 +174,7 @@ while True:
|
||||
sangam-io/
|
||||
├── src/
|
||||
│ ├── main.rs # Entry point, TCP listener
|
||||
│ ├── config.rs # JSON configuration loading
|
||||
│ ├── config.rs # TOML configuration loading
|
||||
│ ├── error.rs # Error types
|
||||
│ │
|
||||
│ ├── core/
|
||||
|
||||
+30
-4
@@ -1,14 +1,40 @@
|
||||
//! Build script for compiling protobuf definitions
|
||||
//! Build script for SangamIO
|
||||
//!
|
||||
//! This script runs at compile time to generate Rust code from protobuf definitions.
|
||||
//!
|
||||
//! # What it does
|
||||
//!
|
||||
//! 1. Compiles `proto/sangamio.proto` using prost-build
|
||||
//! 2. Generates Rust structs and enums in `target/*/build/sangam-io-*/out/`
|
||||
//! 3. Adds clippy suppression for generated code (avoids enum variant name warnings)
|
||||
//!
|
||||
//! # Generated Code Location
|
||||
//!
|
||||
//! The generated code is included in `src/streaming/wire.rs` via:
|
||||
//! ```ignore
|
||||
//! pub mod proto {
|
||||
//! include!(concat!(env!("OUT_DIR"), "/sangamio.rs"));
|
||||
//! }
|
||||
//! ```
|
||||
//!
|
||||
//! # Rebuilding
|
||||
//!
|
||||
//! The script automatically reruns when `proto/sangamio.proto` changes.
|
||||
//! To force regeneration, run `cargo clean` then `cargo build`.
|
||||
|
||||
fn main() -> Result<(), Box<dyn std::error::Error>> {
|
||||
// Configure prost-build to suppress clippy warnings on generated code
|
||||
// Configure prost-build to suppress clippy warnings on generated code.
|
||||
// The generated enum variants sometimes trigger clippy::enum_variant_names
|
||||
// due to naming patterns like SensorValue::BoolVal, SensorValue::U32Val, etc.
|
||||
let mut config = prost_build::Config::new();
|
||||
config.type_attribute(".", "#[allow(clippy::enum_variant_names)]");
|
||||
|
||||
// Compile proto files
|
||||
// Compile proto files into Rust code.
|
||||
// Output goes to $OUT_DIR/sangamio.rs
|
||||
config.compile_protos(&["proto/sangamio.proto"], &["proto/"])?;
|
||||
|
||||
// Rerun if proto files change
|
||||
// Tell Cargo to rerun this script if proto files change.
|
||||
// This ensures generated code stays in sync with schema changes.
|
||||
println!("cargo:rerun-if-changed=proto/sangamio.proto");
|
||||
|
||||
Ok(())
|
||||
|
||||
@@ -1,83 +1,114 @@
|
||||
syntax = "proto3";
|
||||
package sangamio;
|
||||
|
||||
// Top-level message wrapper for all SangamIO TCP communication
|
||||
// Top-level message wrapper for all SangamIO TCP communication.
|
||||
// Every message sent over TCP (inbound or outbound) is wrapped in this type.
|
||||
message Message {
|
||||
// Topic identifier for routing messages.
|
||||
// Outbound topics: "sensors/sensor_status", "sensors/device_version", "sensors/lidar"
|
||||
// Inbound topics: "command"
|
||||
string topic = 1;
|
||||
oneof payload {
|
||||
SensorGroup sensor_group = 2;
|
||||
Command command = 3;
|
||||
SensorGroup sensor_group = 2; // Outbound sensor data
|
||||
Command command = 3; // Inbound commands
|
||||
}
|
||||
}
|
||||
|
||||
// Sensor data from robot (outbound @ 500Hz for telemetry, 5Hz for lidar)
|
||||
// Sensor data container published by the daemon.
|
||||
// Streaming rates: 500Hz for telemetry, 5Hz for lidar scans.
|
||||
message SensorGroup {
|
||||
// Group identifier matching topic suffix (e.g., "sensor_status", "lidar")
|
||||
string group_id = 1;
|
||||
// Microseconds since Unix epoch (1970-01-01 00:00:00 UTC).
|
||||
// Resolution: 1 microsecond. Updated each time sensor data is captured.
|
||||
uint64 timestamp_us = 2;
|
||||
// Key-value pairs of sensor readings. Keys are sensor names (e.g., "bumper_left", "gyro_z").
|
||||
map<string, SensorValue> values = 3;
|
||||
}
|
||||
|
||||
// Tagged union for sensor values - matches Rust SensorValue enum
|
||||
// Tagged union for sensor values - matches Rust SensorValue enum.
|
||||
// Each variant maps to a specific hardware data type.
|
||||
message SensorValue {
|
||||
oneof value {
|
||||
bool bool_val = 1;
|
||||
uint32 u32_val = 2;
|
||||
uint64 u64_val = 3;
|
||||
int32 i32_val = 4;
|
||||
int64 i64_val = 5;
|
||||
float f32_val = 6;
|
||||
double f64_val = 7;
|
||||
string string_val = 8;
|
||||
bytes bytes_val = 9;
|
||||
Vector3 vector3_val = 10;
|
||||
PointCloud2D pointcloud_val = 11;
|
||||
bool bool_val = 1; // Binary sensors (bumpers, cliffs, buttons)
|
||||
uint32 u32_val = 2; // Note: Protobuf encodes u8/u16 as u32
|
||||
uint64 u64_val = 3; // Large counters, timestamps
|
||||
int32 i32_val = 4; // Note: Protobuf encodes i8/i16 as i32
|
||||
int64 i64_val = 5; // Large signed values
|
||||
float f32_val = 6; // Analog sensors (voltage, velocity)
|
||||
double f64_val = 7; // High-precision values
|
||||
string string_val = 8; // Version strings, identifiers
|
||||
bytes bytes_val = 9; // Raw binary data
|
||||
Vector3 vector3_val = 10; // 3D vectors (IMU data)
|
||||
PointCloud2D pointcloud_val = 11; // Lidar scans
|
||||
}
|
||||
}
|
||||
|
||||
// 3D vector for IMU data (gyroscope, accelerometer, tilt)
|
||||
message Vector3 {
|
||||
float x = 1;
|
||||
float y = 2;
|
||||
float z = 3;
|
||||
float x = 1; // X-axis component
|
||||
float y = 2; // Y-axis component
|
||||
float z = 3; // Z-axis component
|
||||
}
|
||||
|
||||
// LiDAR point cloud data
|
||||
// LiDAR point cloud data - one complete 360-degree scan
|
||||
message PointCloud2D {
|
||||
// Points in the scan, ordered by angle (0 to 2*pi)
|
||||
repeated LidarPoint points = 1;
|
||||
}
|
||||
|
||||
// Single LiDAR measurement point
|
||||
message LidarPoint {
|
||||
float angle_rad = 1;
|
||||
float distance_m = 2;
|
||||
uint32 quality = 3;
|
||||
float angle_rad = 1; // Angle in radians (0 = front, CW positive)
|
||||
float distance_m = 2; // Distance in meters (0 = no return / invalid)
|
||||
uint32 quality = 3; // Signal quality/intensity (0-255, higher = stronger)
|
||||
}
|
||||
|
||||
// Commands to robot (inbound)
|
||||
// Commands sent to the robot daemon (inbound from clients)
|
||||
message Command {
|
||||
oneof command {
|
||||
ComponentControl component_control = 1;
|
||||
ProtocolSync protocol_sync = 2;
|
||||
Shutdown shutdown = 3;
|
||||
ComponentControl component_control = 1; // Control components (most common)
|
||||
ProtocolSync protocol_sync = 2; // Wake/sync GD32 MCU
|
||||
Shutdown shutdown = 3; // Graceful daemon shutdown
|
||||
}
|
||||
}
|
||||
|
||||
// Empty messages for simple commands
|
||||
// Protocol sync command - wakes GD32 and synchronizes communication.
|
||||
// Send once at startup before other commands. GD32 echoes back after ~270ms.
|
||||
message ProtocolSync {}
|
||||
|
||||
// Graceful shutdown command - stops all components and exits daemon.
|
||||
message Shutdown {}
|
||||
|
||||
// Component control command (motors, actuators, sensors)
|
||||
// Unified component control command for all sensors and actuators.
|
||||
// See COMMANDS.md for complete command reference.
|
||||
message ComponentControl {
|
||||
string id = 1; // "drive", "vacuum", "lidar", "main_brush", "side_brush", etc.
|
||||
// Component identifier. Valid values:
|
||||
// Motion: "drive"
|
||||
// Actuators: "vacuum", "main_brush", "side_brush", "water_pump", "led", "lidar"
|
||||
// Sensors: "imu", "compass", "cliff_ir"
|
||||
// Power: "main_board", "charger", "mcu"
|
||||
string id = 1;
|
||||
// Action to perform on the component
|
||||
ComponentAction action = 2;
|
||||
}
|
||||
|
||||
// Action specification for ComponentControl commands
|
||||
message ComponentAction {
|
||||
ActionType type = 1;
|
||||
map<string, SensorValue> config = 2; // Optional configuration parameters
|
||||
// Configuration parameters (key-value pairs).
|
||||
// Common keys by component:
|
||||
// - drive: "linear" (F32 m/s), "angular" (F32 rad/s), "left"/"right" (F32 m/s for tank)
|
||||
// - vacuum/brush: "speed" (U8 0-100%)
|
||||
// - led: "state" (U8 0-18)
|
||||
// - lidar: "pwm" (U8 0-100%)
|
||||
// Note: Protobuf encodes U8 values as U32 - daemon handles both types.
|
||||
map<string, SensorValue> config = 2;
|
||||
|
||||
enum ActionType {
|
||||
ENABLE = 0;
|
||||
DISABLE = 1;
|
||||
RESET = 2;
|
||||
CONFIGURE = 3;
|
||||
ENABLE = 0; // Turn on/activate component
|
||||
DISABLE = 1; // Turn off/deactivate component
|
||||
RESET = 2; // Reset to defaults / emergency stop (drive) / calibrate (sensors)
|
||||
CONFIGURE = 3; // Set parameters without changing enable state
|
||||
}
|
||||
}
|
||||
|
||||
@@ -1,14 +1,37 @@
|
||||
# SangamIO Configuration
|
||||
# Hardware abstraction daemon for CRL-200S robot vacuum
|
||||
#
|
||||
# This file configures the SangamIO daemon that runs on the robot.
|
||||
# Deploy to /etc/sangamio.toml on the target device.
|
||||
|
||||
[device]
|
||||
# Device type - determines which driver to load
|
||||
# Currently supported: "crl200s"
|
||||
type = "crl200s"
|
||||
|
||||
# Human-readable device name (for logging)
|
||||
name = "CRL-200S Vacuum Robot"
|
||||
|
||||
[device.hardware]
|
||||
# Serial port for GD32F103 motor controller
|
||||
# Default on CRL-200S: /dev/ttyS3
|
||||
# Baud rate: 115200 (fixed in firmware)
|
||||
gd32_port = "/dev/ttyS3"
|
||||
|
||||
# Serial port for Delta-2D lidar sensor
|
||||
# Default on CRL-200S: /dev/ttyS1
|
||||
# Baud rate: 115200 (fixed in firmware)
|
||||
lidar_port = "/dev/ttyS1"
|
||||
|
||||
# GD32 heartbeat interval in milliseconds
|
||||
# CRITICAL: Must be 20-50ms or motors will stop (hardware watchdog)
|
||||
# Recommended: 20 (provides safety margin)
|
||||
# See src/devices/crl200s/gd32/heartbeat.rs for details
|
||||
heartbeat_interval_ms = 20
|
||||
|
||||
[network]
|
||||
# TCP server bind address for client connections
|
||||
# Format: "host:port"
|
||||
# Use "0.0.0.0:5555" to listen on all interfaces
|
||||
# Clients connect here to receive sensor streams and send commands
|
||||
bind_address = "0.0.0.0:5555"
|
||||
|
||||
+24
-12
@@ -129,28 +129,40 @@ pub enum ComponentAction {
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
pub enum Command {
|
||||
// Unified Component Control
|
||||
/// Control any component (sensor or actuator) with standard actions
|
||||
/// Control any component (sensor or actuator) with standard actions.
|
||||
///
|
||||
/// Component IDs are defined in [`crate::devices::crl200s::gd32::commands`] module.
|
||||
/// See also `proto/sangamio.proto` for the protobuf schema.
|
||||
///
|
||||
/// # Motion Control (id: "drive")
|
||||
/// - `Configure { linear: F32, angular: F32 }` - velocity mode (m/s, rad/s)
|
||||
/// - `Configure { left: F32, right: F32 }` - tank drive mode (m/s)
|
||||
/// - `Disable` - stop (zero velocity)
|
||||
/// - `Enable { mode: U8 }` - enter motor mode (default 0x02 = navigation)
|
||||
/// - `Disable` - stop (zero velocity) and exit motor mode
|
||||
/// - `Reset` - emergency stop (immediate halt, all actuators off)
|
||||
///
|
||||
/// # Actuators
|
||||
/// - `wheel_motor`: Enable/Disable motor mode
|
||||
/// - `vacuum`, `main_brush`, `side_brush`: Enable/Disable/Configure(speed)
|
||||
/// - `led`: Configure(state)
|
||||
/// - `lidar`: Enable/Disable
|
||||
/// # Actuators (speed 0-100%)
|
||||
/// - `vacuum`: Suction motor - Enable/Disable/Configure(speed)
|
||||
/// - `main_brush`: Main brush roller - Enable/Disable/Configure(speed)
|
||||
/// - `side_brush`: Side brush spinner - Enable/Disable/Configure(speed)
|
||||
/// - `water_pump`: Mop water pump - Enable/Disable/Configure(speed)
|
||||
/// - `led`: Status LED - Configure(state: 0-18)
|
||||
/// - `lidar`: Lidar motor - Enable(pwm)/Disable/Configure(pwm)
|
||||
///
|
||||
/// # Sensors
|
||||
/// - `imu`: Enable (query state), Reset (factory calibrate)
|
||||
/// - `compass`: Enable (query state), Reset (start calibration)
|
||||
/// - `cliff_ir`: Enable/Disable/Configure(direction)
|
||||
/// - `imu`: Enable (query calibration state), Reset (factory calibrate)
|
||||
/// - `compass`: Enable (query calibration state), Reset (start calibration)
|
||||
/// - `cliff_ir`: Enable/Disable IR emitters, Configure(direction)
|
||||
///
|
||||
/// # Power Management
|
||||
/// - `main_board`: A33 power - Enable/Disable/Reset (WARNING: terminates daemon!)
|
||||
/// - `charger`: Charger rail - Enable/Disable
|
||||
/// - `mcu`: GD32 MCU - Enable (wake ack), Disable (sleep), Reset (clear errors)
|
||||
ComponentControl {
|
||||
/// Component identifier (e.g., "drive", "vacuum", "imu", "cliff_ir")
|
||||
/// Component identifier (e.g., "drive", "vacuum", "imu", "cliff_ir").
|
||||
/// See module docs for complete list of valid IDs.
|
||||
id: String,
|
||||
/// Action to perform
|
||||
/// Action to perform on the component
|
||||
action: ComponentAction,
|
||||
},
|
||||
|
||||
|
||||
@@ -22,6 +22,20 @@
|
||||
//! - Conversion: multiply by 1000
|
||||
//!
|
||||
//! These conversions are defined by the GD32F103 firmware protocol.
|
||||
//!
|
||||
//! # Protobuf Type Handling
|
||||
//!
|
||||
//! Protobuf3 does not have native u8/u16/i8/i16 types. All small integers are
|
||||
//! encoded as u32/i32. This module handles both representations:
|
||||
//!
|
||||
//! - **U8 values** (speed, pwm, state): Accept both `SensorValue::U8` and `SensorValue::U32`
|
||||
//! - Clients may send either depending on their protobuf implementation
|
||||
//! - Example: `config.get("speed")` checks for both U8 and U32 variants
|
||||
//!
|
||||
//! # Component IDs
|
||||
//!
|
||||
//! Valid component IDs are defined as constants below. See [`handle_component_control`]
|
||||
//! for the complete dispatch table.
|
||||
|
||||
use super::packet::{protocol_sync_packet, TxPacket};
|
||||
use super::state::ComponentState;
|
||||
@@ -41,19 +55,38 @@ const VELOCITY_TO_DEVICE_UNITS: f32 = 1000.0;
|
||||
/// Default IMU calibration payload observed in R2D logs
|
||||
const IMU_DEFAULT_PAYLOAD: [u8; 4] = [0x10, 0x0E, 0x00, 0x00];
|
||||
|
||||
// Component IDs - use these instead of string literals to catch typos at compile time
|
||||
// ============================================================================
|
||||
// Component IDs
|
||||
// ============================================================================
|
||||
// Use these constants instead of string literals to catch typos at compile time.
|
||||
// These must match the IDs used in the protobuf `ComponentControl.id` field.
|
||||
// See also: proto/sangamio.proto ComponentControl message documentation.
|
||||
|
||||
/// Motion control - velocity mode or tank drive
|
||||
const ID_DRIVE: &str = "drive";
|
||||
/// Vacuum suction motor (0-100%)
|
||||
const ID_VACUUM: &str = "vacuum";
|
||||
/// Main brush roller (0-100%)
|
||||
const ID_MAIN_BRUSH: &str = "main_brush";
|
||||
/// Side brush spinner (0-100%)
|
||||
const ID_SIDE_BRUSH: &str = "side_brush";
|
||||
/// Mopping water pump (0-100%)
|
||||
const ID_WATER_PUMP: &str = "water_pump";
|
||||
/// Status LED patterns (0-18)
|
||||
const ID_LED: &str = "led";
|
||||
/// Lidar motor power and PWM
|
||||
const ID_LIDAR: &str = "lidar";
|
||||
/// IMU calibration queries and resets
|
||||
const ID_IMU: &str = "imu";
|
||||
/// Compass/magnetometer calibration
|
||||
const ID_COMPASS: &str = "compass";
|
||||
/// Cliff IR sensor enable/direction
|
||||
const ID_CLIFF_IR: &str = "cliff_ir";
|
||||
/// A33 main board power control (WARNING: affects daemon!)
|
||||
const ID_MAIN_BOARD: &str = "main_board";
|
||||
/// Charger power rail control
|
||||
const ID_CHARGER: &str = "charger";
|
||||
/// GD32 MCU sleep/wake/error reset
|
||||
const ID_MCU: &str = "mcu";
|
||||
|
||||
// ============================================================================
|
||||
|
||||
@@ -89,7 +89,10 @@ pub(super) fn heartbeat_loop(
|
||||
let mut pkt = TxPacket::new(); // For variable commands (velocity, actuators)
|
||||
|
||||
// Counter for periodic STM32 data request (0x0D)
|
||||
// At 20ms interval, 75 cycles = 1.5 seconds (matching R2D MITM log frequency)
|
||||
// The stock R2D firmware sends 0x0D requests approximately every 1.5 seconds,
|
||||
// as observed in MITM captures (see COMMANDS.md "Request STM32 Data" entry).
|
||||
// This appears to be a keep-alive/diagnostic query - response contents are TBD.
|
||||
// At 20ms heartbeat interval: 1500ms / 20ms = 75 cycles between requests.
|
||||
let stm32_request_interval = 1500 / interval_ms;
|
||||
let mut stm32_request_counter: u64 = 0;
|
||||
|
||||
|
||||
@@ -2,6 +2,12 @@
|
||||
//!
|
||||
//! This module contains the reader loop that parses incoming packets from the GD32
|
||||
//! and updates sensor data in real-time.
|
||||
//!
|
||||
//! # Thread Model
|
||||
//!
|
||||
//! The reader runs on a dedicated OS thread, continuously polling the serial port.
|
||||
//! It shares the port mutex with the heartbeat thread but minimizes lock hold time
|
||||
//! (~100μs per packet parse).
|
||||
|
||||
use super::packet::version_request_packet;
|
||||
use super::protocol::{PacketReader, RxPacket};
|
||||
@@ -33,13 +39,19 @@ use std::time::Duration;
|
||||
///
|
||||
/// # Version Request Flow
|
||||
///
|
||||
/// 1. Wait for first packet of any type (indicates GD32 is awake)
|
||||
/// 2. Send version request (CMD=0x42)
|
||||
/// 1. Wait for first packet of any type (indicates GD32 is awake and initialized)
|
||||
/// 2. Send version request (CMD=0x42) - sent exactly once, not retried on failure
|
||||
/// 3. Parse version response payload (string + i32 code)
|
||||
/// 4. Set `version_received` flag to prevent repeated requests
|
||||
///
|
||||
/// This approach (from sangam-io2-backup) ensures we don't request version before
|
||||
/// the GD32 is ready, which would result in lost commands.
|
||||
/// **Why wait for first packet?** The GD32 requires its ~5 second wake/init sequence
|
||||
/// before it will respond to commands. Sending version request too early results in
|
||||
/// the command being silently dropped. By waiting for the first status packet (0x15),
|
||||
/// we know the GD32 is ready.
|
||||
///
|
||||
/// **Failure handling:** If the version request fails to send (e.g., serial error),
|
||||
/// it is NOT retried. The daemon will continue without version info. This is
|
||||
/// acceptable because version info is diagnostic only.
|
||||
///
|
||||
/// # Data Updates
|
||||
///
|
||||
@@ -345,7 +357,4 @@ fn handle_status_packet(packet: &RxPacket, sensor_data: &Arc<Mutex<SensorGroupDa
|
||||
payload[OFFSET_TILT_Z + 1],
|
||||
])),
|
||||
);
|
||||
|
||||
// Raw packet bytes for debugging/reverse engineering
|
||||
data.set("raw_packet", SensorValue::Bytes(payload.to_vec()));
|
||||
}
|
||||
|
||||
@@ -5,9 +5,25 @@
|
||||
//! - **Delta2D**: Lidar sensor via `/dev/ttyS1`
|
||||
//!
|
||||
//! # Sensor Groups Published
|
||||
//! - `sensor_status`: Bumpers, cliffs, encoders, IMU @ 500Hz
|
||||
//! - `device_version`: Firmware version (one-time)
|
||||
//! - `lidar`: Point cloud @ 5Hz
|
||||
//!
|
||||
//! ## `sensor_status` (500Hz from GD32)
|
||||
//! Published on topic `sensors/sensor_status`. Contains all real-time sensor data:
|
||||
//! - **Bumpers**: `bumper_left`, `bumper_right` (Bool)
|
||||
//! - **Cliffs**: `cliff_left_side`, `cliff_left_front`, `cliff_right_front`, `cliff_right_side` (Bool)
|
||||
//! - **Battery**: `battery_voltage` (F32 volts), `battery_level` (U8 %), `is_charging` (Bool)
|
||||
//! - **Encoders**: `wheel_left`, `wheel_right` (U16 ticks)
|
||||
//! - **IMU**: `gyro_x/y/z`, `accel_x/y/z`, `tilt_x/y/z` (I16 raw)
|
||||
//! - **Buttons**: `start_button`, `dock_button` (U16)
|
||||
//! - **Misc**: `dustbox_attached`, `is_dock_connected` (Bool)
|
||||
//!
|
||||
//! ## `device_version` (one-time from GD32)
|
||||
//! Published on topic `sensors/device_version` after first GD32 packet:
|
||||
//! - `version_string`: GD32 firmware version (String)
|
||||
//! - `version_code`: Numeric version code (I32)
|
||||
//!
|
||||
//! ## `lidar` (5Hz from Delta2D)
|
||||
//! Published on topic `sensors/lidar`. Contains 360-degree scan:
|
||||
//! - `scan`: PointCloud2D with (angle_rad, distance_m, quality) tuples
|
||||
|
||||
pub mod constants;
|
||||
pub mod delta2d;
|
||||
@@ -60,23 +76,27 @@ impl DeviceDriver for CRL200SDriver {
|
||||
|
||||
let mut sensor_data = HashMap::new();
|
||||
|
||||
// Create GD32 status sensor group
|
||||
// Create GD32 status sensor group (500Hz telemetry)
|
||||
// Contains: bumpers, cliffs, battery, encoders, IMU, buttons
|
||||
// See module docs for complete field list
|
||||
let sensor_status = SensorGroupData::new("sensor_status");
|
||||
let gd32_data = Arc::new(Mutex::new(sensor_status));
|
||||
sensor_data.insert("sensor_status".to_string(), gd32_data.clone());
|
||||
log::info!("Created sensor group 'sensor_status'");
|
||||
log::info!("Created sensor group 'sensor_status' (bumpers, cliffs, IMU @ 500Hz)");
|
||||
|
||||
// Create GD32 version sensor group
|
||||
// Create GD32 version sensor group (one-time after boot)
|
||||
// Contains: version_string, version_code
|
||||
let device_version = SensorGroupData::new("device_version");
|
||||
let version_data = Arc::new(Mutex::new(device_version));
|
||||
sensor_data.insert("device_version".to_string(), version_data.clone());
|
||||
log::info!("Created sensor group 'device_version'");
|
||||
log::info!("Created sensor group 'device_version' (GD32 firmware version)");
|
||||
|
||||
// Create lidar sensor group
|
||||
// Create lidar sensor group (5Hz scan data)
|
||||
// Contains: scan (PointCloud2D)
|
||||
let lidar_group = SensorGroupData::new("lidar");
|
||||
let lidar_data = Arc::new(Mutex::new(lidar_group));
|
||||
sensor_data.insert("lidar".to_string(), lidar_data.clone());
|
||||
log::info!("Created sensor group 'lidar'");
|
||||
log::info!("Created sensor group 'lidar' (360° point cloud @ 5Hz)");
|
||||
|
||||
// Initialize GD32 motor controller
|
||||
let mut gd32 = GD32Driver::new(
|
||||
|
||||
@@ -107,7 +107,7 @@ fn main() -> Result<()> {
|
||||
let _recv_handle = thread::Builder::new()
|
||||
.name("tcp-receiver".to_string())
|
||||
.spawn(move || {
|
||||
let receiver =
|
||||
let mut receiver =
|
||||
TcpReceiver::new(recv_serializer, driver_clone, recv_running);
|
||||
if let Err(e) = receiver.run(stream) {
|
||||
log::error!("Receiver error: {}", e);
|
||||
|
||||
@@ -72,7 +72,10 @@ impl TcpPublisher {
|
||||
}
|
||||
|
||||
log::trace!("Sent {} (ts: {} -> {})", group_id, last, data.timestamp_us);
|
||||
last_sent.insert(group_id.clone(), data.timestamp_us);
|
||||
// Use get_mut to update in-place, avoiding String clone on every update
|
||||
if let Some(ts) = last_sent.get_mut(group_id) {
|
||||
*ts = data.timestamp_us;
|
||||
}
|
||||
sent_any = true;
|
||||
} else {
|
||||
log::trace!(
|
||||
|
||||
@@ -14,8 +14,13 @@ pub struct TcpReceiver {
|
||||
serializer: Serializer,
|
||||
driver: Arc<Mutex<Box<dyn DeviceDriver>>>,
|
||||
running: Arc<AtomicBool>,
|
||||
/// Reusable buffer for reading command payloads (avoids allocation per command)
|
||||
read_buffer: Vec<u8>,
|
||||
}
|
||||
|
||||
/// Initial capacity for command read buffer (typical command size)
|
||||
const INITIAL_BUFFER_CAPACITY: usize = 256;
|
||||
|
||||
impl TcpReceiver {
|
||||
/// Create a new TCP receiver
|
||||
pub fn new(
|
||||
@@ -27,11 +32,13 @@ impl TcpReceiver {
|
||||
serializer,
|
||||
driver,
|
||||
running,
|
||||
// Pre-allocate buffer to avoid allocation on first command
|
||||
read_buffer: Vec::with_capacity(INITIAL_BUFFER_CAPACITY),
|
||||
}
|
||||
}
|
||||
|
||||
/// Run the receiver loop for a connected client
|
||||
pub fn run(&self, mut stream: TcpStream) -> Result<()> {
|
||||
pub fn run(&mut self, mut stream: TcpStream) -> Result<()> {
|
||||
log::info!("TCP receiver started for client: {:?}", stream.peer_addr());
|
||||
|
||||
// Set read timeout so we can check shutdown flag
|
||||
@@ -78,7 +85,9 @@ impl TcpReceiver {
|
||||
}
|
||||
|
||||
/// Read a command from the client
|
||||
fn read_command(&self, stream: &mut TcpStream) -> Result<Option<Command>> {
|
||||
///
|
||||
/// Uses a reusable internal buffer to avoid allocation per command.
|
||||
fn read_command(&mut self, stream: &mut TcpStream) -> Result<Option<Command>> {
|
||||
// Read length prefix
|
||||
let mut len_buf = [0u8; 4];
|
||||
match stream.read_exact(&mut len_buf) {
|
||||
@@ -104,12 +113,13 @@ impl TcpReceiver {
|
||||
return Err(Error::Other(format!("Message too large: {} bytes", len)));
|
||||
}
|
||||
|
||||
// Read payload
|
||||
let mut buf = vec![0u8; len];
|
||||
stream.read_exact(&mut buf)?;
|
||||
// Reuse buffer - resize only if needed (no allocation if capacity sufficient)
|
||||
self.read_buffer.clear();
|
||||
self.read_buffer.resize(len, 0);
|
||||
stream.read_exact(&mut self.read_buffer)?;
|
||||
|
||||
// Deserialize command
|
||||
self.serializer.deserialize_command(&buf)
|
||||
self.serializer.deserialize_command(&self.read_buffer)
|
||||
}
|
||||
|
||||
/// Handle a command
|
||||
|
||||
@@ -78,14 +78,20 @@ pub fn create_serializer() -> Serializer {
|
||||
// Conversion implementations for proto types
|
||||
impl proto::Message {
|
||||
/// Create a sensor group message from internal types
|
||||
///
|
||||
/// Uses references where possible to minimize allocations in hot path (~500Hz).
|
||||
pub fn from_sensor_group(data: &SensorGroupData) -> Self {
|
||||
use std::collections::HashMap;
|
||||
|
||||
let values: HashMap<String, proto::SensorValue> = data
|
||||
.values
|
||||
.iter()
|
||||
.map(|(k, v)| (k.clone(), proto::SensorValue::from(v.clone())))
|
||||
.collect();
|
||||
// Pre-allocate HashMap with exact capacity to avoid reallocation
|
||||
let mut values: HashMap<String, proto::SensorValue> =
|
||||
HashMap::with_capacity(data.values.len());
|
||||
|
||||
// Convert values using references - only clone the key string,
|
||||
// SensorValue conversion uses references where possible
|
||||
for (k, v) in &data.values {
|
||||
values.insert(k.clone(), proto::SensorValue::from_ref(v));
|
||||
}
|
||||
|
||||
Self {
|
||||
topic: format!("sensors/{}", data.group_id),
|
||||
@@ -98,6 +104,53 @@ impl proto::Message {
|
||||
}
|
||||
}
|
||||
|
||||
impl proto::SensorValue {
|
||||
/// Convert from a reference to SensorValue - avoids cloning for primitive types
|
||||
///
|
||||
/// This is the preferred method in hot paths (~500Hz) as it only clones
|
||||
/// heap-allocated data (String, Bytes, PointCloud2D) when necessary.
|
||||
pub fn from_ref(value: &crate::core::types::SensorValue) -> Self {
|
||||
use crate::core::types::SensorValue as SV;
|
||||
|
||||
let value = match value {
|
||||
// Primitive types - zero-cost copy
|
||||
SV::Bool(v) => proto::sensor_value::Value::BoolVal(*v),
|
||||
SV::U8(v) => proto::sensor_value::Value::U32Val(*v as u32),
|
||||
SV::U16(v) => proto::sensor_value::Value::U32Val(*v as u32),
|
||||
SV::U32(v) => proto::sensor_value::Value::U32Val(*v),
|
||||
SV::U64(v) => proto::sensor_value::Value::U64Val(*v),
|
||||
SV::I8(v) => proto::sensor_value::Value::I32Val(*v as i32),
|
||||
SV::I16(v) => proto::sensor_value::Value::I32Val(*v as i32),
|
||||
SV::I32(v) => proto::sensor_value::Value::I32Val(*v),
|
||||
SV::I64(v) => proto::sensor_value::Value::I64Val(*v),
|
||||
SV::F32(v) => proto::sensor_value::Value::F32Val(*v),
|
||||
SV::F64(v) => proto::sensor_value::Value::F64Val(*v),
|
||||
SV::Vector3(arr) => proto::sensor_value::Value::Vector3Val(proto::Vector3 {
|
||||
x: arr[0],
|
||||
y: arr[1],
|
||||
z: arr[2],
|
||||
}),
|
||||
// Heap types - must clone
|
||||
SV::String(v) => proto::sensor_value::Value::StringVal(v.clone()),
|
||||
SV::Bytes(v) => proto::sensor_value::Value::BytesVal(v.clone()),
|
||||
SV::PointCloud2D(points) => {
|
||||
proto::sensor_value::Value::PointcloudVal(proto::PointCloud2D {
|
||||
points: points
|
||||
.iter()
|
||||
.map(|(angle, dist, quality)| proto::LidarPoint {
|
||||
angle_rad: *angle,
|
||||
distance_m: *dist,
|
||||
quality: *quality as u32,
|
||||
})
|
||||
.collect(),
|
||||
})
|
||||
}
|
||||
};
|
||||
|
||||
proto::SensorValue { value: Some(value) }
|
||||
}
|
||||
}
|
||||
|
||||
impl From<crate::core::types::SensorValue> for proto::SensorValue {
|
||||
fn from(value: crate::core::types::SensorValue) -> Self {
|
||||
use crate::core::types::SensorValue as SV;
|
||||
|
||||
Reference in New Issue
Block a user