Correct telemetry rate to 110Hz and refactor Mahony AHRS API
- Update documentation across all components to reflect actual GD32 telemetry rate of 110Hz (was incorrectly documented as 500Hz) - Add RawImuData and CalibratedImuData structs for cleaner IMU data handling in Mahony AHRS filter - Refactor MahonyAhrs::update() and update_calibrated() to accept IMU data structs instead of individual parameters - Adjust calibration samples from 1500 to 330 for ~3 second calibration at the correct 110Hz rate
This commit is contained in:
codetiger
@@ -25,7 +25,7 @@ VacuumTiger is an open-source robotic vacuum firmware project for the CRL-200S h
|
||||
│ TCP 5555/5556 │
|
||||
┌─────────────▼───────────────▼───────────────────┐
|
||||
│ SangamIO Daemon (Robot) │
|
||||
│ • Telemetry streaming @ 500Hz (sensors) │
|
||||
│ • Telemetry streaming @ 110Hz (sensors) │
|
||||
│ • Lidar streaming @ 5Hz (360° scans) │
|
||||
│ • Command processing (motion/actuators) │
|
||||
│ • Real-time control loop (50Hz) │
|
||||
@@ -123,7 +123,7 @@ See `sangam-io/proto/sangamio.proto` for SangamIO messages (sensors, commands) a
|
||||
### Topics
|
||||
|
||||
**Outbound (Daemon → Client)**:
|
||||
- `sensors/sensor_status`: Sensor data @ 500Hz (all 13 sensors)
|
||||
- `sensors/sensor_status`: Sensor data @ 110Hz (all 13 sensors)
|
||||
- `sensors/device_version`: Version info (one-time)
|
||||
- `sensors/lidar`: LidarScan @ 5Hz (point cloud)
|
||||
|
||||
@@ -248,7 +248,7 @@ VacuumTiger/
|
||||
### GD32 Protocol
|
||||
- **Format**: `[0xFA 0xFB] [LEN] [CMD] [PAYLOAD] [CRC]`
|
||||
- **CRC**: 16-bit big-endian word sum checksum
|
||||
- **Status**: CMD=0x15, 96 bytes @ 500Hz
|
||||
- **Status**: CMD=0x15, 96 bytes @ 110Hz
|
||||
- **Location**: `src/devices/crl200s/gd32/protocol.rs`
|
||||
|
||||
### Delta-2D Protocol
|
||||
@@ -264,7 +264,7 @@ VacuumTiger/
|
||||
- **CPU Usage**: <1% on Allwinner A33
|
||||
- **Latency**: <25ms command-to-action
|
||||
- **Streaming Rates**:
|
||||
- Telemetry: 500Hz (every GD32 packet)
|
||||
- Telemetry: 110Hz (every GD32 packet)
|
||||
- Lidar: 5Hz (native scan rate)
|
||||
- Commands: On-demand
|
||||
|
||||
|
||||
@@ -9,7 +9,7 @@ VacuumTiger is a modular, configuration-driven firmware stack that makes it easy
|
||||
- **Configurable**: Define sensors, actuators, and hardware in a single TOML file
|
||||
- **Extensible**: Add new robot platforms by implementing one trait
|
||||
- **Generic Protocol**: TCP streaming works with any SLAM application
|
||||
- **Real-time**: 500Hz sensor updates, <25ms command latency
|
||||
- **Real-time**: 110Hz sensor updates, <25ms command latency
|
||||
- **Hackable**: Small codebase (~4K lines Rust), no dependencies on proprietary SDKs
|
||||
|
||||
## How It Works
|
||||
|
||||
@@ -40,7 +40,7 @@ DhruvaSLAM computes odometry from wheel encoders and gyroscopes, then performs o
|
||||
|----------|-------|
|
||||
| Language | Rust 2021 edition |
|
||||
| Version | 0.1.0 |
|
||||
| Input Rate | 500Hz (sensors), 5Hz (lidar) |
|
||||
| Input Rate | 110Hz (sensors), 5Hz (lidar) |
|
||||
| Output Rate | Configurable (default 50Hz odometry) |
|
||||
|
||||
## Architecture
|
||||
@@ -230,7 +230,7 @@ dhruva-slam/
|
||||
## Data Flow
|
||||
|
||||
```
|
||||
SangamIO (500Hz sensors, 5Hz lidar)
|
||||
SangamIO (110Hz sensors, 5Hz lidar)
|
||||
│
|
||||
▼
|
||||
┌───────────────────────────────────────┐
|
||||
|
||||
@@ -9,7 +9,7 @@ sangam_address = "192.168.68.101:5555"
|
||||
[output]
|
||||
# TCP port for visualization clients (Drishti connects here)
|
||||
bind_port = 5557
|
||||
# Output rate in Hz (decimated from 500Hz input)
|
||||
# Output rate in Hz (decimated from 110Hz input)
|
||||
odometry_rate_hz = 50
|
||||
# SLAM status/diagnostics rate (Hz)
|
||||
slam_status_rate_hz = 5.0
|
||||
@@ -36,7 +36,7 @@ alpha = 0.8
|
||||
gyro_scale = 0.0001745
|
||||
|
||||
# Gyroscope Z bias in raw units
|
||||
# Auto-calibrated at startup (1500 samples ~3 seconds)
|
||||
# Auto-calibrated at startup (330 samples ~3 seconds)
|
||||
gyro_bias_z = 0.0
|
||||
|
||||
[slam]
|
||||
|
||||
@@ -12,7 +12,9 @@ use dhruva_slam::core::types::{LaserScan, Pose2D};
|
||||
use dhruva_slam::io::bag::{BagMessage, BagPlayer};
|
||||
use dhruva_slam::sensors::odometry::{WheelOdometry, WheelOdometryConfig};
|
||||
use dhruva_slam::sensors::preprocessing::{PreprocessorConfig, ScanPreprocessor};
|
||||
use dhruva_slam::utils::{GYRO_SCALE, GYRO_SIGN, I16Stats, WHEEL_BASE, WHEEL_TICKS_PER_METER, std_dev_i16};
|
||||
use dhruva_slam::utils::{
|
||||
GYRO_SCALE, GYRO_SIGN, I16Stats, WHEEL_BASE, WHEEL_TICKS_PER_METER, std_dev_i16,
|
||||
};
|
||||
use std::env;
|
||||
|
||||
fn main() {
|
||||
@@ -59,8 +61,8 @@ fn show_pose_analysis(bag_path: &str) {
|
||||
let mut matcher = HybridP2LMatcher::new(HybridP2LMatcherConfig::default());
|
||||
|
||||
// Track poses
|
||||
let mut odom_only_pose = Pose2D::identity(); // Pure odometry
|
||||
let mut slam_pose = Pose2D::identity(); // Scan-matched pose (like in benchmark)
|
||||
let mut odom_only_pose = Pose2D::identity(); // Pure odometry
|
||||
let mut slam_pose = Pose2D::identity(); // Scan-matched pose (like in benchmark)
|
||||
let mut prev_odom_at_scan = Pose2D::identity();
|
||||
let mut odom_pose = Pose2D::identity();
|
||||
|
||||
@@ -122,8 +124,9 @@ fn show_pose_analysis(bag_path: &str) {
|
||||
// EXPERIMENT: Try using odom_delta directly without inversion
|
||||
// The matcher should find the transform that aligns source→target
|
||||
// which is the robot motion itself
|
||||
let initial_guess = odom_delta; // Changed from odom_delta.inverse()
|
||||
let match_result = matcher.match_scans(&processed, prev_scan, &initial_guess);
|
||||
let initial_guess = odom_delta; // Changed from odom_delta.inverse()
|
||||
let match_result =
|
||||
matcher.match_scans(&processed, prev_scan, &initial_guess);
|
||||
|
||||
// The match result gives transform from current→previous
|
||||
// Invert to get motion from previous→current
|
||||
@@ -180,11 +183,15 @@ fn show_pose_analysis(bag_path: &str) {
|
||||
if scan_count > 1 {
|
||||
println!(
|
||||
" SLAM pose: (x={:.4}m, y={:.4}m, θ={:.2}°)",
|
||||
slam_pose.x, slam_pose.y, slam_pose.theta.to_degrees()
|
||||
slam_pose.x,
|
||||
slam_pose.y,
|
||||
slam_pose.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" Odom pose: (x={:.4}m, y={:.4}m, θ={:.2}°)",
|
||||
odom_only_pose.x, odom_only_pose.y, odom_only_pose.theta.to_degrees()
|
||||
odom_only_pose.x,
|
||||
odom_only_pose.y,
|
||||
odom_only_pose.theta.to_degrees()
|
||||
);
|
||||
}
|
||||
}
|
||||
@@ -225,15 +232,21 @@ fn show_pose_analysis(bag_path: &str) {
|
||||
println!("Final Poses:");
|
||||
println!(
|
||||
" Raw odom_pose: (x={:.4}m, y={:.4}m, θ={:.2}°)",
|
||||
odom_pose.x, odom_pose.y, odom_pose.theta.to_degrees()
|
||||
odom_pose.x,
|
||||
odom_pose.y,
|
||||
odom_pose.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" Odom-only accumulated: (x={:.4}m, y={:.4}m, θ={:.2}°)",
|
||||
odom_only_pose.x, odom_only_pose.y, odom_only_pose.theta.to_degrees()
|
||||
odom_only_pose.x,
|
||||
odom_only_pose.y,
|
||||
odom_only_pose.theta.to_degrees()
|
||||
);
|
||||
println!(
|
||||
" SLAM pose (scan-matched): (x={:.4}m, y={:.4}m, θ={:.2}°)",
|
||||
global_pose.x, global_pose.y, global_pose.theta.to_degrees()
|
||||
global_pose.x,
|
||||
global_pose.y,
|
||||
global_pose.theta.to_degrees()
|
||||
);
|
||||
|
||||
println!();
|
||||
@@ -254,7 +267,10 @@ fn show_pose_analysis(bag_path: &str) {
|
||||
println!();
|
||||
|
||||
if delta_left > 0 && delta_right > 0 {
|
||||
println!("Motion detected: FORWARD ({} ticks)", (delta_left + delta_right) / 2);
|
||||
println!(
|
||||
"Motion detected: FORWARD ({} ticks)",
|
||||
(delta_left + delta_right) / 2
|
||||
);
|
||||
println!("Expected X position: {:.4}m", expected_x);
|
||||
println!("Actual X position: {:.4}m", global_pose.x);
|
||||
|
||||
|
||||
@@ -40,7 +40,7 @@ struct ImuBiasCalibrator {
|
||||
impl ImuBiasCalibrator {
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
samples: Vec::with_capacity(1500), // ~3 seconds at 500Hz
|
||||
samples: Vec::with_capacity(330), // ~3 seconds at 110Hz
|
||||
}
|
||||
}
|
||||
|
||||
@@ -66,11 +66,7 @@ impl ImuBiasCalibrator {
|
||||
)
|
||||
});
|
||||
|
||||
[
|
||||
(sum.0 / n) as f32,
|
||||
(sum.1 / n) as f32,
|
||||
(sum.2 / n) as f32,
|
||||
]
|
||||
[(sum.0 / n) as f32, (sum.1 / n) as f32, (sum.2 / n) as f32]
|
||||
}
|
||||
}
|
||||
|
||||
@@ -263,7 +259,10 @@ fn run(config: Config) -> Result<(), Box<dyn std::error::Error>> {
|
||||
last_progress = Instant::now();
|
||||
}
|
||||
}
|
||||
println!("\r Sensors stabilized. (discarded {} msgs) ", discarded_count);
|
||||
println!(
|
||||
"\r Sensors stabilized. (discarded {} msgs) ",
|
||||
discarded_count
|
||||
);
|
||||
|
||||
// Flush any buffered messages before calibration
|
||||
let mut flushed = 0;
|
||||
@@ -332,8 +331,7 @@ fn run(config: Config) -> Result<(), Box<dyn std::error::Error>> {
|
||||
}
|
||||
|
||||
// ===== MOTION SETUP (if requested) =====
|
||||
let motion_enabled =
|
||||
config.linear_velocity.is_some() || config.angular_velocity.is_some();
|
||||
let motion_enabled = config.linear_velocity.is_some() || config.angular_velocity.is_some();
|
||||
if motion_enabled {
|
||||
let linear = config.linear_velocity.unwrap_or(0.0);
|
||||
let angular = config.angular_velocity.unwrap_or(0.0);
|
||||
|
||||
@@ -341,7 +341,7 @@ fn run_main_loop(
|
||||
},
|
||||
output_rate_hz: config.output.odometry_rate_hz,
|
||||
// Skip gyro calibration for wheel-only mode
|
||||
calibration_samples: if use_wheel_only { 0 } else { 1500 },
|
||||
calibration_samples: if use_wheel_only { 0 } else { 330 },
|
||||
};
|
||||
let mut pipeline = OdometryPipeline::new(pipeline_config);
|
||||
|
||||
|
||||
@@ -53,7 +53,7 @@ use dhruva_slam::metrics::{
|
||||
};
|
||||
use dhruva_slam::sensors::odometry::{
|
||||
ComplementaryConfig, ComplementaryFilter, Eskf, EskfConfig, MahonyAhrs, MahonyConfig,
|
||||
WheelOdometry, WheelOdometryConfig,
|
||||
RawImuData, WheelOdometry, WheelOdometryConfig,
|
||||
};
|
||||
use dhruva_slam::sensors::preprocessing::{PreprocessorConfig, ScanPreprocessor};
|
||||
use dhruva_slam::utils::{GYRO_SCALE, WHEEL_BASE, WHEEL_TICKS_PER_METER};
|
||||
@@ -751,9 +751,9 @@ fn run_single_full_slam(
|
||||
let integrator = MapIntegrator::new(MapIntegratorConfig::default());
|
||||
|
||||
// Separate tracking for raw odometry and scan-matched SLAM pose
|
||||
let mut odom_pose = Pose2D::identity(); // Raw odometry (always accumulating)
|
||||
let mut prev_odom_at_scan = Pose2D::identity(); // Odometry at previous scan
|
||||
let mut slam_pose = Pose2D::identity(); // Scan-matched pose
|
||||
let mut odom_pose = Pose2D::identity(); // Raw odometry (always accumulating)
|
||||
let mut prev_odom_at_scan = Pose2D::identity(); // Odometry at previous scan
|
||||
let mut slam_pose = Pose2D::identity(); // Scan-matched pose
|
||||
let mut previous_scan: Option<PointCloud2D> = None;
|
||||
let mut scans_processed = 0u64;
|
||||
let mut failed_matches = 0u64;
|
||||
@@ -766,9 +766,9 @@ fn run_single_full_slam(
|
||||
let pid = sysinfo::get_current_pid().ok();
|
||||
|
||||
// Thresholds for accepting scan match results
|
||||
const MIN_MATCH_SCORE: f32 = 0.3; // Minimum acceptable match score
|
||||
const MAX_CORRECTION_DIST: f32 = 0.15; // Max 15cm correction from odometry
|
||||
const MAX_CORRECTION_ANGLE: f32 = 0.35; // Max ~20 degrees correction
|
||||
const MIN_MATCH_SCORE: f32 = 0.3; // Minimum acceptable match score
|
||||
const MAX_CORRECTION_DIST: f32 = 0.15; // Max 15cm correction from odometry
|
||||
const MAX_CORRECTION_ANGLE: f32 = 0.35; // Max ~20 degrees correction
|
||||
|
||||
let start_time = Instant::now();
|
||||
|
||||
@@ -782,7 +782,7 @@ fn run_single_full_slam(
|
||||
status.gyro_raw[2],
|
||||
status.timestamp_us,
|
||||
) {
|
||||
odom_pose = pose; // Always raw odometry
|
||||
odom_pose = pose; // Always raw odometry
|
||||
}
|
||||
}
|
||||
BagMessage::Lidar(timestamped) => {
|
||||
@@ -810,7 +810,8 @@ fn run_single_full_slam(
|
||||
// FIX: Pass odom_delta directly (NOT inverted) as initial guess,
|
||||
// and use match result directly (NOT inverted) as matched_delta.
|
||||
let initial_guess = odom_delta;
|
||||
let match_result = matcher.match_scans(&processed, prev_scan, &initial_guess);
|
||||
let match_result =
|
||||
matcher.match_scans(&processed, prev_scan, &initial_guess);
|
||||
|
||||
// The match result transform is the robot motion from prev→current
|
||||
let matched_delta = match_result.transform;
|
||||
@@ -871,7 +872,9 @@ fn run_single_full_slam(
|
||||
0.0
|
||||
};
|
||||
|
||||
let successful_matches = scans_processed.saturating_sub(failed_matches).saturating_sub(1); // -1 for first scan
|
||||
let successful_matches = scans_processed
|
||||
.saturating_sub(failed_matches)
|
||||
.saturating_sub(1); // -1 for first scan
|
||||
let avg_match_score = if successful_matches > 0 {
|
||||
total_score / successful_matches as f64
|
||||
} else {
|
||||
@@ -1099,9 +1102,9 @@ impl MahonyOdometry {
|
||||
ticks_per_meter: WHEEL_TICKS_PER_METER,
|
||||
wheel_base: WHEEL_BASE,
|
||||
};
|
||||
// Use shorter calibration for benchmark (500 samples = 1 second at 500Hz)
|
||||
// Use shorter calibration for benchmark (110 samples = 1 second at 110Hz)
|
||||
let mahony_config = MahonyConfig {
|
||||
calibration_samples: 500,
|
||||
calibration_samples: 110,
|
||||
..MahonyConfig::default()
|
||||
};
|
||||
Self {
|
||||
@@ -1122,11 +1125,11 @@ impl MahonyOdometry {
|
||||
// Update Mahony AHRS (uses gyro_yaw as Z-axis rotation)
|
||||
// For 2D odometry, we only need yaw from the AHRS
|
||||
// Pass zeros for other axes and gravity (simplified 2D case)
|
||||
let (_roll, _pitch, yaw) = self.ahrs.update(
|
||||
gyro_yaw, 0, 0, // gyro: yaw on X input (Mahony remaps internally)
|
||||
0, 0, 1000, // tilt: assume level (gravity pointing down)
|
||||
timestamp_us,
|
||||
let imu = RawImuData::new(
|
||||
[gyro_yaw, 0, 0], // gyro: yaw on X input (Mahony remaps internally)
|
||||
[0, 0, 1000], // tilt: assume level (gravity pointing down)
|
||||
);
|
||||
let (_roll, _pitch, yaw) = self.ahrs.update(&imu, timestamp_us);
|
||||
|
||||
// Get wheel odometry delta
|
||||
if let Some(encoder_delta) = self.wheel.update(left, right) {
|
||||
|
||||
@@ -10,7 +10,7 @@ message Message {
|
||||
}
|
||||
}
|
||||
|
||||
// Sensor data from robot (outbound @ 500Hz for telemetry, 5Hz for lidar)
|
||||
// Sensor data from robot (outbound @ 110Hz for telemetry, 5Hz for lidar)
|
||||
message SensorGroup {
|
||||
string group_id = 1;
|
||||
uint64 timestamp_us = 2;
|
||||
|
||||
@@ -589,12 +589,14 @@ impl PointToLineIcp {
|
||||
let final_transform = if score > 0.95 {
|
||||
let init_trans = (initial_guess.x.powi(2) + initial_guess.y.powi(2)).sqrt();
|
||||
let init_rot = initial_guess.theta.abs();
|
||||
let curr_trans = (current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
|
||||
let curr_trans =
|
||||
(current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
|
||||
let curr_rot = current_transform.theta.abs();
|
||||
|
||||
// If both initial guess and result are near-identity, snap to initial
|
||||
// This prevents P2L aperture problem from causing rotation drift
|
||||
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05 {
|
||||
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05
|
||||
{
|
||||
*initial_guess
|
||||
} else {
|
||||
current_transform
|
||||
@@ -618,9 +620,11 @@ impl PointToLineIcp {
|
||||
let final_transform = if score > 0.95 {
|
||||
let init_trans = (initial_guess.x.powi(2) + initial_guess.y.powi(2)).sqrt();
|
||||
let init_rot = initial_guess.theta.abs();
|
||||
let curr_trans = (current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
|
||||
let curr_trans =
|
||||
(current_transform.x.powi(2) + current_transform.y.powi(2)).sqrt();
|
||||
let curr_rot = current_transform.theta.abs();
|
||||
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05 {
|
||||
if init_trans < 0.03 && init_rot < 0.05 && curr_trans < 0.03 && curr_rot < 0.05
|
||||
{
|
||||
*initial_guess
|
||||
} else {
|
||||
current_transform
|
||||
|
||||
@@ -280,7 +280,7 @@ let config = OdometryPipelineConfig {
|
||||
|
||||
let mut pipeline = OdometryPipeline::new(config);
|
||||
|
||||
// Process each sensor update (500Hz)
|
||||
// Process each sensor update (110Hz)
|
||||
if let Some(pose) = pipeline.process(left_ticks, right_ticks, gyro_z, timestamp_us) {
|
||||
// Decimated output at configured rate
|
||||
publish_pose(&pose, timestamp_us);
|
||||
@@ -297,7 +297,7 @@ let current = pipeline.pose();
|
||||
|
||||
**Features:**
|
||||
- Auto-calibrates gyro bias on startup (1500 samples)
|
||||
- Decimates 500Hz input to configurable output rate
|
||||
- Decimates 110Hz input to configurable output rate
|
||||
- Provides timing and quality diagnostics
|
||||
|
||||
## File Structure
|
||||
|
||||
@@ -242,7 +242,7 @@ mod tests {
|
||||
for i in 0..100 {
|
||||
recorder
|
||||
.record_sensor_status(&SensorStatusMsg {
|
||||
timestamp_us: i * 2000, // 500 Hz
|
||||
timestamp_us: i * 9091, // 110 Hz
|
||||
encoder: EncoderTicks::new(i as u16, i as u16),
|
||||
gyro_raw: [0, 0, 0],
|
||||
accel_raw: [0, 0, 1638],
|
||||
@@ -256,7 +256,7 @@ mod tests {
|
||||
assert_eq!(info.message_count, 100);
|
||||
assert_eq!(info.sensor_count, 100);
|
||||
assert_eq!(info.lidar_count, 0);
|
||||
assert_eq!(info.duration_us, 99 * 2000);
|
||||
assert_eq!(info.duration_us, 99 * 9091);
|
||||
}
|
||||
|
||||
#[test]
|
||||
|
||||
@@ -22,7 +22,7 @@ impl BagTestFixture {
|
||||
/// Create a synthetic bag file with stationary robot (5 seconds).
|
||||
///
|
||||
/// Contains:
|
||||
/// - 2500 sensor status messages at 500 Hz
|
||||
/// - 550 sensor status messages at 110 Hz
|
||||
/// - 25 lidar scans at 5 Hz
|
||||
/// - Robot stationary at origin
|
||||
pub fn synthetic_5_seconds() -> Self {
|
||||
@@ -33,14 +33,14 @@ impl BagTestFixture {
|
||||
|
||||
let start_time_us = 1_000_000_000u64; // 1 second since epoch
|
||||
let duration_us = 5_000_000u64; // 5 seconds
|
||||
let sensor_period_us = 2000u64; // 500 Hz
|
||||
let sensor_period_us = 9091u64; // 110 Hz
|
||||
let lidar_period_us = 200_000u64; // 5 Hz
|
||||
|
||||
let mut time = start_time_us;
|
||||
let encoder_value = 1000u16;
|
||||
|
||||
while time < start_time_us + duration_us {
|
||||
// Sensor status at 500Hz
|
||||
// Sensor status at 110Hz
|
||||
recorder
|
||||
.record_sensor_status(&SensorStatusMsg {
|
||||
timestamp_us: time,
|
||||
@@ -91,14 +91,14 @@ impl BagTestFixture {
|
||||
|
||||
let duration_s = distance_m / speed_mps;
|
||||
let duration_us = (duration_s * 1_000_000.0) as u64;
|
||||
let sensor_period_us = 2000u64; // 500 Hz
|
||||
let sensor_period_us = 9091u64; // 110 Hz
|
||||
|
||||
let start_time_us = 1_000_000_000u64;
|
||||
let mut time = start_time_us;
|
||||
let mut position = 0.0f32;
|
||||
|
||||
while time < start_time_us + duration_us {
|
||||
let dt = 0.002; // 2ms = 500Hz
|
||||
let dt = 0.00909; // 9.09ms = 110Hz
|
||||
position += speed_mps * dt;
|
||||
|
||||
let ticks = (position * ticks_per_meter) as u16;
|
||||
@@ -138,7 +138,7 @@ impl BagTestFixture {
|
||||
|
||||
let duration_s = angle_rad.abs() / angular_velocity_radps.abs();
|
||||
let duration_us = (duration_s * 1_000_000.0) as u64;
|
||||
let sensor_period_us = 2000u64; // 500 Hz
|
||||
let sensor_period_us = 9091u64; // 110 Hz
|
||||
|
||||
let start_time_us = 1_000_000_000u64;
|
||||
let mut time = start_time_us;
|
||||
@@ -191,7 +191,7 @@ impl BagTestFixture {
|
||||
|
||||
let straight_time_s = side_length_m / linear_speed_mps;
|
||||
let turn_time_s = (std::f32::consts::FRAC_PI_2) / angular_speed_radps;
|
||||
let sensor_period_us = 2000u64;
|
||||
let sensor_period_us = 9091u64; // 110 Hz
|
||||
|
||||
let start_time_us = 1_000_000_000u64;
|
||||
let mut time = start_time_us;
|
||||
@@ -204,7 +204,7 @@ impl BagTestFixture {
|
||||
let segment_end_time = time + segment_duration_us;
|
||||
|
||||
while time < segment_end_time {
|
||||
let dt = 0.002;
|
||||
let dt = 0.00909; // 9.09ms = 110Hz
|
||||
total_distance += linear_speed_mps * dt;
|
||||
|
||||
let ticks = (total_distance * ticks_per_meter) as u16;
|
||||
|
||||
@@ -94,7 +94,7 @@ impl EncoderTicks {
|
||||
|
||||
/// Sensor status message from SangamIO.
|
||||
///
|
||||
/// Contains wheel encoder and IMU data at 500Hz.
|
||||
/// Contains wheel encoder and IMU data at 110Hz.
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
pub struct SensorStatusMsg {
|
||||
/// Timestamp in microseconds since epoch
|
||||
@@ -133,7 +133,7 @@ impl SensorStatusMsg {
|
||||
/// Each variant corresponds to a different sensor stream.
|
||||
#[derive(Debug, Clone, Serialize, Deserialize)]
|
||||
pub enum BagMessage {
|
||||
/// Sensor status (500 Hz): encoders + IMU
|
||||
/// Sensor status (110 Hz): encoders + IMU
|
||||
SensorStatus(SensorStatusMsg),
|
||||
/// LiDAR scan (5 Hz): point cloud
|
||||
Lidar(Timestamped<LidarScan>),
|
||||
|
||||
@@ -11,7 +11,7 @@
|
||||
//! let config = OdometryPipelineConfig::default();
|
||||
//! let mut pipeline = OdometryPipeline::new(config);
|
||||
//!
|
||||
//! // In sensor loop (500Hz from SangamIO)
|
||||
//! // In sensor loop (110Hz from SangamIO)
|
||||
//! if let Some(pose) = pipeline.process(left_ticks, right_ticks, gyro_z, timestamp_us) {
|
||||
//! // New pose available at output rate (e.g., 50Hz)
|
||||
//! publisher.publish_pose(&pose, timestamp_us);
|
||||
@@ -35,13 +35,13 @@ pub struct OdometryPipelineConfig {
|
||||
|
||||
/// Output rate in Hz (decimation from input rate)
|
||||
///
|
||||
/// Input is typically 500Hz from SangamIO, output should be
|
||||
/// Input is typically 110Hz from SangamIO, output should be
|
||||
/// lower (e.g., 50Hz) for visualization.
|
||||
pub output_rate_hz: f32,
|
||||
|
||||
/// Number of samples to collect for gyro bias calibration at startup.
|
||||
///
|
||||
/// At 500Hz, 1500 samples = 3 seconds of calibration.
|
||||
/// At 110Hz, 330 samples = 3 seconds of calibration.
|
||||
/// Set to 0 to disable auto-calibration.
|
||||
pub calibration_samples: u32,
|
||||
}
|
||||
@@ -52,7 +52,7 @@ impl Default for OdometryPipelineConfig {
|
||||
wheel_odom: WheelOdometryConfig::default(),
|
||||
filter: ComplementaryConfig::default(),
|
||||
output_rate_hz: 50.0,
|
||||
calibration_samples: 1500, // ~3 seconds at 500Hz
|
||||
calibration_samples: 330, // ~3 seconds at 110Hz
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -131,9 +131,10 @@ pub use core::types::{Point2D, Pose2D, Timestamped};
|
||||
|
||||
// Sensors - Odometry
|
||||
pub use sensors::odometry::{
|
||||
ComplementaryConfig, ComplementaryFilter, Eskf, EskfConfig, EulerAngles, EvaluationResult,
|
||||
MahonyAhrs, MahonyConfig, MeasurementNoise, OdometryEvaluator, ProcessNoise, Quaternion,
|
||||
ScenarioBounds, Stats, TestScenario, WheelOdometry, WheelOdometryConfig,
|
||||
CalibratedImuData, ComplementaryConfig, ComplementaryFilter, Eskf, EskfConfig, EulerAngles,
|
||||
EvaluationResult, MahonyAhrs, MahonyConfig, MeasurementNoise, OdometryEvaluator, ProcessNoise,
|
||||
Quaternion, RawImuData, ScenarioBounds, Stats, TestScenario, WheelOdometry,
|
||||
WheelOdometryConfig,
|
||||
};
|
||||
|
||||
// Sensors - Preprocessing
|
||||
|
||||
@@ -47,7 +47,7 @@ impl GyroBiasEstimator {
|
||||
///
|
||||
/// * `target_samples` - Number of samples to collect before computing bias.
|
||||
/// More samples = more accurate but longer calibration time.
|
||||
/// Typical values: 50-200 samples at 500Hz = 100-400ms
|
||||
/// Typical values: 10-45 samples at 110Hz = 100-400ms
|
||||
pub fn new(target_samples: usize) -> Self {
|
||||
Self {
|
||||
target_samples: target_samples.max(2), // Minimum 2 samples for variance calculation
|
||||
|
||||
@@ -99,7 +99,7 @@ impl Default for EskfConfig {
|
||||
gyro_bias_z: 0.0,
|
||||
gyro_sign: 1.0, // SangamIO provides REP-103 compliant data (CCW positive)
|
||||
initial_position_variance: 0.01, // 10cm std dev
|
||||
initial_heading_variance: 0.01, // ~6° std dev
|
||||
initial_heading_variance: 0.01, // ~6° std dev
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -24,18 +24,15 @@
|
||||
//! # Example
|
||||
//!
|
||||
//! ```ignore
|
||||
//! use dhruva_slam::sensors::odometry::{MahonyAhrs, MahonyConfig};
|
||||
//! use dhruva_slam::sensors::odometry::{MahonyAhrs, MahonyConfig, RawImuData};
|
||||
//!
|
||||
//! let config = MahonyConfig::default();
|
||||
//! let mut ahrs = MahonyAhrs::new(config);
|
||||
//!
|
||||
//! // Process IMU data at sensor rate (~500Hz)
|
||||
//! // Process IMU data at sensor rate (~110Hz)
|
||||
//! loop {
|
||||
//! let (roll, pitch, yaw) = ahrs.update(
|
||||
//! gyro_x, gyro_y, gyro_z,
|
||||
//! tilt_x, tilt_y, tilt_z,
|
||||
//! timestamp_us,
|
||||
//! );
|
||||
//! let imu = RawImuData::new(gyro_raw, tilt_raw);
|
||||
//! let (roll, pitch, yaw) = ahrs.update(&imu, timestamp_us);
|
||||
//!
|
||||
//! if ahrs.is_calibrated() {
|
||||
//! // Use orientation for odometry fusion
|
||||
@@ -75,7 +72,7 @@ pub struct MahonyConfig {
|
||||
pub gravity_scale: f32,
|
||||
|
||||
/// Number of samples for initial bias calibration.
|
||||
/// At 500Hz, 1500 samples = 3 seconds.
|
||||
/// At 110Hz, 330 samples = 3 seconds.
|
||||
pub calibration_samples: u32,
|
||||
|
||||
/// Sample rate in Hz (used for fixed dt if timestamps unavailable).
|
||||
@@ -85,12 +82,12 @@ pub struct MahonyConfig {
|
||||
impl Default for MahonyConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
kp: 5.0, // Proportional gain (responsive but stable)
|
||||
ki: 0.1, // Integral gain (slow bias adaptation)
|
||||
kp: 5.0, // Proportional gain (responsive but stable)
|
||||
ki: 0.1, // Integral gain (slow bias adaptation)
|
||||
gyro_scale: CRL200S_GYRO_SCALE,
|
||||
gravity_scale: GRAVITY_SCALE,
|
||||
calibration_samples: 1500, // ~3 seconds at 500Hz
|
||||
sample_rate_hz: 500.0,
|
||||
calibration_samples: 330, // ~3 seconds at 110Hz
|
||||
sample_rate_hz: 110.0,
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -176,6 +173,68 @@ impl EulerAngles {
|
||||
}
|
||||
}
|
||||
|
||||
/// Raw IMU data from sensor (before calibration/scaling).
|
||||
#[derive(Debug, Clone, Copy, Default)]
|
||||
pub struct RawImuData {
|
||||
/// Raw gyroscope X (roll rate) in sensor units
|
||||
pub gyro_x: i16,
|
||||
/// Raw gyroscope Y (pitch rate) in sensor units
|
||||
pub gyro_y: i16,
|
||||
/// Raw gyroscope Z (yaw rate) in sensor units
|
||||
pub gyro_z: i16,
|
||||
/// LP-filtered gravity/tilt X
|
||||
pub tilt_x: i16,
|
||||
/// LP-filtered gravity/tilt Y
|
||||
pub tilt_y: i16,
|
||||
/// LP-filtered gravity/tilt Z
|
||||
pub tilt_z: i16,
|
||||
}
|
||||
|
||||
impl RawImuData {
|
||||
/// Create new raw IMU data.
|
||||
pub fn new(gyro: [i16; 3], tilt: [i16; 3]) -> Self {
|
||||
Self {
|
||||
gyro_x: gyro[0],
|
||||
gyro_y: gyro[1],
|
||||
gyro_z: gyro[2],
|
||||
tilt_x: tilt[0],
|
||||
tilt_y: tilt[1],
|
||||
tilt_z: tilt[2],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Calibrated IMU data in physical units.
|
||||
#[derive(Debug, Clone, Copy, Default)]
|
||||
pub struct CalibratedImuData {
|
||||
/// Gyroscope X (roll rate) in rad/s
|
||||
pub gx: f32,
|
||||
/// Gyroscope Y (pitch rate) in rad/s
|
||||
pub gy: f32,
|
||||
/// Gyroscope Z (yaw rate) in rad/s
|
||||
pub gz: f32,
|
||||
/// Accelerometer/gravity X (normalized or m/s²)
|
||||
pub ax: f32,
|
||||
/// Accelerometer/gravity Y (normalized or m/s²)
|
||||
pub ay: f32,
|
||||
/// Accelerometer/gravity Z (normalized or m/s²)
|
||||
pub az: f32,
|
||||
}
|
||||
|
||||
impl CalibratedImuData {
|
||||
/// Create new calibrated IMU data.
|
||||
pub fn new(gyro: [f32; 3], accel: [f32; 3]) -> Self {
|
||||
Self {
|
||||
gx: gyro[0],
|
||||
gy: gyro[1],
|
||||
gz: gyro[2],
|
||||
ax: accel[0],
|
||||
ay: accel[1],
|
||||
az: accel[2],
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/// Gyro bias calibration state.
|
||||
#[derive(Debug)]
|
||||
struct BiasCalibration {
|
||||
@@ -326,34 +385,20 @@ impl MahonyAhrs {
|
||||
self.bias_cal.reset();
|
||||
}
|
||||
|
||||
/// Update the filter with new IMU data.
|
||||
/// Update the filter with new raw IMU data.
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `gyro_x` - Raw gyro X (roll rate, ROS REP-103 frame)
|
||||
/// * `gyro_y` - Raw gyro Y (pitch rate, ROS REP-103 frame)
|
||||
/// * `gyro_z` - Raw gyro Z (yaw rate, ROS REP-103 frame)
|
||||
/// * `tilt_x` - LP-filtered gravity X
|
||||
/// * `tilt_y` - LP-filtered gravity Y
|
||||
/// * `tilt_z` - LP-filtered gravity Z
|
||||
/// * `imu` - Raw IMU data (gyro and tilt in sensor units, ROS REP-103 frame)
|
||||
/// * `timestamp_us` - Timestamp in microseconds
|
||||
///
|
||||
/// # Returns
|
||||
///
|
||||
/// Euler angles (roll, pitch, yaw) in radians, or (0, 0, 0) during calibration.
|
||||
pub fn update(
|
||||
&mut self,
|
||||
gyro_x: i16,
|
||||
gyro_y: i16,
|
||||
gyro_z: i16,
|
||||
tilt_x: i16,
|
||||
tilt_y: i16,
|
||||
tilt_z: i16,
|
||||
timestamp_us: u64,
|
||||
) -> (f32, f32, f32) {
|
||||
pub fn update(&mut self, imu: &RawImuData, timestamp_us: u64) -> (f32, f32, f32) {
|
||||
// Calibration phase - collect bias samples
|
||||
if !self.bias_cal.is_calibrated() {
|
||||
self.bias_cal.add_sample(gyro_x, gyro_y, gyro_z);
|
||||
self.bias_cal.add_sample(imu.gyro_x, imu.gyro_y, imu.gyro_z);
|
||||
self.last_timestamp_us = Some(timestamp_us);
|
||||
return (0.0, 0.0, 0.0);
|
||||
}
|
||||
@@ -382,9 +427,9 @@ impl MahonyAhrs {
|
||||
// gyro_x = Roll rate (X axis rotation)
|
||||
// gyro_y = Pitch rate (Y axis rotation)
|
||||
// gyro_z = Yaw rate (Z axis rotation)
|
||||
let gx = (gyro_x as f32 - bias[0]) * self.config.gyro_scale; // Roll
|
||||
let gy = (gyro_y as f32 - bias[1]) * self.config.gyro_scale; // Pitch
|
||||
let gz = (gyro_z as f32 - bias[2]) * self.config.gyro_scale; // Yaw
|
||||
let gx = (imu.gyro_x as f32 - bias[0]) * self.config.gyro_scale; // Roll
|
||||
let gy = (imu.gyro_y as f32 - bias[1]) * self.config.gyro_scale; // Pitch
|
||||
let gz = (imu.gyro_z as f32 - bias[2]) * self.config.gyro_scale; // Yaw
|
||||
|
||||
// Get current quaternion
|
||||
let q0 = self.q.w;
|
||||
@@ -393,9 +438,9 @@ impl MahonyAhrs {
|
||||
let q3 = self.q.z;
|
||||
|
||||
// Normalize gravity vector
|
||||
let mut ax = tilt_x as f32 * self.config.gravity_scale;
|
||||
let mut ay = tilt_y as f32 * self.config.gravity_scale;
|
||||
let mut az = tilt_z as f32 * self.config.gravity_scale;
|
||||
let mut ax = imu.tilt_x as f32 * self.config.gravity_scale;
|
||||
let mut ay = imu.tilt_y as f32 * self.config.gravity_scale;
|
||||
let mut az = imu.tilt_z as f32 * self.config.gravity_scale;
|
||||
|
||||
let accel_norm = (ax * ax + ay * ay + az * az).sqrt();
|
||||
|
||||
@@ -449,19 +494,15 @@ impl MahonyAhrs {
|
||||
self.q.to_euler()
|
||||
}
|
||||
|
||||
/// Update with pre-calibrated gyro values in rad/s.
|
||||
/// Update with pre-calibrated IMU data in physical units.
|
||||
///
|
||||
/// Use this if you've already applied bias correction and scaling.
|
||||
pub fn update_calibrated(
|
||||
&mut self,
|
||||
gx_rad_s: f32,
|
||||
gy_rad_s: f32,
|
||||
gz_rad_s: f32,
|
||||
ax: f32,
|
||||
ay: f32,
|
||||
az: f32,
|
||||
dt: f32,
|
||||
) -> (f32, f32, f32) {
|
||||
///
|
||||
/// # Arguments
|
||||
///
|
||||
/// * `imu` - Calibrated IMU data (gyro in rad/s, accel normalized or in m/s²)
|
||||
/// * `dt` - Time delta in seconds
|
||||
pub fn update_calibrated(&mut self, imu: &CalibratedImuData, dt: f32) -> (f32, f32, f32) {
|
||||
if dt <= 0.0 || dt > 0.1 {
|
||||
return self.q.to_euler();
|
||||
}
|
||||
@@ -471,14 +512,14 @@ impl MahonyAhrs {
|
||||
let q2 = self.q.y;
|
||||
let q3 = self.q.z;
|
||||
|
||||
let (mut gx, mut gy, mut gz) = (gx_rad_s, gy_rad_s, gz_rad_s);
|
||||
let (mut gx, mut gy, mut gz) = (imu.gx, imu.gy, imu.gz);
|
||||
|
||||
// Normalize accelerometer
|
||||
let accel_norm = (ax * ax + ay * ay + az * az).sqrt();
|
||||
let accel_norm = (imu.ax * imu.ax + imu.ay * imu.ay + imu.az * imu.az).sqrt();
|
||||
if accel_norm > 0.01 {
|
||||
let ax_n = ax / accel_norm;
|
||||
let ay_n = ay / accel_norm;
|
||||
let az_n = az / accel_norm;
|
||||
let ax_n = imu.ax / accel_norm;
|
||||
let ay_n = imu.ay / accel_norm;
|
||||
let az_n = imu.az / accel_norm;
|
||||
|
||||
// Estimated gravity from quaternion
|
||||
let vx = 2.0 * (q1 * q3 - q0 * q2);
|
||||
@@ -569,8 +610,9 @@ mod tests {
|
||||
let mut ahrs = MahonyAhrs::new(config);
|
||||
|
||||
// During calibration, should return zeros
|
||||
let imu = RawImuData::new([100, 50, -30], [0, 0, 1000]);
|
||||
for i in 0..10 {
|
||||
let (r, p, y) = ahrs.update(100, 50, -30, 0, 0, 1000, i * 2000);
|
||||
let (r, p, y) = ahrs.update(&imu, i * 2000);
|
||||
if i < 9 {
|
||||
assert!(!ahrs.is_calibrated());
|
||||
}
|
||||
@@ -594,13 +636,14 @@ mod tests {
|
||||
let mut ahrs = MahonyAhrs::new(config);
|
||||
|
||||
// Calibration phase
|
||||
let imu = RawImuData::new([0, 0, 0], [0, 0, 1000]);
|
||||
for i in 0..5 {
|
||||
ahrs.update(0, 0, 0, 0, 0, 1000, i * 2000);
|
||||
ahrs.update(&imu, i * 2000);
|
||||
}
|
||||
|
||||
// Stationary updates (gyro at bias = 0)
|
||||
for i in 5..20 {
|
||||
let (roll, pitch, yaw) = ahrs.update(0, 0, 0, 0, 0, 1000, i * 2000);
|
||||
let (roll, pitch, yaw) = ahrs.update(&imu, i * 2000);
|
||||
// Should stay near zero when stationary
|
||||
assert!(roll.abs() < 0.1);
|
||||
assert!(pitch.abs() < 0.1);
|
||||
@@ -644,8 +687,9 @@ mod tests {
|
||||
let mut ahrs = MahonyAhrs::new(config);
|
||||
|
||||
// Complete calibration
|
||||
let imu1 = RawImuData::new([100, 0, 0], [0, 0, 1000]);
|
||||
for i in 0..5 {
|
||||
ahrs.update(100, 0, 0, 0, 0, 1000, i * 2000);
|
||||
ahrs.update(&imu1, i * 2000);
|
||||
}
|
||||
assert!(ahrs.is_calibrated());
|
||||
assert_relative_eq!(ahrs.gyro_bias()[0], 100.0, epsilon = 0.1);
|
||||
@@ -655,8 +699,9 @@ mod tests {
|
||||
assert!(!ahrs.is_calibrated());
|
||||
|
||||
// New calibration with different bias
|
||||
let imu2 = RawImuData::new([50, 0, 0], [0, 0, 1000]);
|
||||
for i in 0..5 {
|
||||
ahrs.update(50, 0, 0, 0, 0, 1000, (i + 10) * 2000);
|
||||
ahrs.update(&imu2, (i + 10) * 2000);
|
||||
}
|
||||
assert!(ahrs.is_calibrated());
|
||||
assert_relative_eq!(ahrs.gyro_bias()[0], 50.0, epsilon = 0.1);
|
||||
|
||||
@@ -31,7 +31,8 @@
|
||||
//!
|
||||
//! // Use Mahony AHRS for orientation with automatic bias calibration
|
||||
//! let mut ahrs = MahonyAhrs::new(MahonyConfig::default());
|
||||
//! let (roll, pitch, yaw) = ahrs.update(gyro_x, gyro_y, gyro_z, tilt_x, tilt_y, tilt_z, timestamp_us);
|
||||
//! let imu = RawImuData::new(gyro_raw, tilt_raw);
|
||||
//! let (roll, pitch, yaw) = ahrs.update(&imu, timestamp_us);
|
||||
//! ```
|
||||
|
||||
mod complementary;
|
||||
@@ -43,5 +44,7 @@ mod wheel_odometry;
|
||||
pub use complementary::{CRL200S_GYRO_SCALE, ComplementaryConfig, ComplementaryFilter};
|
||||
pub use eskf::{Eskf, EskfConfig, MeasurementNoise, ProcessNoise};
|
||||
pub use evaluator::{EvaluationResult, OdometryEvaluator, ScenarioBounds, Stats, TestScenario};
|
||||
pub use mahony::{EulerAngles, MahonyAhrs, MahonyConfig, Quaternion};
|
||||
pub use mahony::{
|
||||
CalibratedImuData, EulerAngles, MahonyAhrs, MahonyConfig, Quaternion, RawImuData,
|
||||
};
|
||||
pub use wheel_odometry::{WheelOdometry, WheelOdometryConfig};
|
||||
|
||||
@@ -310,7 +310,7 @@ fn test_filter_straight_line_no_gyro() {
|
||||
let mut timestamp = 0u64;
|
||||
for (left, right) in ticks {
|
||||
if let Some(delta) = odom.update(left, right) {
|
||||
timestamp += 2000; // 2ms per update
|
||||
timestamp += 9091; // 9.09ms per update (110Hz)
|
||||
filter.update(delta, 0, timestamp);
|
||||
}
|
||||
}
|
||||
@@ -472,7 +472,7 @@ fn run_straight_line_evaluation(
|
||||
for (left, right) in ticks {
|
||||
if let Some(delta) = odom.update(left, right) {
|
||||
pose = pose.compose(&delta);
|
||||
timestamp += 2000; // 2ms per update
|
||||
timestamp += 9091; // 9.09ms per update (110Hz)
|
||||
evaluator.add_estimate(pose, timestamp);
|
||||
}
|
||||
}
|
||||
@@ -483,7 +483,7 @@ fn run_straight_line_evaluation(
|
||||
for (left, right) in return_ticks {
|
||||
if let Some(delta) = odom.update(left, right) {
|
||||
pose = pose.compose(&delta);
|
||||
timestamp += 2000;
|
||||
timestamp += 9091;
|
||||
evaluator.add_estimate(pose, timestamp);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -91,7 +91,7 @@ fn test_sangam_connection() {
|
||||
println!("Encoder messages: {}", encoder_count);
|
||||
println!("Gyro messages: {}", gyro_count);
|
||||
|
||||
// We should receive some encoder data at least (500Hz from sensor_status)
|
||||
// We should receive some encoder data at least (110Hz from sensor_status)
|
||||
assert!(encoder_count > 0, "Should receive encoder data");
|
||||
}
|
||||
|
||||
@@ -153,8 +153,8 @@ fn test_wheel_odometry_hardware() {
|
||||
println!("Collecting wheel odometry data for ~1 second...");
|
||||
println!("Keep the robot stationary for baseline test.");
|
||||
|
||||
// Collect ~500 samples (at 500Hz, this is ~1 second)
|
||||
for _ in 0..500 {
|
||||
// Collect ~110 samples (at 110Hz, this is ~1 second)
|
||||
for _ in 0..110 {
|
||||
match client.recv() {
|
||||
Ok(msg) => {
|
||||
if let Some((left, right)) = msg.encoder_ticks()
|
||||
@@ -224,7 +224,7 @@ fn test_complementary_filter_hardware() {
|
||||
alpha: 0.98,
|
||||
gyro_scale: 0.001, // Estimated, needs calibration based on IMU datasheet
|
||||
gyro_bias_z: 0.0, // Should be calibrated at startup when stationary
|
||||
..ComplementaryConfig::default() // Uses gyro_sign=-1.0 for CRL-200S
|
||||
..ComplementaryConfig::default() // Uses gyro_sign=-1.0 for CRL-200S
|
||||
};
|
||||
let mut filter = ComplementaryFilter::new(filter_config);
|
||||
|
||||
@@ -234,8 +234,8 @@ fn test_complementary_filter_hardware() {
|
||||
println!("Running complementary filter for ~2 seconds...");
|
||||
println!("Move the robot to see pose changes, or keep stationary for baseline.");
|
||||
|
||||
// Collect ~1000 samples (at 500Hz, this is ~2 seconds)
|
||||
for _ in 0..1000 {
|
||||
// Collect ~220 samples (at 110Hz, this is ~2 seconds)
|
||||
for _ in 0..220 {
|
||||
match client.recv() {
|
||||
Ok(msg) => {
|
||||
// Get encoder data
|
||||
|
||||
+1
-1
@@ -31,7 +31,7 @@ python drishti.py --robot 192.168.68.101
|
||||
## Features
|
||||
|
||||
- Full-screen robot diagram with sensor overlays
|
||||
- Real-time telemetry monitoring (500Hz)
|
||||
- Real-time telemetry monitoring (110Hz)
|
||||
- JSON protocol over TCP
|
||||
- Visual indicators for all 13 sensors
|
||||
- Drive control with arrow keys
|
||||
|
||||
@@ -35,7 +35,7 @@ class IMUProcessor:
|
||||
# Gravity scale (LP filtered gravity vector)
|
||||
GRAVITY_SCALE = 1.0 / 1000.0
|
||||
|
||||
def __init__(self, sample_rate_hz: float = 500.0):
|
||||
def __init__(self, sample_rate_hz: float = 110.0):
|
||||
self.dt = 1.0 / sample_rate_hz
|
||||
|
||||
# Quaternion [w, x, y, z]
|
||||
@@ -48,7 +48,7 @@ class IMUProcessor:
|
||||
self.gyro_bias = [0.0, 0.0, 0.0]
|
||||
self._calibration_samples = []
|
||||
self._calibration_count = 0
|
||||
self._calibration_duration = 1500 # ~3 seconds at 500Hz
|
||||
self._calibration_duration = 330 # ~3 seconds at 110Hz
|
||||
|
||||
# UI decimation
|
||||
self._counter = 0
|
||||
|
||||
@@ -1062,7 +1062,7 @@ class Robot3DView(QWidget):
|
||||
"""
|
||||
Update orientation from IMU data.
|
||||
|
||||
Called at 500Hz from telemetry thread. Updates every call for
|
||||
Called at 110Hz from telemetry thread. Updates every call for
|
||||
smooth real-time response (labels decimated separately).
|
||||
|
||||
Args:
|
||||
|
||||
@@ -94,7 +94,7 @@ fn checksum(data: &[u8]) -> u16 {
|
||||
| 0x07 | Version Response | Variable | 2x | HIGH | Response to version request |
|
||||
| 0x08 | Init Ack | None | 1x | HIGH | Echo acknowledgment |
|
||||
| 0x0C | Protocol Sync Ack | 1 byte | 1x | HIGH | Echo of 0x0C command (~270ms delay) |
|
||||
| 0x15 | Status Packet | 96 bytes | 12993x | HIGH | Continuous sensor data @ ~500Hz |
|
||||
| 0x15 | Status Packet | 96 bytes | 12993x | HIGH | Continuous sensor data @ ~110Hz |
|
||||
| 0x17 | Lidar Config Ack | 1 byte | 1x | LOW | Response `[0x01]` |
|
||||
| 0x18 | Lidar Query Response | 8 bytes | 1x | LOW | Lidar state data |
|
||||
| 0xA2 | IMU Cal Response | 1 byte | 3x | HIGH | Response `[0x01]` |
|
||||
|
||||
+3
-3
@@ -4,7 +4,7 @@ Hardware abstraction daemon for the CRL-200S robotic vacuum platform, providing
|
||||
|
||||
## Overview
|
||||
|
||||
SangamIO acts as a bridge between high-level clients (SLAM, visualization) and low-level hardware (GD32 motor controller, Delta-2D lidar). It runs as a standalone daemon on the robot, streaming sensor data at 500Hz and accepting commands over TCP.
|
||||
SangamIO acts as a bridge between high-level clients (SLAM, visualization) and low-level hardware (GD32 motor controller, Delta-2D lidar). It runs as a standalone daemon on the robot, streaming sensor data at 110Hz (limited by 115200 baud serial) and accepting commands over TCP.
|
||||
|
||||
```
|
||||
┌─────────────────────────────────────────────────┐
|
||||
@@ -13,7 +13,7 @@ SangamIO acts as a bridge between high-level clients (SLAM, visualization) and l
|
||||
│ TCP 5555 │
|
||||
┌─────────────▼───────────────▼───────────────────┐
|
||||
│ SangamIO Daemon (Robot) │
|
||||
│ • Telemetry streaming @ 500Hz (sensors) │
|
||||
│ • Telemetry streaming @ 110Hz (sensors) │
|
||||
│ • Lidar streaming @ 5Hz (360° scans) │
|
||||
│ • Command processing (motion/actuators) │
|
||||
│ • Real-time control loop (20ms heartbeat) │
|
||||
@@ -167,7 +167,7 @@ See `proto/sangamio.proto` for the complete schema.
|
||||
|
||||
| Direction | Topic | Rate | Description |
|
||||
|-----------|-------|------|-------------|
|
||||
| Out | `sensors/sensor_status` | 500Hz | All sensor data |
|
||||
| Out | `sensors/sensor_status` | 110Hz | All sensor data |
|
||||
| Out | `sensors/device_version` | Once | Version info |
|
||||
| Out | `sensors/lidar` | 5Hz | 360° point cloud |
|
||||
| In | `command` | On-demand | Robot commands |
|
||||
|
||||
@@ -1,13 +1,13 @@
|
||||
# GD32 Sensor Status Packet (0x15)
|
||||
|
||||
This document details the 96-byte sensor status packet sent by the GD32 motor controller at approximately 500Hz.
|
||||
This document details the 96-byte sensor status packet sent by the GD32 motor controller at approximately 110Hz (limited by 115200 baud serial).
|
||||
|
||||
## Packet Structure
|
||||
|
||||
```
|
||||
Packet Format: [0xFA 0xFB] [LEN] [0x15] [PAYLOAD (96 bytes)] [CRC_H] [CRC_L]
|
||||
Total Size: 102 bytes (6 header/CRC + 96 payload)
|
||||
Frequency: ~500 Hz (every 2ms)
|
||||
Frequency: ~110 Hz (every ~9ms, limited by 115200 baud)
|
||||
```
|
||||
|
||||
## Payload Byte Map
|
||||
@@ -272,7 +272,7 @@ All multi-byte fields use **little-endian** encoding:
|
||||
|
||||
### Sensor Update Frequency
|
||||
|
||||
- **Status Packets:** ~500 Hz (every 2ms)
|
||||
- **Status Packets:** ~110 Hz (every ~9ms)
|
||||
- **All sensors updated:** Per packet (real-time)
|
||||
- **Encoder Resolution:** Incremental ticks (wrapping u16 counters)
|
||||
|
||||
|
||||
@@ -15,7 +15,7 @@ message Message {
|
||||
}
|
||||
|
||||
// Sensor data container published by the daemon.
|
||||
// Streaming rates: 500Hz for telemetry, 5Hz for lidar scans.
|
||||
// Streaming rates: 110Hz for telemetry, 5Hz for lidar scans.
|
||||
message SensorGroup {
|
||||
// Group identifier matching topic suffix (e.g., "sensor_status", "lidar")
|
||||
string group_id = 1;
|
||||
|
||||
@@ -12,7 +12,7 @@ use std::sync::{Arc, Mutex};
|
||||
pub struct DriverInitResult {
|
||||
/// Shared sensor data for polling-based access (for low-rate sensors like lidar)
|
||||
pub sensor_data: HashMap<String, Arc<Mutex<SensorGroupData>>>,
|
||||
/// Optional streaming channels for high-rate sensors (e.g., 500Hz GD32 status)
|
||||
/// Optional streaming channels for high-rate sensors (e.g., 110Hz GD32 status)
|
||||
/// Key is group_id (e.g., "sensor_status")
|
||||
pub stream_receivers: HashMap<String, StreamReceiver>,
|
||||
}
|
||||
@@ -63,7 +63,7 @@ pub trait DeviceDriver: Send {
|
||||
///
|
||||
/// Returns [`DriverInitResult`] containing:
|
||||
/// - `sensor_data`: Sensor groups keyed by group ID (e.g., "sensor_status", "lidar")
|
||||
/// - `stream_receivers`: Optional streaming channels for high-rate sensors (e.g., 500Hz)
|
||||
/// - `stream_receivers`: Optional streaming channels for high-rate sensors (e.g., 110Hz)
|
||||
///
|
||||
/// The TCP publisher uses sensor_data for low-rate polling and stream_receivers
|
||||
/// for high-rate streaming without data loss.
|
||||
|
||||
@@ -100,7 +100,7 @@ impl SensorGroupData {
|
||||
/// Update timestamp to current time and increment sequence number.
|
||||
///
|
||||
/// The sequence number ensures each update is detected by the TCP publisher
|
||||
/// even when timestamps appear equal (e.g., at high update rates like 500Hz).
|
||||
/// even when timestamps appear equal (e.g., at high update rates like 110Hz).
|
||||
#[inline]
|
||||
pub fn touch(&mut self) {
|
||||
self.timestamp_us = std::time::SystemTime::now()
|
||||
@@ -189,8 +189,8 @@ pub enum Command {
|
||||
}
|
||||
|
||||
/// Bounded channel capacity for streaming sensor data.
|
||||
/// At 500Hz with ~150 bytes per message, 1000 messages ≈ 150KB buffer.
|
||||
/// This allows ~2 seconds of buffering before dropping messages.
|
||||
/// At 110Hz with ~150 bytes per message, 1000 messages ≈ 150KB buffer.
|
||||
/// This allows ~9 seconds of buffering before dropping messages.
|
||||
pub const STREAM_CHANNEL_CAPACITY: usize = 1000;
|
||||
|
||||
/// Streaming channel for sensor data.
|
||||
|
||||
@@ -17,7 +17,7 @@
|
||||
//! - Uses blocking mutex locks to guarantee timing
|
||||
//!
|
||||
//! 2. **Reader Thread** (continuous):
|
||||
//! - Parses incoming status packets (CMD=0x15 @ ~500Hz)
|
||||
//! - Parses incoming status packets (CMD=0x15 @ ~110Hz)
|
||||
//! - Updates shared sensor data in-place (no allocations)
|
||||
//! - Requests version info after first packet
|
||||
//! - Handles protocol synchronization and checksums
|
||||
@@ -153,7 +153,7 @@ impl GD32Driver {
|
||||
/// # Arguments
|
||||
/// - `sensor_data`: Shared mutex for latest sensor values (for polling-based access)
|
||||
/// - `version_data`: Shared mutex for version info (one-time update)
|
||||
/// - `stream_tx`: Optional channel sender for streaming sensor data at full 500Hz rate
|
||||
/// - `stream_tx`: Optional channel sender for streaming sensor data at full 110Hz rate
|
||||
/// - `gyro_transform`: Axis transform for gyroscope data (identity = no change)
|
||||
/// - `accel_transform`: Axis transform for accelerometer data (identity = no change)
|
||||
pub fn start(
|
||||
|
||||
@@ -22,7 +22,7 @@ pub const MAX_PAYLOAD_SIZE: usize = 128;
|
||||
/// Zero-allocation parsed packet from GD32
|
||||
///
|
||||
/// Uses a fixed-size array instead of `Vec<u8>` to eliminate heap allocations.
|
||||
/// At 500Hz packet rate, this saves ~48KB/sec of allocations.
|
||||
/// At 110Hz packet rate, this saves ~11KB/sec of allocations.
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub struct RxPacket {
|
||||
pub cmd: u8,
|
||||
@@ -72,7 +72,7 @@ impl Default for RxPacket {
|
||||
/// Simple Vec-based packet reader for reliable parsing
|
||||
///
|
||||
/// Uses a plain Vec<u8> with drain() for simplicity and correctness.
|
||||
/// At 500Hz with ~100 byte packets, the O(n) drain is negligible.
|
||||
/// At 110Hz with ~100 byte packets, the O(n) drain is negligible.
|
||||
pub struct PacketReader {
|
||||
buffer: Vec<u8>,
|
||||
/// Reusable packet buffer - avoids allocation on every read
|
||||
|
||||
@@ -35,7 +35,7 @@ use std::time::Duration;
|
||||
///
|
||||
/// # Packet Handling
|
||||
///
|
||||
/// - **Status packets (CMD=0x15)**: Arrive at ~500Hz, contain all sensor data (bumpers, encoders, etc.)
|
||||
/// - **Status packets (CMD=0x15)**: Arrive at ~110Hz, contain all sensor data (bumpers, encoders, etc.)
|
||||
/// - **Version response (CMD=0x07)**: Sent once after we request it following first packet
|
||||
///
|
||||
/// # Version Request Flow
|
||||
@@ -129,7 +129,7 @@ pub(super) fn reader_loop(
|
||||
&gyro_transform,
|
||||
&accel_transform,
|
||||
) {
|
||||
// Push to streaming channel if available (for 500Hz TCP streaming)
|
||||
// Push to streaming channel if available (for 110Hz TCP streaming)
|
||||
if let Some(ref tx) = stream_tx {
|
||||
// Use try_send to avoid blocking - drop message if channel full
|
||||
if tx.try_send(cloned).is_err() {
|
||||
@@ -150,7 +150,7 @@ pub(super) fn reader_loop(
|
||||
}
|
||||
}
|
||||
// Note: No sleep here! The serial port read is blocking with timeout.
|
||||
// At 500Hz, we need to process every packet immediately.
|
||||
// At 110Hz, we need to process every packet immediately.
|
||||
}
|
||||
|
||||
log::info!("Reader thread exiting");
|
||||
|
||||
@@ -2,7 +2,7 @@
|
||||
//!
|
||||
//! Supports two modes for different data rates:
|
||||
//! - **Polling mode**: For low-rate sensors (lidar @ 5Hz), polls shared mutex
|
||||
//! - **Streaming mode**: For high-rate sensors (GD32 @ 500Hz), consumes from channel
|
||||
//! - **Streaming mode**: For high-rate sensors (GD32 @ 110Hz), consumes from channel
|
||||
|
||||
use crate::core::types::{SensorGroupData, StreamReceiver};
|
||||
use crate::error::Result;
|
||||
@@ -20,7 +20,7 @@ pub struct TcpPublisher {
|
||||
serializer: Serializer,
|
||||
/// Mutex-based sensor data for low-rate polling (lidar, version)
|
||||
sensor_data: HashMap<String, Arc<Mutex<SensorGroupData>>>,
|
||||
/// Channel-based receivers for high-rate streaming (sensor_status @ 500Hz)
|
||||
/// Channel-based receivers for high-rate streaming (sensor_status @ 110Hz)
|
||||
stream_receivers: HashMap<String, StreamReceiver>,
|
||||
running: Arc<AtomicBool>,
|
||||
}
|
||||
@@ -67,7 +67,7 @@ impl TcpPublisher {
|
||||
while self.running.load(Ordering::Relaxed) {
|
||||
let mut sent_any = false;
|
||||
|
||||
// Phase 1: Drain all streaming channels (high-rate sensors like GD32 @ 500Hz)
|
||||
// Phase 1: Drain all streaming channels (high-rate sensors like GD32 @ 110Hz)
|
||||
for (group_id, rx) in &self.stream_receivers {
|
||||
loop {
|
||||
match rx.try_recv() {
|
||||
|
||||
@@ -22,7 +22,7 @@
|
||||
//!
|
||||
//! - **Latency**: <1ms serialization time (typical)
|
||||
//! - **Throughput**: ~50,000 messages/sec
|
||||
//! - **Bandwidth**: Sensor stream @ 500Hz ≈ 60KB/s
|
||||
//! - **Bandwidth**: Sensor stream @ 110Hz ≈ 13KB/s
|
||||
|
||||
use crate::core::types::{Command, SensorGroupData};
|
||||
use crate::error::{Error, Result};
|
||||
@@ -79,7 +79,7 @@ pub fn create_serializer() -> Serializer {
|
||||
impl proto::Message {
|
||||
/// Create a sensor group message from internal types
|
||||
///
|
||||
/// Uses references where possible to minimize allocations in hot path (~500Hz).
|
||||
/// Uses references where possible to minimize allocations in hot path (~110Hz).
|
||||
pub fn from_sensor_group(data: &SensorGroupData) -> Self {
|
||||
use std::collections::HashMap;
|
||||
|
||||
@@ -107,7 +107,7 @@ 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
|
||||
/// This is the preferred method in hot paths (~110Hz) 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;
|
||||
|
||||
Reference in New Issue
Block a user