Merge pull request #2 from gkfabs/feat/roborock-s5max-driver-pr
feat(sangam-io): add native Roborock S5 Max support
This commit is contained in:
Harishankar
@@ -6,3 +6,8 @@
|
||||
[target.armv7-unknown-linux-musleabihf]
|
||||
linker = "armv7-unknown-linux-musleabihf-gcc"
|
||||
rustflags = ["-C", "target-feature=+neon", "-C", "target-cpu=cortex-a7"]
|
||||
|
||||
# Roborock S5 Max stock firmware uses glibc/gnueabihf, not musl.
|
||||
[target.armv7-unknown-linux-gnueabihf]
|
||||
linker = "arm-linux-gnueabihf-gcc"
|
||||
rustflags = ["-C", "target-feature=+neon", "-C", "target-cpu=cortex-a7"]
|
||||
|
||||
+149
-3
@@ -1,6 +1,11 @@
|
||||
# SangamIO
|
||||
|
||||
Hardware abstraction daemon for the CRL-200S robotic vacuum platform, providing real-time sensor streaming via UDP and command control via TCP.
|
||||
Hardware abstraction daemon for robotic vacuum platforms, providing real-time sensor streaming via UDP and command control via TCP.
|
||||
|
||||
Supported hardware:
|
||||
|
||||
- CRL-200S
|
||||
- Roborock S5 Max (rooted firmware `4.1.2_1668`)
|
||||
|
||||
## Overview
|
||||
|
||||
@@ -73,6 +78,18 @@ arm-linux-gnueabihf-strip \
|
||||
target/armv7-unknown-linux-musleabihf/release/sangam-io
|
||||
```
|
||||
|
||||
The Roborock S5 Max stock image uses glibc rather than musl:
|
||||
|
||||
```bash
|
||||
rustup target add armv7-unknown-linux-gnueabihf
|
||||
cargo build --release --target armv7-unknown-linux-gnueabihf
|
||||
```
|
||||
|
||||
The S5 Max sequence helper is firmware-specific. Its current offset is for
|
||||
firmware `4.1.2_1668` and `libuart_api` Build ID
|
||||
`8d639ff160b8766b7bb87e848e9b0a9c0ff12a08`; re-derive the offset before using
|
||||
it with another firmware build.
|
||||
|
||||
### Feature Flags
|
||||
|
||||
| Feature | Description |
|
||||
@@ -101,8 +118,137 @@ ssh root@vacuum "RUST_LOG=info /usr/sbin/sangamio"
|
||||
|
||||
> **Important**: Always overwrite `/usr/sbin/sangamio` directly. The robot monitor auto-restarts processes, so renaming AuxCtrl prevents conflicts.
|
||||
|
||||
### Roborock S5 Max live takeover
|
||||
|
||||
The S5 Max driver must exclusively own `/dev/ttyS2`, `/dev/ttyS1`,
|
||||
`/dev/lds_motor`, and `/dev/watchdog`. The stock `rr_loader` and `WatchDoge`
|
||||
processes therefore cannot run alongside SangamIO. The MCU also rejects stale
|
||||
transmit sequence numbers, so a live handoff must capture the last stock
|
||||
sequence while `rr_loader` is frozen.
|
||||
|
||||
The procedure below was validated on rooted firmware `4.1.2_1668`. Keep a
|
||||
second terminal open. A forced reboot is the recovery path and restores the
|
||||
stock stack because the `/etc/inittab` override is a temporary bind mount.
|
||||
|
||||
Build and upload the daemon, helper, and base configuration:
|
||||
|
||||
```bash
|
||||
TARGET=armv7-unknown-linux-gnueabihf
|
||||
cargo build --release --target "$TARGET" \
|
||||
--bin sangam-io --bin roborock_s5max_sequence
|
||||
scp -O \
|
||||
"target/$TARGET/release/sangam-io" \
|
||||
"target/$TARGET/release/roborock_s5max_sequence" \
|
||||
roborock-s5max.toml root@vacuum:/mnt/data/
|
||||
```
|
||||
|
||||
Then perform the handoff as root on the robot:
|
||||
|
||||
```sh
|
||||
BIN=/mnt/data/sangam-io
|
||||
SEQ=/mnt/data/roborock_s5max_sequence
|
||||
BASE=/mnt/data/roborock-s5max.toml
|
||||
LIVE=/tmp/roborock-s5max-live.toml
|
||||
|
||||
# Every exit after this point restores the stock stack by rebooting.
|
||||
recover()
|
||||
{
|
||||
trap - EXIT HUP INT TERM
|
||||
sync
|
||||
/sbin/reboot -f
|
||||
}
|
||||
trap recover EXIT HUP INT TERM
|
||||
|
||||
chmod 755 "$BIN" "$SEQ"
|
||||
WD_PIDS="$(pidof WatchDoge)"
|
||||
RR_PIDS="$(pidof rr_loader)"
|
||||
|
||||
# Select the rr_loader instance that owns the MCU UART.
|
||||
RR_PID=""
|
||||
for pid in $RR_PIDS; do
|
||||
for fd in /proc/$pid/fd/*; do
|
||||
if [ "$(readlink "$fd" 2>/dev/null)" = /dev/ttyS2 ]; then
|
||||
RR_PID="$pid"
|
||||
break 2
|
||||
fi
|
||||
done
|
||||
done
|
||||
test -n "$WD_PIDS" && test -n "$RR_PID" || exit 1
|
||||
|
||||
# Prevent init from immediately respawning WatchDoge during takeover.
|
||||
grep -v WatchDoge /etc/inittab > /tmp/inittab.no-watchdog
|
||||
mount --bind /tmp/inittab.no-watchdog /etc/inittab
|
||||
kill -HUP 1
|
||||
sleep 1
|
||||
|
||||
# Freeze every process that can race the handoff, then read the sequence.
|
||||
kill -STOP $WD_PIDS $RR_PIDS
|
||||
kill -STOP $(pidof RoboController AppProxy 2>/dev/null) 2>/dev/null
|
||||
LAST_SEQUENCE="$($SEQ "$RR_PID")" || exit 1
|
||||
echo "Captured stock MCU sequence: $LAST_SEQUENCE"
|
||||
|
||||
# Let TERM cleanup release the watchdog, UARTs, and LDS motor.
|
||||
kill -TERM $WD_PIDS $RR_PIDS
|
||||
kill -CONT $WD_PIDS $RR_PIDS
|
||||
sleep 2
|
||||
for pid in $WD_PIDS $RR_PIDS; do
|
||||
kill -0 "$pid" 2>/dev/null && kill -KILL "$pid"
|
||||
done
|
||||
|
||||
sed "s/^# last_stock_sequence = 0/last_stock_sequence = $LAST_SEQUENCE/" \
|
||||
"$BASE" > "$LIVE"
|
||||
grep '^last_stock_sequence' "$LIVE" >/dev/null || exit 1
|
||||
RUST_LOG=info "$BIN" --config "$LIVE"
|
||||
```
|
||||
|
||||
For the default read-only configuration, leave `allow_actuation = false`.
|
||||
This provides MCU telemetry only: starting the LDS requires charger and
|
||||
subsystem-control writes, so lidar remains disabled behind the actuation gate.
|
||||
Set `allow_actuation = true` only after reviewing the safety limits and only
|
||||
when a command client will control the robot.
|
||||
|
||||
To stop SangamIO, send `SIGINT` from the second terminal. Its ordered shutdown
|
||||
stops wheels, cleaning motors, and the LDS; the shell's exit trap then reboots
|
||||
the robot and restores all stock processes:
|
||||
|
||||
```sh
|
||||
kill -INT $(pidof sangam-io)
|
||||
```
|
||||
|
||||
If the daemon or SSH session fails, use `/sbin/reboot -f` from the second
|
||||
terminal. If SSH is unavailable, the hardware watchdog should reboot the robot
|
||||
after its 16-second timeout. Do not try to restart only part of the stock stack
|
||||
after a takeover.
|
||||
|
||||
## Configuration
|
||||
|
||||
For the Roborock S5 Max, start with `roborock-s5max.toml`. Actuation is
|
||||
disabled by default and live takeover must continue the stock MCU transmit
|
||||
sequence using `roborock_s5max_sequence`.
|
||||
|
||||
The S5 Max publishes its detailed native values alongside the established
|
||||
`sensor_status` keys expected by Dhruva clients:
|
||||
|
||||
| Common key | S5 Max source |
|
||||
|------------|---------------|
|
||||
| `wheel_left`, `wheel_right` | Low 16 bits of the signed 32-bit monotonic odometry counters; wrapping deltas remain valid |
|
||||
| `gyro_x`, `gyro_y`, `gyro_z` | MCU angular rates converted from rad/s to signed milliradians/s |
|
||||
| `bumper_left`, `bumper_right` | Conservative aliases: both become true when either raw bumper channel is active |
|
||||
| `cliff_left_side`, `cliff_left_front`, `cliff_right_front`, `cliff_right_side` | Conservative aliases: all become true when any raw cliff channel is active |
|
||||
| `battery_voltage`, `battery_level` | MCU BMS voltage and state-of-charge report |
|
||||
| `is_dock_connected`, `is_charging` | MCU dock-state report |
|
||||
| `dustbox_attached` | MCU dustbin presence report |
|
||||
|
||||
The conservative bumper and cliff aliases avoid understating a safety event;
|
||||
their physical channel ordering has not yet been proven. Exact values remain
|
||||
available as `bumper_raw` and `cliff_raw`. Native signed wheel counters remain
|
||||
available as `wheel_left_ticks` and `wheel_right_ticks`.
|
||||
|
||||
The `c0` drive command fields are mean encoder ticks per 20 ms and body yaw in
|
||||
radians per second. This was established from stock captures (forced turns use
|
||||
exactly `pi/2`, while the measured yaw rate settles near `1.57`) and exercised
|
||||
on hardware by the bounded wheel-deadman test and autonomous mapping runs.
|
||||
|
||||
Edit `sangamio.toml`:
|
||||
|
||||
```toml
|
||||
@@ -311,8 +457,8 @@ Commands use a unified `ComponentControl` interface:
|
||||
|-----------|--------|---------|-----------|
|
||||
| `drive` | Mode 0x02 | Stop + Mode 0x00 | `linear`, `angular` (m/s, rad/s) |
|
||||
| `vacuum` | 100% | 0% | `speed` (0-100%) |
|
||||
| `main_brush` | 100% | 0% | `speed` (0-100%) |
|
||||
| `side_brush` | 100% | 0% | `speed` (0-100%) |
|
||||
| `main_brush` | 100% (S5 Max: stock 71%) | 0% | `speed` (0-100%) |
|
||||
| `side_brush` | 100% (S5 Max: stock 30%) | 0% | `speed` (0-100%) |
|
||||
| `water_pump` | 100% | 0% | `speed` (0-100%) |
|
||||
| `lidar` | Power on | Power off | - |
|
||||
| `led` | - | - | `state` (0-18) |
|
||||
|
||||
@@ -0,0 +1,23 @@
|
||||
# SangamIO configuration for a rooted Roborock S5 Max.
|
||||
#
|
||||
# Actuation remains disabled unless explicitly enabled below.
|
||||
|
||||
[device]
|
||||
type = "roborock_s5max"
|
||||
name = "Roborock S5 Max"
|
||||
|
||||
[device.roborock_s5max]
|
||||
mcu_port = "/dev/ttyS2"
|
||||
lds_port = "/dev/ttyS1"
|
||||
lds_motor = "/dev/lds_motor"
|
||||
watchdog = "/dev/watchdog"
|
||||
wheel_mm_per_tick = 0.798
|
||||
wheel_track_m = 0.229
|
||||
lds_forward_angle_deg = 261.2
|
||||
allow_actuation = false
|
||||
# Required at runtime. Use 0 only when SangamIO owns the MCU from reset; a live
|
||||
# handoff must set the final sequence captured from the frozen stock sender.
|
||||
# last_stock_sequence = 0
|
||||
|
||||
[network]
|
||||
bind_address = "0.0.0.0:5555"
|
||||
@@ -0,0 +1,76 @@
|
||||
//! Read the stock S5 Max AP transmit sequence from a frozen `rr_loader`.
|
||||
//!
|
||||
//! This is deliberately a separate handoff utility: the robot's 32-bit `dd`
|
||||
//! cannot seek to `/proc/PID/mem` addresses above `i32::MAX`.
|
||||
|
||||
use std::env;
|
||||
use std::fs;
|
||||
use std::fs::File;
|
||||
use std::io::{Read, Seek, SeekFrom};
|
||||
use std::process::ExitCode;
|
||||
|
||||
const LIBRARY_NAME: &str = "libuart_api";
|
||||
// Offset of g_stRuaUartInfo's transmit sequence in the libuart_api build from
|
||||
// Roborock firmware 4.1.2_1668 (Build ID
|
||||
// 8d639ff160b8766b7bb87e848e9b0a9c0ff12a08). Re-derive this offset before
|
||||
// using the handoff helper with any other firmware/library build.
|
||||
const SEQUENCE_OFFSET: u64 = 0x2ff4d;
|
||||
|
||||
fn run() -> Result<u8, String> {
|
||||
let mut arguments = env::args().skip(1);
|
||||
let pid = arguments
|
||||
.next()
|
||||
.ok_or_else(|| "usage: roborock_s5max_sequence <rr_loader-pid>".to_string())?;
|
||||
if arguments.next().is_some() || pid.parse::<u32>().is_err() {
|
||||
return Err("usage: roborock_s5max_sequence <rr_loader-pid>".to_string());
|
||||
}
|
||||
|
||||
let maps_path = format!("/proc/{pid}/maps");
|
||||
let maps =
|
||||
fs::read_to_string(&maps_path).map_err(|error| format!("read {maps_path}: {error}"))?;
|
||||
let load_bias = maps
|
||||
.lines()
|
||||
.find_map(|line| {
|
||||
let mut fields = line.split_whitespace();
|
||||
let range = fields.next()?;
|
||||
fields.next()?;
|
||||
let file_offset = fields.next()?;
|
||||
if !line.contains(LIBRARY_NAME) {
|
||||
return None;
|
||||
}
|
||||
let start = range.split_once('-')?.0;
|
||||
let start = u64::from_str_radix(start, 16).ok()?;
|
||||
let file_offset = u64::from_str_radix(file_offset, 16).ok()?;
|
||||
start.checked_sub(file_offset)
|
||||
})
|
||||
.ok_or_else(|| format!("cannot find {LIBRARY_NAME} mapping in {maps_path}"))?;
|
||||
|
||||
let memory_path = format!("/proc/{pid}/mem");
|
||||
let mut memory =
|
||||
File::open(&memory_path).map_err(|error| format!("open {memory_path}: {error}"))?;
|
||||
let address = load_bias
|
||||
.checked_add(SEQUENCE_OFFSET)
|
||||
.ok_or_else(|| "sequence address overflow".to_string())?;
|
||||
memory
|
||||
.seek(SeekFrom::Start(address))
|
||||
.map_err(|error| format!("seek {memory_path} to 0x{address:x}: {error}"))?;
|
||||
|
||||
let mut sequence = [0_u8; 1];
|
||||
memory
|
||||
.read_exact(&mut sequence)
|
||||
.map_err(|error| format!("read sequence at 0x{address:x}: {error}"))?;
|
||||
Ok(sequence[0])
|
||||
}
|
||||
|
||||
fn main() -> ExitCode {
|
||||
match run() {
|
||||
Ok(sequence) => {
|
||||
println!("{sequence}");
|
||||
ExitCode::SUCCESS
|
||||
}
|
||||
Err(error) => {
|
||||
eprintln!("{error}");
|
||||
ExitCode::FAILURE
|
||||
}
|
||||
}
|
||||
}
|
||||
+249
-7
@@ -305,7 +305,7 @@ impl LidarMountingConfig {
|
||||
// Hardware Configuration
|
||||
// ============================================================================
|
||||
|
||||
/// Hardware configuration for device drivers
|
||||
/// CRL-200S hardware configuration.
|
||||
///
|
||||
/// Specifies serial port paths and timing parameters for hardware communication.
|
||||
#[derive(Debug, Clone, Deserialize)]
|
||||
@@ -369,6 +369,96 @@ fn default_lidar_pwm() -> u8 {
|
||||
60
|
||||
}
|
||||
|
||||
/// Roborock S5 Max hardware configuration.
|
||||
///
|
||||
/// Defaults match firmware 4.1.2_1668 and the measured robot calibration.
|
||||
/// Physical values remain configurable for manufacturing and wear variation.
|
||||
#[derive(Debug, Clone, Deserialize)]
|
||||
pub struct RoborockS5MaxHardwareConfig {
|
||||
/// Main MCU UART (`1,152,000` baud, 8N1).
|
||||
#[serde(default = "default_s5max_mcu_port")]
|
||||
pub mcu_port: String,
|
||||
|
||||
/// LDS UART (`115,200` baud, 8N1).
|
||||
#[serde(default = "default_s5max_lds_port")]
|
||||
pub lds_port: String,
|
||||
|
||||
/// Kernel LDS motor-control device.
|
||||
#[serde(default = "default_s5max_lds_motor")]
|
||||
pub lds_motor: String,
|
||||
|
||||
/// Hardware watchdog device inherited from the stock stack.
|
||||
#[serde(default = "default_s5max_watchdog")]
|
||||
pub watchdog: String,
|
||||
|
||||
/// Linear distance represented by one wheel encoder tick, in millimetres.
|
||||
#[serde(default = "default_s5max_wheel_mm_per_tick")]
|
||||
pub wheel_mm_per_tick: f32,
|
||||
|
||||
/// Effective differential-drive wheel track, in metres.
|
||||
#[serde(default = "default_s5max_wheel_track_m")]
|
||||
pub wheel_track_m: f32,
|
||||
|
||||
/// Raw clockwise LDS angle corresponding to robot-forward, in degrees.
|
||||
#[serde(default = "default_s5max_lds_forward_angle_deg")]
|
||||
pub lds_forward_angle_deg: f32,
|
||||
|
||||
/// Permit commands that can energize motors or alter charging state.
|
||||
#[serde(default)]
|
||||
pub allow_actuation: bool,
|
||||
|
||||
/// Last AP transmit sequence used before this process takes ownership.
|
||||
///
|
||||
/// Set this to zero only when SangamIO owns the MCU from a fresh boot.
|
||||
/// A live stock-process handoff must supply the captured final sequence.
|
||||
#[serde(default)]
|
||||
pub last_stock_sequence: Option<u8>,
|
||||
}
|
||||
|
||||
impl Default for RoborockS5MaxHardwareConfig {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
mcu_port: default_s5max_mcu_port(),
|
||||
lds_port: default_s5max_lds_port(),
|
||||
lds_motor: default_s5max_lds_motor(),
|
||||
watchdog: default_s5max_watchdog(),
|
||||
wheel_mm_per_tick: default_s5max_wheel_mm_per_tick(),
|
||||
wheel_track_m: default_s5max_wheel_track_m(),
|
||||
lds_forward_angle_deg: default_s5max_lds_forward_angle_deg(),
|
||||
allow_actuation: false,
|
||||
last_stock_sequence: None,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn default_s5max_mcu_port() -> String {
|
||||
"/dev/ttyS2".to_string()
|
||||
}
|
||||
|
||||
fn default_s5max_lds_port() -> String {
|
||||
"/dev/ttyS1".to_string()
|
||||
}
|
||||
|
||||
fn default_s5max_lds_motor() -> String {
|
||||
"/dev/lds_motor".to_string()
|
||||
}
|
||||
|
||||
fn default_s5max_watchdog() -> String {
|
||||
"/dev/watchdog".to_string()
|
||||
}
|
||||
|
||||
fn default_s5max_wheel_mm_per_tick() -> f32 {
|
||||
0.798
|
||||
}
|
||||
|
||||
fn default_s5max_wheel_track_m() -> f32 {
|
||||
0.229
|
||||
}
|
||||
|
||||
fn default_s5max_lds_forward_angle_deg() -> f32 {
|
||||
261.2
|
||||
}
|
||||
|
||||
/// Device configuration
|
||||
///
|
||||
/// Describes the robot hardware and sensor specifications.
|
||||
@@ -376,7 +466,8 @@ fn default_lidar_pwm() -> u8 {
|
||||
pub struct DeviceConfig {
|
||||
/// Device type identifier
|
||||
///
|
||||
/// **Valid values**: "crl200s", "mock" (mock requires --features mock)
|
||||
/// **Valid values**: "crl200s", "roborock_s5max", "mock" (mock requires
|
||||
/// `--features mock`)
|
||||
/// **Required**: Yes
|
||||
#[serde(rename = "type")]
|
||||
pub device_type: String,
|
||||
@@ -394,6 +485,13 @@ pub struct DeviceConfig {
|
||||
#[serde(default)]
|
||||
pub hardware: Option<HardwareConfig>,
|
||||
|
||||
/// Roborock S5 Max hardware configuration.
|
||||
///
|
||||
/// **Required**: For "roborock_s5max" device type
|
||||
/// **Ignored**: For other device types
|
||||
#[serde(default)]
|
||||
pub roborock_s5max: Option<RoborockS5MaxHardwareConfig>,
|
||||
|
||||
/// Simulation configuration
|
||||
///
|
||||
/// **Required**: For "mock" device type
|
||||
@@ -482,11 +580,15 @@ impl Config {
|
||||
let config: Config = basic_toml::from_str(&content)
|
||||
.map_err(|e| Error::Config(format!("Failed to parse config: {}", e)))?;
|
||||
|
||||
// Device-specific validation
|
||||
match config.device.device_type.as_str() {
|
||||
config.validate()?;
|
||||
Ok(config)
|
||||
}
|
||||
|
||||
fn validate(&self) -> Result<()> {
|
||||
match self.device.device_type.as_str() {
|
||||
"crl200s" => {
|
||||
// Hardware config is required for CRL-200S
|
||||
let hardware = config.device.hardware.as_ref().ok_or_else(|| {
|
||||
let hardware = self.device.hardware.as_ref().ok_or_else(|| {
|
||||
Error::Config("crl200s device requires [device.hardware] section".to_string())
|
||||
})?;
|
||||
|
||||
@@ -500,10 +602,49 @@ impl Config {
|
||||
)));
|
||||
}
|
||||
}
|
||||
"roborock_s5max" => {
|
||||
let hardware = self.device.roborock_s5max.as_ref().ok_or_else(|| {
|
||||
Error::Config(
|
||||
"roborock_s5max device requires [device.roborock_s5max] section"
|
||||
.to_string(),
|
||||
)
|
||||
})?;
|
||||
|
||||
for (name, path) in [
|
||||
("mcu_port", hardware.mcu_port.as_str()),
|
||||
("lds_port", hardware.lds_port.as_str()),
|
||||
("lds_motor", hardware.lds_motor.as_str()),
|
||||
("watchdog", hardware.watchdog.as_str()),
|
||||
] {
|
||||
if path.trim().is_empty() {
|
||||
return Err(Error::Config(format!(
|
||||
"device.roborock_s5max.{name} must not be empty"
|
||||
)));
|
||||
}
|
||||
}
|
||||
|
||||
if !hardware.wheel_mm_per_tick.is_finite() || hardware.wheel_mm_per_tick <= 0.0 {
|
||||
return Err(Error::Config(
|
||||
"device.roborock_s5max.wheel_mm_per_tick must be finite and positive"
|
||||
.to_string(),
|
||||
));
|
||||
}
|
||||
if !hardware.wheel_track_m.is_finite() || hardware.wheel_track_m <= 0.0 {
|
||||
return Err(Error::Config(
|
||||
"device.roborock_s5max.wheel_track_m must be finite and positive"
|
||||
.to_string(),
|
||||
));
|
||||
}
|
||||
if !hardware.lds_forward_angle_deg.is_finite() {
|
||||
return Err(Error::Config(
|
||||
"device.roborock_s5max.lds_forward_angle_deg must be finite".to_string(),
|
||||
));
|
||||
}
|
||||
}
|
||||
#[cfg(feature = "mock")]
|
||||
"mock" => {
|
||||
// Simulation config is required for mock device
|
||||
let simulation = config.device.simulation.as_ref().ok_or_else(|| {
|
||||
let simulation = self.device.simulation.as_ref().ok_or_else(|| {
|
||||
Error::Config("mock device requires [device.simulation] section".to_string())
|
||||
})?;
|
||||
|
||||
@@ -530,6 +671,107 @@ impl Config {
|
||||
}
|
||||
}
|
||||
|
||||
Ok(config)
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::Config;
|
||||
|
||||
fn parse(text: &str) -> Config {
|
||||
basic_toml::from_str(text).unwrap()
|
||||
}
|
||||
|
||||
const NETWORK: &str = r#"
|
||||
[network]
|
||||
bind_address = "127.0.0.1:5555"
|
||||
"#;
|
||||
|
||||
#[test]
|
||||
fn s5max_section_uses_safe_measured_defaults() {
|
||||
let config = parse(&format!(
|
||||
r#"
|
||||
[device]
|
||||
type = "roborock_s5max"
|
||||
name = "S5 Max"
|
||||
|
||||
[device.roborock_s5max]
|
||||
{NETWORK}
|
||||
"#
|
||||
));
|
||||
config.validate().unwrap();
|
||||
|
||||
let hardware = config.device.roborock_s5max.unwrap();
|
||||
assert_eq!(hardware.mcu_port, "/dev/ttyS2");
|
||||
assert_eq!(hardware.lds_port, "/dev/ttyS1");
|
||||
assert_eq!(hardware.lds_motor, "/dev/lds_motor");
|
||||
assert_eq!(hardware.watchdog, "/dev/watchdog");
|
||||
assert_eq!(hardware.wheel_mm_per_tick, 0.798);
|
||||
assert_eq!(hardware.wheel_track_m, 0.229);
|
||||
assert_eq!(hardware.lds_forward_angle_deg, 261.2);
|
||||
assert!(!hardware.allow_actuation);
|
||||
assert_eq!(hardware.last_stock_sequence, None);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn s5max_section_accepts_explicit_overrides() {
|
||||
let config = parse(&format!(
|
||||
r#"
|
||||
[device]
|
||||
type = "roborock_s5max"
|
||||
name = "S5 Max test fixture"
|
||||
|
||||
[device.roborock_s5max]
|
||||
mcu_port = "/dev/test-mcu"
|
||||
lds_port = "/dev/test-lds"
|
||||
lds_motor = "/dev/test-lds-motor"
|
||||
watchdog = "/dev/test-watchdog"
|
||||
wheel_mm_per_tick = 0.8
|
||||
wheel_track_m = 0.23
|
||||
lds_forward_angle_deg = 270.0
|
||||
allow_actuation = true
|
||||
last_stock_sequence = 127
|
||||
{NETWORK}
|
||||
"#
|
||||
));
|
||||
config.validate().unwrap();
|
||||
|
||||
let hardware = config.device.roborock_s5max.unwrap();
|
||||
assert_eq!(hardware.mcu_port, "/dev/test-mcu");
|
||||
assert_eq!(hardware.wheel_mm_per_tick, 0.8);
|
||||
assert!(hardware.allow_actuation);
|
||||
assert_eq!(hardware.last_stock_sequence, Some(127));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn s5max_requires_its_hardware_section() {
|
||||
let config = parse(&format!(
|
||||
r#"
|
||||
[device]
|
||||
type = "roborock_s5max"
|
||||
name = "S5 Max"
|
||||
{NETWORK}
|
||||
"#
|
||||
));
|
||||
let error = config.validate().unwrap_err().to_string();
|
||||
assert!(error.contains("requires [device.roborock_s5max]"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn s5max_rejects_unsafe_calibration_values() {
|
||||
let config = parse(&format!(
|
||||
r#"
|
||||
[device]
|
||||
type = "roborock_s5max"
|
||||
name = "S5 Max"
|
||||
|
||||
[device.roborock_s5max]
|
||||
wheel_mm_per_tick = 0.0
|
||||
{NETWORK}
|
||||
"#
|
||||
));
|
||||
let error = config.validate().unwrap_err().to_string();
|
||||
assert!(error.contains("wheel_mm_per_tick must be finite and positive"));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -23,6 +23,7 @@
|
||||
//! See [`crl200s::CRL200SDriver`] for a complete example.
|
||||
|
||||
pub mod crl200s;
|
||||
pub mod roborock_s5max;
|
||||
|
||||
#[cfg(feature = "mock")]
|
||||
pub mod mock;
|
||||
@@ -31,6 +32,7 @@ use crate::config::Config;
|
||||
use crate::core::driver::DeviceDriver;
|
||||
use crate::error::{Error, Result};
|
||||
use crl200s::CRL200SDriver;
|
||||
use roborock_s5max::RoborockS5MaxDriver;
|
||||
|
||||
#[cfg(feature = "mock")]
|
||||
use mock::MockDriver;
|
||||
@@ -39,6 +41,7 @@ use mock::MockDriver;
|
||||
///
|
||||
/// Supported device types:
|
||||
/// - `crl200s`: Real CRL-200S robot hardware
|
||||
/// - `roborock_s5max`: Roborock S5 Max hardware
|
||||
/// - `mock`: Simulated robot for algorithm testing (requires `mock` feature)
|
||||
///
|
||||
/// To add a new device type, add a match arm here:
|
||||
@@ -51,6 +54,10 @@ pub fn create_device(config: &Config) -> Result<Box<dyn DeviceDriver>> {
|
||||
let driver = CRL200SDriver::new(config.device.clone())?;
|
||||
Ok(Box::new(driver))
|
||||
}
|
||||
"roborock_s5max" => {
|
||||
let driver = RoborockS5MaxDriver::new(config.device.clone())?;
|
||||
Ok(Box::new(driver))
|
||||
}
|
||||
#[cfg(feature = "mock")]
|
||||
"mock" => {
|
||||
let driver = MockDriver::new(config.device.clone())?;
|
||||
@@ -63,3 +70,33 @@ pub fn create_device(config: &Config) -> Result<Box<dyn DeviceDriver>> {
|
||||
_ => Err(Error::UnknownDevice(config.device.device_type.clone())),
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::create_device;
|
||||
use crate::config::Config;
|
||||
|
||||
#[test]
|
||||
fn factory_constructs_s5max_without_touching_hardware() {
|
||||
let config: Config = basic_toml::from_str(
|
||||
r#"
|
||||
[device]
|
||||
type = "roborock_s5max"
|
||||
name = "S5 Max test"
|
||||
|
||||
[device.roborock_s5max]
|
||||
|
||||
[network]
|
||||
bind_address = "127.0.0.1:5555"
|
||||
"#,
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
let mut driver = create_device(&config).unwrap();
|
||||
let error = match driver.initialize() {
|
||||
Ok(_) => panic!("driver unexpectedly initialized without a sequence handoff"),
|
||||
Err(error) => error,
|
||||
};
|
||||
assert!(error.to_string().contains("last_stock_sequence"));
|
||||
}
|
||||
}
|
||||
|
||||
@@ -0,0 +1,352 @@
|
||||
//! Pure AP-to-MCU command encoders for the Roborock S5 Max.
|
||||
//!
|
||||
//! These functions only build wire bytes. They never open a device or transmit
|
||||
//! a command, which keeps active hardware behavior behind an explicit owner.
|
||||
|
||||
use super::frame::encode_binary_frame;
|
||||
|
||||
pub const QUERY_MCU_VERSION: u8 = 0x80;
|
||||
pub const SYNC_SENSOR_STATUS: u8 = 0x82;
|
||||
pub const QUERY_DEVICE_INFO: u8 = 0x88;
|
||||
pub const QUERY_CURRENT_ERRORS: u8 = 0x8c;
|
||||
pub const QUERY_BATTERY_CAPACITY: u8 = 0x8f;
|
||||
pub const SYSTEM_MODE_COMMAND: u8 = 0xb0;
|
||||
pub const SUBSYSTEM_STATE_COMMAND: u8 = 0xb1;
|
||||
pub const CHARGER_COMMAND: u8 = 0xb7;
|
||||
pub const WATER_PUMP_COMMAND: u8 = 0xbf;
|
||||
pub const WHEEL_COMMAND: u8 = 0xc0;
|
||||
pub const FAN_COMMAND: u8 = 0xc1;
|
||||
pub const MAIN_BRUSH_COMMAND: u8 = 0xc3;
|
||||
pub const SIDE_BRUSH_COMMAND: u8 = 0xc4;
|
||||
pub const ACK_REPORT: u8 = 0xd1;
|
||||
pub const HEARTBEAT: u8 = 0xd3;
|
||||
|
||||
pub const MAIN_BRUSH_SUBSYSTEM: u32 = 0x00080;
|
||||
pub const SIDE_BRUSH_SUBSYSTEM: u32 = 0x00100;
|
||||
pub const FAN_SUBSYSTEM: u32 = 0x00200;
|
||||
pub const WHEEL_ODOMETRY_SUBSYSTEM: u32 = 0x00400;
|
||||
pub const OPERATIONAL_SUBSYSTEMS: &[u32] = &[
|
||||
0x00002, // bumper
|
||||
0x00004, // compatibility hook used by the stock operational transition
|
||||
0x00008, // cliff sensors
|
||||
0x00020, // drop/lift sensors
|
||||
0x00040, // dustbin sensor
|
||||
MAIN_BRUSH_SUBSYSTEM,
|
||||
SIDE_BRUSH_SUBSYSTEM,
|
||||
FAN_SUBSYSTEM,
|
||||
WHEEL_ODOMETRY_SUBSYSTEM,
|
||||
0x00800, // gyro
|
||||
0x02000, // dock IR receivers
|
||||
0x04000, // water-box sensor
|
||||
0x08000, // water pump
|
||||
0x20000, // light-touch/wall sensor
|
||||
];
|
||||
|
||||
const AP_ENVELOPE_MESSAGE: u8 = 0xd0;
|
||||
const AP_ENVELOPE_COMMAND: u8 = 0x02;
|
||||
|
||||
/// Increment the AP transmit counter while preserving zero as a reserved value.
|
||||
///
|
||||
/// A takeover must continue the stock counter. Replaying a stale sequence is
|
||||
/// silently ignored by the MCU.
|
||||
pub const fn next_tx_sequence(previous: u8) -> u8 {
|
||||
if previous == u8::MAX { 1 } else { previous + 1 }
|
||||
}
|
||||
|
||||
/// Encode a generic AP command envelope.
|
||||
pub fn command_frame(
|
||||
sequence: u8,
|
||||
command: u8,
|
||||
request_ack: bool,
|
||||
data: &[u8],
|
||||
) -> Result<Vec<u8>, String> {
|
||||
if sequence == 0 {
|
||||
return Err("MCU envelope sequence zero is reserved".to_string());
|
||||
}
|
||||
let data_length = u8::try_from(data.len()).map_err(|_| {
|
||||
format!(
|
||||
"command 0x{command:02x} data is too long: {} bytes",
|
||||
data.len()
|
||||
)
|
||||
})?;
|
||||
let mut payload = Vec::with_capacity(6 + data.len());
|
||||
payload.extend_from_slice(&[
|
||||
AP_ENVELOPE_MESSAGE,
|
||||
AP_ENVELOPE_COMMAND,
|
||||
u8::from(request_ack),
|
||||
sequence,
|
||||
command,
|
||||
data_length,
|
||||
]);
|
||||
payload.extend_from_slice(data);
|
||||
encode_binary_frame(&payload)
|
||||
}
|
||||
|
||||
/// Encode a command with no data body.
|
||||
pub fn query_frame(sequence: u8, command: u8, request_ack: bool) -> Result<Vec<u8>, String> {
|
||||
command_frame(sequence, command, request_ack, &[])
|
||||
}
|
||||
|
||||
pub fn mcu_version_query_frame(sequence: u8) -> Result<Vec<u8>, String> {
|
||||
query_frame(sequence, QUERY_MCU_VERSION, false)
|
||||
}
|
||||
|
||||
pub fn device_info_query_frame(sequence: u8) -> Result<Vec<u8>, String> {
|
||||
query_frame(sequence, QUERY_DEVICE_INFO, false)
|
||||
}
|
||||
|
||||
pub fn battery_capacity_query_frame(sequence: u8) -> Result<Vec<u8>, String> {
|
||||
query_frame(sequence, QUERY_BATTERY_CAPACITY, false)
|
||||
}
|
||||
|
||||
pub fn current_errors_query_frame(sequence: u8) -> Result<Vec<u8>, String> {
|
||||
query_frame(sequence, QUERY_CURRENT_ERRORS, true)
|
||||
}
|
||||
|
||||
/// Encode the periodically refreshed water-pump interval command.
|
||||
pub fn water_pump_command_frame(
|
||||
sequence: u8,
|
||||
on_interval: u8,
|
||||
off_interval: u8,
|
||||
) -> Result<Vec<u8>, String> {
|
||||
command_frame(
|
||||
sequence,
|
||||
WATER_PUMP_COMMAND,
|
||||
false,
|
||||
&[on_interval, off_interval],
|
||||
)
|
||||
}
|
||||
|
||||
pub fn sensor_sync_query_frame(sequence: u8) -> Result<Vec<u8>, String> {
|
||||
query_frame(sequence, SYNC_SENSOR_STATUS, true)
|
||||
}
|
||||
|
||||
/// Request an MCU system/power mode transition.
|
||||
///
|
||||
/// Mode zero is the stock normal/awake state. The MCU confirms this command
|
||||
/// with a `0x02` system-mode report, which the session acknowledges through
|
||||
/// the normal incoming-report path.
|
||||
pub fn system_mode_command_frame(sequence: u8, mode: u8) -> Result<Vec<u8>, String> {
|
||||
let mut data = *b"sys_md\0\0";
|
||||
data[7] = mode;
|
||||
command_frame(sequence, SYSTEM_MODE_COMMAND, false, &data)
|
||||
}
|
||||
|
||||
/// Enable or disable one MCU subsystem using the stock `b1/len4` bit selector.
|
||||
pub fn subsystem_state_frame(
|
||||
sequence: u8,
|
||||
selector: u32,
|
||||
enabled: bool,
|
||||
) -> Result<Vec<u8>, String> {
|
||||
if selector == 0 || !selector.is_power_of_two() || selector & 1 != 0 {
|
||||
return Err(format!("invalid MCU subsystem selector 0x{selector:08x}"));
|
||||
}
|
||||
let state = selector | u32::from(enabled);
|
||||
command_frame(
|
||||
sequence,
|
||||
SUBSYSTEM_STATE_COMMAND,
|
||||
true,
|
||||
&state.to_le_bytes(),
|
||||
)
|
||||
}
|
||||
|
||||
/// Enable or disable the dock charging path.
|
||||
pub fn charger_command_frame(sequence: u8, enabled: bool) -> Result<Vec<u8>, String> {
|
||||
command_frame(sequence, CHARGER_COMMAND, true, &[u8::from(enabled)])
|
||||
}
|
||||
|
||||
/// Encode the stock `c0/len12` differential-drive target.
|
||||
pub fn wheel_command_frame(
|
||||
sequence: u8,
|
||||
linear_target: f32,
|
||||
angular_target: f32,
|
||||
forced: bool,
|
||||
) -> Result<Vec<u8>, String> {
|
||||
if !linear_target.is_finite() || !angular_target.is_finite() {
|
||||
return Err("wheel targets must be finite".to_string());
|
||||
}
|
||||
let mut data = [0_u8; 12];
|
||||
data[0..4].copy_from_slice(&linear_target.to_le_bytes());
|
||||
data[4..8].copy_from_slice(&angular_target.to_le_bytes());
|
||||
data[8..12].copy_from_slice(&(if forced { 1_u32 } else { 0_u32 }).to_le_bytes());
|
||||
command_frame(sequence, WHEEL_COMMAND, false, &data)
|
||||
}
|
||||
|
||||
/// Encode the stock `c1/len2` fan target.
|
||||
pub fn fan_command_frame(sequence: u8, target: u8) -> Result<Vec<u8>, String> {
|
||||
command_frame(sequence, FAN_COMMAND, true, &[target, 0])
|
||||
}
|
||||
|
||||
/// Encode the stock `c3/len2` main-brush target.
|
||||
pub fn main_brush_command_frame(
|
||||
sequence: u8,
|
||||
target: u8,
|
||||
force_start: bool,
|
||||
) -> Result<Vec<u8>, String> {
|
||||
command_frame(
|
||||
sequence,
|
||||
MAIN_BRUSH_COMMAND,
|
||||
true,
|
||||
&[target, u8::from(force_start)],
|
||||
)
|
||||
}
|
||||
|
||||
/// Encode the stock `c4/len2` side-brush target.
|
||||
///
|
||||
/// This MCU ignores the second byte, but stock sometimes sets it while
|
||||
/// stopping, so it remains explicit for wire compatibility.
|
||||
pub fn side_brush_command_frame(
|
||||
sequence: u8,
|
||||
target: u8,
|
||||
compatibility_flag: bool,
|
||||
) -> Result<Vec<u8>, String> {
|
||||
command_frame(
|
||||
sequence,
|
||||
SIDE_BRUSH_COMMAND,
|
||||
true,
|
||||
&[target, u8::from(compatibility_flag)],
|
||||
)
|
||||
}
|
||||
|
||||
/// Acknowledge an MCU frame that requested an AP response.
|
||||
pub fn acknowledgement_frame(sequence: u8, received_sequence: u8) -> Result<Vec<u8>, String> {
|
||||
command_frame(sequence, ACK_REPORT, false, &[received_sequence])
|
||||
}
|
||||
|
||||
/// Encode the stock zero-data heartbeat body.
|
||||
pub fn heartbeat_frame(sequence: u8) -> Result<Vec<u8>, String> {
|
||||
command_frame(sequence, HEARTBEAT, false, &[0, 0, 0, 0])
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn sequence_skips_reserved_zero() {
|
||||
assert_eq!(next_tx_sequence(0x7e), 0x7f);
|
||||
assert_eq!(next_tx_sequence(0xff), 0x01);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_captured_queries() {
|
||||
assert_eq!(
|
||||
mcu_version_query_frame(0x7f).unwrap(),
|
||||
[0xaa, 0x06, 0x01, 0xd0, 0x02, 0x00, 0x7f, 0x80, 0x00, 0xd8]
|
||||
);
|
||||
assert_eq!(
|
||||
sensor_sync_query_frame(0x13).unwrap(),
|
||||
[0xaa, 0x06, 0x01, 0xd0, 0x02, 0x01, 0x13, 0x82, 0x00, 0xd8]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_captured_ack_and_heartbeat() {
|
||||
assert_eq!(
|
||||
acknowledgement_frame(0x15, 0xd1).unwrap(),
|
||||
[
|
||||
0xaa, 0x07, 0x01, 0xd0, 0x02, 0x00, 0x15, 0xd1, 0x01, 0xd1, 0x03
|
||||
]
|
||||
);
|
||||
assert_eq!(
|
||||
heartbeat_frame(0x6a).unwrap(),
|
||||
[
|
||||
0xaa, 0x0a, 0x01, 0xd0, 0x02, 0x00, 0x6a, 0xd3, 0x04, 0x00, 0x00, 0x00, 0x00, 0x47,
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_captured_actuator_frames() {
|
||||
assert_eq!(
|
||||
wheel_command_frame(0xe5, 0.0, 0.0, true).unwrap(),
|
||||
[
|
||||
0xaa, 0x12, 0x01, 0xd0, 0x02, 0x00, 0xe5, 0xc0, 0x0c, 0x00, 0x00, 0x00, 0x00, 0x00,
|
||||
0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x0c,
|
||||
]
|
||||
);
|
||||
assert_eq!(
|
||||
fan_command_frame(0xe6, 0).unwrap(),
|
||||
[
|
||||
0xaa, 0x08, 0x01, 0xd0, 0x02, 0x01, 0xe6, 0xc1, 0x02, 0x00, 0x00, 0x7d
|
||||
]
|
||||
);
|
||||
assert_eq!(
|
||||
main_brush_command_frame(0x39, 0, true).unwrap(),
|
||||
[
|
||||
0xaa, 0x08, 0x01, 0xd0, 0x02, 0x01, 0x39, 0xc3, 0x02, 0x00, 0x01, 0x15
|
||||
]
|
||||
);
|
||||
assert_eq!(
|
||||
side_brush_command_frame(0x3b, 0, true).unwrap(),
|
||||
[
|
||||
0xaa, 0x08, 0x01, 0xd0, 0x02, 0x01, 0x3b, 0xc4, 0x02, 0x00, 0x01, 0x10
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_invalid_envelopes_and_wheel_values() {
|
||||
assert!(fan_command_frame(0, 0).is_err());
|
||||
assert!(wheel_command_frame(1, f32::NAN, 0.0, true).is_err());
|
||||
assert!(wheel_command_frame(1, 0.0, f32::INFINITY, true).is_err());
|
||||
assert!(command_frame(1, 0x80, false, &[0; 256]).is_err());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_zero_water_pump_stop() {
|
||||
let frame = water_pump_command_frame(0x24, 0, 0).unwrap();
|
||||
assert_eq!(
|
||||
&frame[3..11],
|
||||
&[0xd0, 0x02, 0x00, 0x24, 0xbf, 0x02, 0x00, 0x00]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_captured_charger_transitions() {
|
||||
let disabled = charger_command_frame(0x20, false).unwrap();
|
||||
let enabled = charger_command_frame(0x21, true).unwrap();
|
||||
assert_eq!(
|
||||
&disabled[3..10],
|
||||
&[0xd0, 0x02, 0x01, 0x20, 0xb7, 0x01, 0x00]
|
||||
);
|
||||
assert_eq!(&enabled[3..10], &[0xd0, 0x02, 0x01, 0x21, 0xb7, 0x01, 0x01]);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_captured_normal_system_mode_transition() {
|
||||
let frame = system_mode_command_frame(0x20, 0).unwrap();
|
||||
assert_eq!(
|
||||
&frame[3..17],
|
||||
&[
|
||||
0xd0, 0x02, 0x00, 0x20, 0xb0, 0x08, b's', b'y', b's', b'_', b'm', b'd', 0, 0,
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn encodes_captured_motor_subsystem_enables() {
|
||||
assert_eq!(
|
||||
subsystem_state_frame(0x98, FAN_SUBSYSTEM, true).unwrap(),
|
||||
[
|
||||
0xaa, 0x0a, 0x01, 0xd0, 0x02, 0x01, 0x98, 0xb1, 0x04, 0x01, 0x02, 0x00, 0x00, 0x57,
|
||||
]
|
||||
);
|
||||
assert_eq!(
|
||||
&subsystem_state_frame(0x96, MAIN_BRUSH_SUBSYSTEM, true).unwrap()[9..13],
|
||||
&[0x81, 0x00, 0x00, 0x00]
|
||||
);
|
||||
assert_eq!(
|
||||
&subsystem_state_frame(0x97, SIDE_BRUSH_SUBSYSTEM, true).unwrap()[9..13],
|
||||
&[0x01, 0x01, 0x00, 0x00]
|
||||
);
|
||||
assert_eq!(
|
||||
subsystem_state_frame(0x9a, WHEEL_ODOMETRY_SUBSYSTEM, true).unwrap(),
|
||||
[
|
||||
0xaa, 0x0a, 0x01, 0xd0, 0x02, 0x01, 0x9a, 0xb1, 0x04, 0x01, 0x04, 0x00, 0x00, 0xfc,
|
||||
]
|
||||
);
|
||||
assert!(subsystem_state_frame(1, 0, true).is_err());
|
||||
assert!(subsystem_state_frame(1, 0x300, true).is_err());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
//! Side-effect-free MCU binary frame encoding.
|
||||
//!
|
||||
//! The decoder remains in [`super::packet`]. Keeping the encoder independent
|
||||
//! from UART ownership lets tests and the production worker share exactly the
|
||||
//! same wire representation.
|
||||
|
||||
use super::packet::{SYNC, crc8};
|
||||
|
||||
/// Encode one MCU payload, append its CRC, and escape reserved bytes.
|
||||
pub fn encode_binary_frame(payload: &[u8]) -> Result<Vec<u8>, String> {
|
||||
let payload_length = u8::try_from(payload.len())
|
||||
.map_err(|_| format!("payload too large: {} bytes", payload.len()))?;
|
||||
let crc = crc8(payload, 0);
|
||||
let mut escaped = Vec::with_capacity(payload.len() + 1);
|
||||
let mut expansions = 0u8;
|
||||
|
||||
for byte in payload.iter().copied().chain(std::iter::once(crc)) {
|
||||
match byte {
|
||||
0xa9 => {
|
||||
escaped.extend_from_slice(&[0xa9, 0x00]);
|
||||
expansions = expansions
|
||||
.checked_add(1)
|
||||
.ok_or_else(|| "escape overhead overflow".to_string())?;
|
||||
}
|
||||
0xaa => {
|
||||
escaped.extend_from_slice(&[0xa9, 0x01]);
|
||||
expansions = expansions
|
||||
.checked_add(1)
|
||||
.ok_or_else(|| "escape overhead overflow".to_string())?;
|
||||
}
|
||||
byte => escaped.push(byte),
|
||||
}
|
||||
}
|
||||
|
||||
let escape_overhead = expansions
|
||||
.checked_add(1)
|
||||
.ok_or_else(|| "escape overhead overflow".to_string())?;
|
||||
let mut raw = Vec::with_capacity(3 + escaped.len());
|
||||
raw.extend_from_slice(&[SYNC, payload_length, escape_overhead]);
|
||||
raw.extend_from_slice(&escaped);
|
||||
Ok(raw)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::encode_binary_frame;
|
||||
|
||||
#[test]
|
||||
fn escapes_reserved_payload_bytes() {
|
||||
assert_eq!(
|
||||
encode_binary_frame(&[0xa9, 0xaa]).unwrap(),
|
||||
[0xaa, 0x02, 0x03, 0xa9, 0x00, 0xa9, 0x01, 0x8d]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_payloads_larger_than_the_wire_length_field() {
|
||||
assert!(encode_binary_frame(&[0; 256]).is_err());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,418 @@
|
||||
//! Passive Roborock LDS packet framing.
|
||||
//!
|
||||
//! Captured S5 Max traffic uses the 22-byte XV11 packet layout:
|
||||
//! `fa index speed_le sample[4] checksum_le`.
|
||||
//! Each packet contains four consecutive one-degree samples. Index `0xa0`
|
||||
//! starts at 0 degrees and index `0xf9` ends at 359 degrees.
|
||||
//!
|
||||
//! Synchronized scan/IMU registration shows that raw angles increase clockwise
|
||||
//! when viewed from above. Fixed-wall synchronization measured robot-forward
|
||||
//! at raw 261.2 degrees. The reader keeps this mounting angle configurable and
|
||||
//! transforms samples with `body = wrap(forward_raw - raw_angle)`.
|
||||
|
||||
pub const SYNC: u8 = 0xfa;
|
||||
pub const PACKET_LENGTH: usize = 22;
|
||||
pub const INDEX_MIN: u8 = 0xa0;
|
||||
pub const INDEX_MAX: u8 = 0xf9;
|
||||
pub const PACKETS_PER_REVOLUTION: usize = 90;
|
||||
pub const SAMPLES_PER_REVOLUTION: usize = 360;
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct LdsSample {
|
||||
pub distance_raw: u16,
|
||||
pub signal_strength: u16,
|
||||
}
|
||||
|
||||
impl LdsSample {
|
||||
/// Bit 15 marks a sample for which no valid distance was returned.
|
||||
pub const fn invalid(self) -> bool {
|
||||
self.distance_raw & 0x8000 != 0
|
||||
}
|
||||
|
||||
/// Bit 14 marks a sample whose signal strength is below the sensor limit.
|
||||
pub const fn strength_warning(self) -> bool {
|
||||
self.distance_raw & 0x4000 != 0
|
||||
}
|
||||
|
||||
/// Valid distance bits, in millimetres.
|
||||
pub const fn distance_mm(self) -> u16 {
|
||||
self.distance_raw & 0x3fff
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct LdsPacket {
|
||||
pub index: u8,
|
||||
pub speed_raw: u16,
|
||||
pub samples: [LdsSample; 4],
|
||||
pub checksum: u16,
|
||||
}
|
||||
|
||||
impl LdsPacket {
|
||||
pub fn parse(bytes: &[u8; PACKET_LENGTH]) -> Option<Self> {
|
||||
if bytes[0] != SYNC || !(INDEX_MIN..=INDEX_MAX).contains(&bytes[1]) {
|
||||
return None;
|
||||
}
|
||||
let sample = |offset| LdsSample {
|
||||
distance_raw: u16::from_le_bytes([bytes[offset], bytes[offset + 1]]),
|
||||
signal_strength: u16::from_le_bytes([bytes[offset + 2], bytes[offset + 3]]),
|
||||
};
|
||||
Some(Self {
|
||||
index: bytes[1],
|
||||
speed_raw: u16::from_le_bytes([bytes[2], bytes[3]]),
|
||||
samples: [sample(4), sample(8), sample(12), sample(16)],
|
||||
checksum: u16::from_le_bytes([bytes[20], bytes[21]]),
|
||||
})
|
||||
}
|
||||
|
||||
pub const fn start_angle_degrees(self) -> u16 {
|
||||
(self.index - INDEX_MIN) as u16 * 4
|
||||
}
|
||||
|
||||
pub fn speed_rpm(self) -> f32 {
|
||||
self.speed_raw as f32 / 64.0
|
||||
}
|
||||
|
||||
pub fn checksum_valid(self, bytes: &[u8; PACKET_LENGTH]) -> bool {
|
||||
self.checksum == packet_checksum(bytes)
|
||||
}
|
||||
}
|
||||
|
||||
/// XV11's 15-bit rolling checksum over the first ten little-endian words.
|
||||
pub fn packet_checksum(bytes: &[u8; PACKET_LENGTH]) -> u16 {
|
||||
let mut accumulator = 0_u32;
|
||||
for word in bytes[..20].chunks(2) {
|
||||
accumulator = (accumulator << 1) + u16::from_le_bytes([word[0], word[1]]) as u32;
|
||||
}
|
||||
(((accumulator & 0x7fff) + (accumulator >> 15)) & 0x7fff) as u16
|
||||
}
|
||||
|
||||
#[derive(Debug, Default)]
|
||||
pub struct LdsDecoder {
|
||||
buffer: Vec<u8>,
|
||||
discarded: u64,
|
||||
}
|
||||
|
||||
impl LdsDecoder {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
buffer: Vec::with_capacity(PACKET_LENGTH * 4),
|
||||
discarded: 0,
|
||||
}
|
||||
}
|
||||
|
||||
pub const fn discarded(&self) -> u64 {
|
||||
self.discarded
|
||||
}
|
||||
|
||||
pub fn push(&mut self, bytes: &[u8]) -> Vec<LdsPacket> {
|
||||
self.buffer.extend_from_slice(bytes);
|
||||
let mut packets = Vec::new();
|
||||
loop {
|
||||
let Some(start) = self.buffer.iter().position(|byte| *byte == SYNC) else {
|
||||
self.discarded += self.buffer.len() as u64;
|
||||
self.buffer.clear();
|
||||
break;
|
||||
};
|
||||
if start != 0 {
|
||||
self.discarded += start as u64;
|
||||
self.buffer.drain(..start);
|
||||
}
|
||||
if self.buffer.len() < PACKET_LENGTH {
|
||||
break;
|
||||
}
|
||||
let raw: [u8; PACKET_LENGTH] = self.buffer[..PACKET_LENGTH].try_into().unwrap();
|
||||
if let Some(packet) =
|
||||
LdsPacket::parse(&raw).filter(|packet| packet.checksum_valid(&raw))
|
||||
{
|
||||
packets.push(packet);
|
||||
self.buffer.drain(..PACKET_LENGTH);
|
||||
} else {
|
||||
self.discarded += 1;
|
||||
self.buffer.drain(..1);
|
||||
}
|
||||
}
|
||||
packets
|
||||
}
|
||||
}
|
||||
|
||||
/// Passive reconstruction of one LDS revolution for capture diagnostics.
|
||||
///
|
||||
/// This intentionally has no actuator or UART-write functionality. A scan can
|
||||
/// be partial when observation starts in the middle of a revolution or bytes
|
||||
/// were lost before reaching this decoder.
|
||||
#[derive(Debug, Clone)]
|
||||
pub struct LdsRevolution {
|
||||
pub samples: [Option<LdsSample>; SAMPLES_PER_REVOLUTION],
|
||||
pub packet_count: usize,
|
||||
pub duplicate_packets: usize,
|
||||
pub min_speed_raw: u16,
|
||||
pub max_speed_raw: u16,
|
||||
speed_raw_sum: u64,
|
||||
}
|
||||
|
||||
impl LdsRevolution {
|
||||
pub const fn missing_packets(&self) -> usize {
|
||||
PACKETS_PER_REVOLUTION - self.packet_count
|
||||
}
|
||||
|
||||
pub const fn complete(&self) -> bool {
|
||||
self.packet_count == PACKETS_PER_REVOLUTION
|
||||
}
|
||||
|
||||
pub fn mean_speed_rpm(&self) -> f32 {
|
||||
if self.packet_count == 0 {
|
||||
return 0.0;
|
||||
}
|
||||
self.speed_raw_sum as f32 / self.packet_count as f32 / 64.0
|
||||
}
|
||||
|
||||
pub fn min_speed_rpm(&self) -> f32 {
|
||||
self.min_speed_raw as f32 / 64.0
|
||||
}
|
||||
|
||||
pub fn max_speed_rpm(&self) -> f32 {
|
||||
self.max_speed_raw as f32 / 64.0
|
||||
}
|
||||
|
||||
pub fn valid_sample_count(&self) -> usize {
|
||||
self.samples
|
||||
.iter()
|
||||
.flatten()
|
||||
.filter(|sample| !sample.invalid())
|
||||
.count()
|
||||
}
|
||||
|
||||
pub fn invalid_sample_count(&self) -> usize {
|
||||
self.samples
|
||||
.iter()
|
||||
.flatten()
|
||||
.filter(|sample| sample.invalid())
|
||||
.count()
|
||||
}
|
||||
|
||||
pub fn strength_warning_count(&self) -> usize {
|
||||
self.samples
|
||||
.iter()
|
||||
.flatten()
|
||||
.filter(|sample| sample.strength_warning())
|
||||
.count()
|
||||
}
|
||||
|
||||
pub fn observed_sample_count(&self) -> usize {
|
||||
self.samples.iter().flatten().count()
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
struct RevolutionBuilder {
|
||||
revolution: LdsRevolution,
|
||||
seen_packets: [bool; PACKETS_PER_REVOLUTION],
|
||||
last_index: Option<u8>,
|
||||
}
|
||||
|
||||
impl RevolutionBuilder {
|
||||
fn new() -> Self {
|
||||
Self {
|
||||
revolution: LdsRevolution {
|
||||
samples: [None; SAMPLES_PER_REVOLUTION],
|
||||
packet_count: 0,
|
||||
duplicate_packets: 0,
|
||||
min_speed_raw: u16::MAX,
|
||||
max_speed_raw: 0,
|
||||
speed_raw_sum: 0,
|
||||
},
|
||||
seen_packets: [false; PACKETS_PER_REVOLUTION],
|
||||
last_index: None,
|
||||
}
|
||||
}
|
||||
|
||||
fn insert(&mut self, packet: LdsPacket) {
|
||||
let packet_index = (packet.index - INDEX_MIN) as usize;
|
||||
if self.seen_packets[packet_index] {
|
||||
self.revolution.duplicate_packets += 1;
|
||||
self.last_index = Some(packet.index);
|
||||
return;
|
||||
}
|
||||
self.seen_packets[packet_index] = true;
|
||||
self.revolution.packet_count += 1;
|
||||
self.revolution.min_speed_raw = self.revolution.min_speed_raw.min(packet.speed_raw);
|
||||
self.revolution.max_speed_raw = self.revolution.max_speed_raw.max(packet.speed_raw);
|
||||
self.revolution.speed_raw_sum += u64::from(packet.speed_raw);
|
||||
let first_angle = packet.start_angle_degrees() as usize;
|
||||
for (offset, sample) in packet.samples.into_iter().enumerate() {
|
||||
self.revolution.samples[first_angle + offset] = Some(sample);
|
||||
}
|
||||
self.last_index = Some(packet.index);
|
||||
}
|
||||
|
||||
fn finish(mut self) -> LdsRevolution {
|
||||
if self.revolution.packet_count == 0 {
|
||||
self.revolution.min_speed_raw = 0;
|
||||
}
|
||||
self.revolution
|
||||
}
|
||||
}
|
||||
|
||||
/// Groups ordered, checksum-valid LDS packets at each index wrap.
|
||||
#[derive(Debug, Default)]
|
||||
pub struct LdsRevolutionAssembler {
|
||||
current: Option<RevolutionBuilder>,
|
||||
}
|
||||
|
||||
impl LdsRevolutionAssembler {
|
||||
pub fn new() -> Self {
|
||||
Self::default()
|
||||
}
|
||||
|
||||
pub fn push(&mut self, packet: LdsPacket) -> Option<LdsRevolution> {
|
||||
let wrapped = self
|
||||
.current
|
||||
.as_ref()
|
||||
.and_then(|current| current.last_index)
|
||||
.is_some_and(|last_index| packet.index < last_index);
|
||||
let completed = wrapped.then(|| {
|
||||
self.current
|
||||
.take()
|
||||
.expect("wrap requires a current scan")
|
||||
.finish()
|
||||
});
|
||||
self.current
|
||||
.get_or_insert_with(RevolutionBuilder::new)
|
||||
.insert(packet);
|
||||
completed
|
||||
}
|
||||
|
||||
pub fn finish(mut self) -> Option<LdsRevolution> {
|
||||
self.current.take().map(RevolutionBuilder::finish)
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn parses_captured_packet_shape() {
|
||||
let raw = [
|
||||
0xfa, 0xbb, 0xc2, 0x4a, 0x7f, 0x0f, 0x78, 0x00, 0x64, 0x0f, 0x8e, 0x01, 0x27, 0x0f,
|
||||
0x6d, 0x02, 0xf4, 0x0e, 0xf7, 0x02, 0x45, 0x40,
|
||||
];
|
||||
let packet = LdsPacket::parse(&raw).unwrap();
|
||||
assert_eq!(packet.index, 0xbb);
|
||||
assert_eq!(packet.start_angle_degrees(), 108);
|
||||
assert_eq!(packet.speed_raw, 0x4ac2);
|
||||
assert_eq!(packet.speed_rpm(), 299.03125);
|
||||
assert_eq!(packet.samples[0].distance_raw, 0x0f7f);
|
||||
assert_eq!(packet.samples[0].distance_mm(), 0x0f7f);
|
||||
assert_eq!(packet.samples[0].signal_strength, 0x0078);
|
||||
assert_eq!(packet.checksum, 0x4045);
|
||||
assert!(packet.checksum_valid(&raw));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn identifies_no_return_sample() {
|
||||
let raw = [
|
||||
0xfa, 0xb9, 0xc2, 0x4a, 0x20, 0x80, 0, 0, 0x20, 0x80, 0, 0, 0x20, 0x80, 0, 0, 0x20,
|
||||
0x80, 0, 0, 0x67, 0x4f,
|
||||
];
|
||||
let packet = LdsPacket::parse(&raw).unwrap();
|
||||
assert!(packet.samples.iter().all(|sample| sample.invalid()));
|
||||
assert_eq!(packet.samples[0].distance_mm(), 0x20);
|
||||
assert!(packet.checksum_valid(&raw));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn resynchronizes_across_chunks() {
|
||||
let raw = [
|
||||
0xfa, 0xbb, 0xc2, 0x4a, 0x7f, 0x0f, 0x78, 0x00, 0x64, 0x0f, 0x8e, 0x01, 0x27, 0x0f,
|
||||
0x6d, 0x02, 0xf4, 0x0e, 0xf7, 0x02, 0x45, 0x40,
|
||||
];
|
||||
let mut decoder = LdsDecoder::new();
|
||||
assert!(decoder.push(&raw[..7]).is_empty());
|
||||
let packets = decoder.push(&raw[7..]);
|
||||
assert_eq!(packets.len(), 1);
|
||||
assert_eq!(packets[0].index, 0xbb);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_a_corrupt_packet_while_resynchronizing() {
|
||||
let mut raw = [
|
||||
0xfa, 0xbb, 0xc2, 0x4a, 0x7f, 0x0f, 0x78, 0x00, 0x64, 0x0f, 0x8e, 0x01, 0x27, 0x0f,
|
||||
0x6d, 0x02, 0xf4, 0x0e, 0xf7, 0x02, 0x45, 0x40,
|
||||
];
|
||||
raw[4] ^= 1;
|
||||
let mut decoder = LdsDecoder::new();
|
||||
assert!(decoder.push(&raw).is_empty());
|
||||
assert!(decoder.discarded() > 0);
|
||||
}
|
||||
|
||||
fn synthetic_packet(index: u8, speed_raw: u16, invalid: bool) -> LdsPacket {
|
||||
let distance_raw = if invalid { 0x8000 } else { 1_000 };
|
||||
LdsPacket {
|
||||
index,
|
||||
speed_raw,
|
||||
samples: [LdsSample {
|
||||
distance_raw,
|
||||
signal_strength: 10,
|
||||
}; 4],
|
||||
checksum: 0,
|
||||
}
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reconstructs_a_complete_revolution() {
|
||||
let mut assembler = LdsRevolutionAssembler::new();
|
||||
for index in INDEX_MIN..=INDEX_MAX {
|
||||
assert!(
|
||||
assembler
|
||||
.push(synthetic_packet(index, 19_200, false))
|
||||
.is_none()
|
||||
);
|
||||
}
|
||||
let scan = assembler
|
||||
.push(synthetic_packet(INDEX_MIN, 19_264, true))
|
||||
.unwrap();
|
||||
assert!(scan.complete());
|
||||
assert_eq!(scan.packet_count, 90);
|
||||
assert_eq!(scan.missing_packets(), 0);
|
||||
assert_eq!(scan.observed_sample_count(), 360);
|
||||
assert_eq!(scan.valid_sample_count(), 360);
|
||||
assert_eq!(scan.invalid_sample_count(), 0);
|
||||
assert_eq!(scan.mean_speed_rpm(), 300.0);
|
||||
|
||||
let partial = assembler.finish().unwrap();
|
||||
assert_eq!(partial.packet_count, 1);
|
||||
assert_eq!(partial.invalid_sample_count(), 4);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn reports_partial_revolutions_and_duplicates() {
|
||||
let mut assembler = LdsRevolutionAssembler::new();
|
||||
assert!(
|
||||
assembler
|
||||
.push(synthetic_packet(0xb0, 19_000, false))
|
||||
.is_none()
|
||||
);
|
||||
assert!(
|
||||
assembler
|
||||
.push(synthetic_packet(0xb0, 19_000, false))
|
||||
.is_none()
|
||||
);
|
||||
assert!(
|
||||
assembler
|
||||
.push(synthetic_packet(0xb2, 19_100, true))
|
||||
.is_none()
|
||||
);
|
||||
let scan = assembler
|
||||
.push(synthetic_packet(INDEX_MIN, 19_200, false))
|
||||
.unwrap();
|
||||
assert!(!scan.complete());
|
||||
assert_eq!(scan.packet_count, 2);
|
||||
assert_eq!(scan.duplicate_packets, 1);
|
||||
assert_eq!(scan.missing_packets(), 88);
|
||||
assert_eq!(scan.observed_sample_count(), 8);
|
||||
assert_eq!(scan.valid_sample_count(), 4);
|
||||
assert_eq!(scan.invalid_sample_count(), 4);
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,107 @@
|
||||
//! Stop-only LDS motor ownership used by the safety lifecycle.
|
||||
|
||||
use super::sys::IoctlRequest;
|
||||
use crate::error::{Error, Result};
|
||||
use std::fs::{File, OpenOptions};
|
||||
use std::io;
|
||||
use std::os::fd::AsRawFd;
|
||||
use std::os::unix::fs::OpenOptionsExt;
|
||||
use std::sync::Arc;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::thread;
|
||||
use std::time::Duration;
|
||||
|
||||
const LDS_MOTOR_SET_SPEED: IoctlRequest = 0x4004_f810u32 as IoctlRequest;
|
||||
pub(crate) const LDS_MOTOR_SET_CURRENT_SPEED: IoctlRequest = 0x4004_f812u32 as IoctlRequest;
|
||||
const LDS_MOTOR_START: IoctlRequest = 0x4004_f813u32 as IoctlRequest;
|
||||
const LDS_MOTOR_STOP: IoctlRequest = 0x4004_f814u32 as IoctlRequest;
|
||||
const LDS_MOTOR_SET_PRODUCT_ID: IoctlRequest = 0x4004_f826u32 as IoctlRequest;
|
||||
const TARGET_RPM_X100: libc::c_int = 30_000;
|
||||
const PRODUCT_ID: libc::c_int = 1;
|
||||
const START_VALUE: libc::c_int = 20_000;
|
||||
const STOP_SETTLE: Duration = Duration::from_millis(50);
|
||||
|
||||
pub(crate) struct LdsMotorGuard {
|
||||
file: File,
|
||||
path: String,
|
||||
armed: bool,
|
||||
running: Arc<AtomicBool>,
|
||||
}
|
||||
|
||||
impl LdsMotorGuard {
|
||||
pub(crate) fn open(path: &str) -> Result<Self> {
|
||||
let file = OpenOptions::new()
|
||||
.read(true)
|
||||
.write(true)
|
||||
.custom_flags(libc::O_CLOEXEC)
|
||||
.open(path)
|
||||
.map_err(|error| Error::Other(format!("open S5 Max LDS motor {path}: {error}")))?;
|
||||
Ok(Self {
|
||||
file,
|
||||
path: path.to_string(),
|
||||
armed: true,
|
||||
running: Arc::new(AtomicBool::new(false)),
|
||||
})
|
||||
}
|
||||
|
||||
fn ioctl_value(
|
||||
&self,
|
||||
request: IoctlRequest,
|
||||
value: libc::c_int,
|
||||
operation: &str,
|
||||
) -> Result<()> {
|
||||
let mut argument = value;
|
||||
if unsafe { libc::ioctl(self.file.as_raw_fd(), request, &mut argument) } < 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"{operation} S5 Max LDS motor {}: {}",
|
||||
self.path,
|
||||
io::Error::last_os_error()
|
||||
)));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn configure_and_start(&mut self) -> Result<()> {
|
||||
self.ioctl_value(LDS_MOTOR_SET_SPEED, TARGET_RPM_X100, "set target speed")?;
|
||||
self.ioctl_value(LDS_MOTOR_SET_PRODUCT_ID, PRODUCT_ID, "set product ID")?;
|
||||
self.ioctl_value(LDS_MOTOR_START, START_VALUE, "start")?;
|
||||
self.running.store(true, Ordering::Release);
|
||||
log::info!("S5 Max LDS motor started at 300 RPM");
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn feedback_file(&self) -> Result<File> {
|
||||
self.file.try_clone().map_err(Error::from)
|
||||
}
|
||||
|
||||
pub(crate) fn running_flag(&self) -> Arc<AtomicBool> {
|
||||
Arc::clone(&self.running)
|
||||
}
|
||||
|
||||
pub(crate) fn is_running(&self) -> bool {
|
||||
self.running.load(Ordering::Acquire)
|
||||
}
|
||||
|
||||
pub(crate) fn stop(&self) -> Result<()> {
|
||||
self.running.store(false, Ordering::Release);
|
||||
self.ioctl_value(LDS_MOTOR_STOP, 0, "stop")?;
|
||||
thread::sleep(STOP_SETTLE);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn stop_and_disarm(&mut self) -> Result<()> {
|
||||
self.stop()?;
|
||||
self.armed = false;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for LdsMotorGuard {
|
||||
fn drop(&mut self) {
|
||||
if self.armed
|
||||
&& let Err(error) = self.stop()
|
||||
{
|
||||
log::error!("S5 Max LDS motor drop-stop failed: {error}");
|
||||
}
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,280 @@
|
||||
//! Read-only LDS UART worker.
|
||||
|
||||
use super::lds::{LdsDecoder, LdsRevolution, LdsRevolutionAssembler};
|
||||
use super::lds_motor::LDS_MOTOR_SET_CURRENT_SPEED;
|
||||
use super::lifecycle::Lifecycle;
|
||||
use super::tty::ExclusiveTty;
|
||||
use crate::core::types::{SensorGroupData, SensorValue};
|
||||
use crate::error::{Error, Result};
|
||||
use std::f32::consts::{PI, TAU};
|
||||
use std::fs::File;
|
||||
use std::os::fd::AsRawFd;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::thread::{self, JoinHandle};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
const LDS_BAUD: libc::speed_t = libc::B115200;
|
||||
const LDS_BAUD_NUMBER: u32 = 115_200;
|
||||
const ZERO_FEEDBACK_INTERVAL: Duration = Duration::from_millis(50);
|
||||
// A cold rotor cannot report speed immediately. Preserve the stock abnormal-
|
||||
// speed window before sending zero feedback to the kernel motor controller.
|
||||
const COLD_START_FEEDBACK_GRACE: Duration = Duration::from_secs(3);
|
||||
// Publish a usable scan through occasional UART loss while keeping absent
|
||||
// samples explicit for downstream filtering.
|
||||
const MIN_PUBLISHABLE_PACKETS: usize = 75;
|
||||
|
||||
const fn revolution_publishable(packet_count: usize) -> bool {
|
||||
packet_count >= MIN_PUBLISHABLE_PACKETS
|
||||
}
|
||||
|
||||
fn zero_feedback_due(
|
||||
motor_running: bool,
|
||||
packet_seen: bool,
|
||||
motor_started: Option<Instant>,
|
||||
last_feedback: Instant,
|
||||
now: Instant,
|
||||
) -> bool {
|
||||
motor_running
|
||||
&& now.duration_since(last_feedback) >= ZERO_FEEDBACK_INTERVAL
|
||||
&& (packet_seen
|
||||
|| motor_started
|
||||
.is_some_and(|started| now.duration_since(started) >= COLD_START_FEEDBACK_GRACE))
|
||||
}
|
||||
|
||||
pub(crate) fn speed_feedback_rpm_x100(speed_raw: u16) -> libc::c_int {
|
||||
((u32::from(speed_raw) * 100 + 32) / 64) as libc::c_int
|
||||
}
|
||||
|
||||
fn feed_speed(file: &File, speed: libc::c_int) -> Result<()> {
|
||||
let mut argument = speed;
|
||||
if unsafe { libc::ioctl(file.as_raw_fd(), LDS_MOTOR_SET_CURRENT_SPEED, &mut argument) } < 0 {
|
||||
return Err(std::io::Error::last_os_error().into());
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) struct LdsReader {
|
||||
handle: Option<JoinHandle<Result<()>>>,
|
||||
}
|
||||
|
||||
impl LdsReader {
|
||||
pub(crate) fn start(
|
||||
path: &str,
|
||||
forward_angle_degrees: f32,
|
||||
sensor_data: Arc<Mutex<SensorGroupData>>,
|
||||
lifecycle: Lifecycle,
|
||||
shutdown: Arc<AtomicBool>,
|
||||
feedback_motor: File,
|
||||
motor_running: Arc<AtomicBool>,
|
||||
) -> Result<Self> {
|
||||
let tty = ExclusiveTty::open(path, LDS_BAUD, LDS_BAUD_NUMBER)?;
|
||||
let path = path.to_string();
|
||||
let thread_lifecycle = lifecycle;
|
||||
let handle = thread::Builder::new()
|
||||
.name("s5max-lds".to_string())
|
||||
.spawn(move || {
|
||||
let result = reader_loop(
|
||||
tty,
|
||||
forward_angle_degrees,
|
||||
sensor_data,
|
||||
&shutdown,
|
||||
feedback_motor,
|
||||
motor_running,
|
||||
);
|
||||
if let Err(error) = &result {
|
||||
log::error!("S5 Max LDS worker failed on {path}: {error}");
|
||||
thread_lifecycle.fault(format!("LDS worker: {error}"));
|
||||
shutdown.store(true, Ordering::Release);
|
||||
}
|
||||
result
|
||||
})
|
||||
.map_err(|error| Error::Other(format!("spawn S5 Max LDS worker: {error}")))?;
|
||||
Ok(Self {
|
||||
handle: Some(handle),
|
||||
})
|
||||
}
|
||||
|
||||
pub(crate) fn join(&mut self) -> Result<()> {
|
||||
let Some(handle) = self.handle.take() else {
|
||||
return Ok(());
|
||||
};
|
||||
handle.join().map_err(|_| Error::ThreadPanic)?
|
||||
}
|
||||
}
|
||||
|
||||
fn reader_loop(
|
||||
tty: ExclusiveTty,
|
||||
forward_angle_degrees: f32,
|
||||
sensor_data: Arc<Mutex<SensorGroupData>>,
|
||||
shutdown: &AtomicBool,
|
||||
feedback_motor: File,
|
||||
motor_running: Arc<AtomicBool>,
|
||||
) -> Result<()> {
|
||||
let mut decoder = LdsDecoder::new();
|
||||
let mut assembler = LdsRevolutionAssembler::new();
|
||||
let forward_radians = forward_angle_degrees.to_radians();
|
||||
let mut buffer = [0u8; 1024];
|
||||
let mut last_feedback = Instant::now();
|
||||
let mut motor_started = None;
|
||||
let mut packet_seen = false;
|
||||
|
||||
while !shutdown.load(Ordering::Acquire) {
|
||||
let now = Instant::now();
|
||||
let running = motor_running.load(Ordering::Acquire);
|
||||
match (running, motor_started) {
|
||||
(true, None) => motor_started = Some(now),
|
||||
(false, Some(_)) => {
|
||||
motor_started = None;
|
||||
packet_seen = false;
|
||||
}
|
||||
_ => {}
|
||||
}
|
||||
if !tty.poll_readable(Duration::from_millis(50))? {
|
||||
if zero_feedback_due(running, packet_seen, motor_started, last_feedback, now) {
|
||||
feed_speed(&feedback_motor, 0)?;
|
||||
last_feedback = Instant::now();
|
||||
}
|
||||
continue;
|
||||
}
|
||||
let count = tty.read_available(&mut buffer)?;
|
||||
for packet in decoder.push(&buffer[..count]) {
|
||||
packet_seen = true;
|
||||
if running {
|
||||
feed_speed(&feedback_motor, speed_feedback_rpm_x100(packet.speed_raw))?;
|
||||
last_feedback = Instant::now();
|
||||
}
|
||||
if let Some(revolution) = assembler.push(packet)
|
||||
&& revolution_publishable(revolution.packet_count)
|
||||
{
|
||||
publish_revolution(&sensor_data, &revolution, forward_radians);
|
||||
}
|
||||
}
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn publish_revolution(
|
||||
sensor_data: &Arc<Mutex<SensorGroupData>>,
|
||||
revolution: &LdsRevolution,
|
||||
forward_radians: f32,
|
||||
) {
|
||||
let points = revolution
|
||||
.samples
|
||||
.iter()
|
||||
.enumerate()
|
||||
.filter_map(|(raw_angle, sample)| {
|
||||
let sample = sample.as_ref()?;
|
||||
if sample.invalid() || sample.distance_mm() == 0 {
|
||||
return None;
|
||||
}
|
||||
let raw_radians = raw_angle as f32 * PI / 180.0;
|
||||
let body_bearing = (forward_radians - raw_radians).rem_euclid(TAU);
|
||||
Some((
|
||||
body_bearing,
|
||||
sample.distance_mm() as f32 / 1000.0,
|
||||
sample.signal_strength.min(u8::MAX as u16) as u8,
|
||||
))
|
||||
})
|
||||
.collect::<Vec<_>>();
|
||||
|
||||
let Ok(mut data) = sensor_data.lock() else {
|
||||
log::error!("S5 Max lidar sensor-group mutex poisoned");
|
||||
return;
|
||||
};
|
||||
data.set("scan", SensorValue::PointCloud2D(points));
|
||||
data.set(
|
||||
"rotation_speed_rpm",
|
||||
SensorValue::F32(revolution.mean_speed_rpm()),
|
||||
);
|
||||
data.set(
|
||||
"invalid_samples",
|
||||
SensorValue::U16(revolution.invalid_sample_count() as u16),
|
||||
);
|
||||
data.touch();
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{
|
||||
COLD_START_FEEDBACK_GRACE, publish_revolution, revolution_publishable,
|
||||
speed_feedback_rpm_x100, zero_feedback_due,
|
||||
};
|
||||
use crate::core::types::{SensorGroupData, SensorValue};
|
||||
use crate::devices::roborock_s5max::lds::{
|
||||
INDEX_MIN, LdsDecoder, LdsRevolutionAssembler, PACKET_LENGTH,
|
||||
};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
#[test]
|
||||
fn applies_the_measured_clockwise_to_body_transform() {
|
||||
// Use the parser's captured packet fixture and close its partial scan
|
||||
// with a wrapped index. This test checks transformation independently
|
||||
// of UART ownership.
|
||||
let raw: [u8; PACKET_LENGTH] = [
|
||||
0xfa, 0xbb, 0xc2, 0x4a, 0x7f, 0x0f, 0x78, 0x00, 0x64, 0x0f, 0x8e, 0x01, 0x27, 0x0f,
|
||||
0x6d, 0x02, 0xf4, 0x0e, 0xf7, 0x02, 0x45, 0x40,
|
||||
];
|
||||
let mut decoder = LdsDecoder::new();
|
||||
let packet = decoder.push(&raw).pop().unwrap();
|
||||
let mut assembler = LdsRevolutionAssembler::new();
|
||||
assert!(assembler.push(packet).is_none());
|
||||
|
||||
// `finish` returns the partial revolution; publication itself accepts
|
||||
// partial fixtures although the production loop publishes only complete scans.
|
||||
let revolution = assembler.finish().unwrap();
|
||||
let data = Arc::new(Mutex::new(SensorGroupData::new("lidar")));
|
||||
publish_revolution(&data, &revolution, 261.2_f32.to_radians());
|
||||
let data = data.lock().unwrap();
|
||||
let SensorValue::PointCloud2D(points) = data.values.get("scan").unwrap() else {
|
||||
panic!("scan has unexpected type");
|
||||
};
|
||||
let expected = (261.2_f32 - (packet.index - INDEX_MIN) as f32 * 4.0).to_radians();
|
||||
assert!((points[0].0 - expected).abs() < 1e-5);
|
||||
assert!((points[0].1 - 3.967).abs() < 1e-6);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn converts_packet_q10_6_speed_to_kernel_feedback() {
|
||||
assert_eq!(speed_feedback_rpm_x100(0), 0);
|
||||
assert_eq!(speed_feedback_rpm_x100(300 * 64), 30_000);
|
||||
assert_eq!(speed_feedback_rpm_x100(0x4ac2), 29_903);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn accepts_only_sufficiently_complete_revolutions() {
|
||||
assert!(!revolution_publishable(74));
|
||||
assert!(revolution_publishable(75));
|
||||
assert!(revolution_publishable(90));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cold_rotor_gets_a_bounded_feedback_grace_period() {
|
||||
let now = Instant::now();
|
||||
let last_feedback = now - Duration::from_millis(100);
|
||||
assert!(!zero_feedback_due(
|
||||
true,
|
||||
false,
|
||||
Some(now),
|
||||
last_feedback,
|
||||
now
|
||||
));
|
||||
assert!(zero_feedback_due(true, true, Some(now), last_feedback, now));
|
||||
let after_grace = now + COLD_START_FEEDBACK_GRACE;
|
||||
assert!(zero_feedback_due(
|
||||
true,
|
||||
false,
|
||||
Some(now),
|
||||
last_feedback,
|
||||
after_grace
|
||||
));
|
||||
assert!(!zero_feedback_due(
|
||||
false,
|
||||
true,
|
||||
Some(now),
|
||||
last_feedback,
|
||||
after_grace
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,223 @@
|
||||
//! Explicit S5 Max driver lifecycle and telemetry-freshness state.
|
||||
|
||||
use crate::core::types::{SensorGroupData, SensorValue};
|
||||
use crate::error::{Error, Result};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
pub(crate) const SAFETY_REPORT_TIMEOUT: Duration = Duration::from_millis(500);
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub(crate) enum DriverState {
|
||||
Created,
|
||||
Synchronizing,
|
||||
ReadOnlyReady,
|
||||
Operational,
|
||||
Stopping,
|
||||
Stopped,
|
||||
Fault,
|
||||
}
|
||||
|
||||
impl DriverState {
|
||||
const fn name(self) -> &'static str {
|
||||
match self {
|
||||
Self::Created => "created",
|
||||
Self::Synchronizing => "synchronizing",
|
||||
Self::ReadOnlyReady => "read_only_ready",
|
||||
Self::Operational => "operational",
|
||||
Self::Stopping => "stopping",
|
||||
Self::Stopped => "stopped",
|
||||
Self::Fault => "fault",
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct LifecycleInner {
|
||||
state: DriverState,
|
||||
last_fault: Option<String>,
|
||||
last_safety_report: Option<Instant>,
|
||||
}
|
||||
|
||||
#[derive(Clone)]
|
||||
pub(crate) struct Lifecycle {
|
||||
inner: Arc<Mutex<LifecycleInner>>,
|
||||
sensor_data: Arc<Mutex<SensorGroupData>>,
|
||||
}
|
||||
|
||||
impl Lifecycle {
|
||||
pub(crate) fn new() -> Self {
|
||||
let sensor_data = Arc::new(Mutex::new(SensorGroupData::new("driver_status")));
|
||||
let lifecycle = Self {
|
||||
inner: Arc::new(Mutex::new(LifecycleInner {
|
||||
state: DriverState::Created,
|
||||
last_fault: None,
|
||||
last_safety_report: None,
|
||||
})),
|
||||
sensor_data,
|
||||
};
|
||||
lifecycle.publish(DriverState::Created, None);
|
||||
lifecycle
|
||||
}
|
||||
|
||||
pub(crate) fn sensor_data(&self) -> Arc<Mutex<SensorGroupData>> {
|
||||
Arc::clone(&self.sensor_data)
|
||||
}
|
||||
|
||||
pub(crate) fn state(&self) -> DriverState {
|
||||
self.inner
|
||||
.lock()
|
||||
.map(|inner| inner.state)
|
||||
.unwrap_or(DriverState::Fault)
|
||||
}
|
||||
|
||||
pub(crate) fn begin_synchronizing(&self) -> Result<()> {
|
||||
let current = self.state();
|
||||
if !matches!(current, DriverState::Created | DriverState::Stopped) {
|
||||
return Err(Error::Other(format!(
|
||||
"S5 Max cannot initialize from lifecycle state {}",
|
||||
current.name()
|
||||
)));
|
||||
}
|
||||
self.set_state(DriverState::Synchronizing)
|
||||
}
|
||||
|
||||
pub(crate) fn mark_read_only_ready(&self) -> Result<()> {
|
||||
self.transition(DriverState::Synchronizing, DriverState::ReadOnlyReady)
|
||||
}
|
||||
|
||||
pub(crate) fn mark_operational(&self) -> Result<()> {
|
||||
self.transition(DriverState::ReadOnlyReady, DriverState::Operational)
|
||||
}
|
||||
|
||||
pub(crate) fn begin_stopping(&self) -> Result<()> {
|
||||
match self.state() {
|
||||
DriverState::Created | DriverState::Stopped => Ok(()),
|
||||
DriverState::Fault => Ok(()),
|
||||
DriverState::Synchronizing | DriverState::ReadOnlyReady | DriverState::Operational => {
|
||||
self.set_state(DriverState::Stopping)
|
||||
}
|
||||
DriverState::Stopping => Ok(()),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn mark_stopped(&self) -> Result<()> {
|
||||
match self.state() {
|
||||
DriverState::Created | DriverState::Stopped => self.set_state(DriverState::Stopped),
|
||||
DriverState::Stopping => self.set_state(DriverState::Stopped),
|
||||
DriverState::Fault => Ok(()),
|
||||
state => Err(Error::Other(format!(
|
||||
"S5 Max cannot enter stopped from {}",
|
||||
state.name()
|
||||
))),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn fault(&self, reason: impl Into<String>) {
|
||||
let reason = reason.into();
|
||||
if let Ok(mut inner) = self.inner.lock() {
|
||||
inner.state = DriverState::Fault;
|
||||
inner.last_fault = Some(reason.clone());
|
||||
} else {
|
||||
log::error!("S5 Max lifecycle mutex poisoned while recording fault: {reason}");
|
||||
}
|
||||
self.publish(DriverState::Fault, Some(&reason));
|
||||
}
|
||||
|
||||
pub(crate) fn mark_safety_report(&self, now: Instant) {
|
||||
if let Ok(mut inner) = self.inner.lock() {
|
||||
inner.last_safety_report = Some(now);
|
||||
} else {
|
||||
self.fault("lifecycle mutex poisoned while recording safety telemetry");
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn safety_reports_fresh(&self, now: Instant) -> bool {
|
||||
self.inner
|
||||
.lock()
|
||||
.ok()
|
||||
.and_then(|inner| inner.last_safety_report)
|
||||
.is_some_and(|last| now.saturating_duration_since(last) <= SAFETY_REPORT_TIMEOUT)
|
||||
}
|
||||
|
||||
fn transition(&self, from: DriverState, to: DriverState) -> Result<()> {
|
||||
let current = self.state();
|
||||
if current != from {
|
||||
return Err(Error::Other(format!(
|
||||
"invalid S5 Max lifecycle transition {} -> {}; expected {}",
|
||||
current.name(),
|
||||
to.name(),
|
||||
from.name()
|
||||
)));
|
||||
}
|
||||
self.set_state(to)
|
||||
}
|
||||
|
||||
fn set_state(&self, state: DriverState) -> Result<()> {
|
||||
let fault = {
|
||||
let mut inner = self
|
||||
.inner
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max lifecycle".to_string()))?;
|
||||
inner.state = state;
|
||||
if state == DriverState::Synchronizing {
|
||||
inner.last_fault = None;
|
||||
inner.last_safety_report = None;
|
||||
}
|
||||
inner.last_fault.clone()
|
||||
};
|
||||
self.publish(state, fault.as_deref());
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn publish(&self, state: DriverState, fault: Option<&str>) {
|
||||
let Ok(mut data) = self.sensor_data.lock() else {
|
||||
log::error!("S5 Max driver-status sensor mutex poisoned");
|
||||
return;
|
||||
};
|
||||
data.set("state", SensorValue::String(state.name().to_string()));
|
||||
data.set("faulted", SensorValue::Bool(state == DriverState::Fault));
|
||||
if let Some(fault) = fault {
|
||||
data.set("last_fault", SensorValue::String(fault.to_string()));
|
||||
}
|
||||
data.touch();
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{DriverState, Lifecycle, SAFETY_REPORT_TIMEOUT};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
#[test]
|
||||
fn enforces_the_explicit_lifecycle() {
|
||||
let lifecycle = Lifecycle::new();
|
||||
assert_eq!(lifecycle.state(), DriverState::Created);
|
||||
assert!(lifecycle.mark_operational().is_err());
|
||||
lifecycle.begin_synchronizing().unwrap();
|
||||
lifecycle.mark_read_only_ready().unwrap();
|
||||
lifecycle.mark_operational().unwrap();
|
||||
lifecycle.begin_stopping().unwrap();
|
||||
lifecycle.mark_stopped().unwrap();
|
||||
assert_eq!(lifecycle.state(), DriverState::Stopped);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn tracks_fresh_safety_reports_and_fault_reason() {
|
||||
let lifecycle = Lifecycle::new();
|
||||
let now = Instant::now();
|
||||
assert!(!lifecycle.safety_reports_fresh(now));
|
||||
lifecycle.mark_safety_report(now);
|
||||
assert!(lifecycle.safety_reports_fresh(now + SAFETY_REPORT_TIMEOUT));
|
||||
assert!(
|
||||
!lifecycle.safety_reports_fresh(now + SAFETY_REPORT_TIMEOUT + Duration::from_millis(1))
|
||||
);
|
||||
|
||||
lifecycle.fault("test fault");
|
||||
assert_eq!(lifecycle.state(), DriverState::Fault);
|
||||
let data = lifecycle.sensor_data.lock().unwrap();
|
||||
assert!(matches!(
|
||||
data.values.get("last_fault"),
|
||||
Some(crate::core::types::SensorValue::String(value)) if value == "test fault"
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,904 @@
|
||||
//! Single-owner MCU session with framed reports and guarded command dispatch.
|
||||
|
||||
use super::commands::{
|
||||
ACK_REPORT, FAN_SUBSYSTEM, MAIN_BRUSH_SUBSYSTEM, SIDE_BRUSH_SUBSYSTEM,
|
||||
WHEEL_ODOMETRY_SUBSYSTEM, acknowledgement_frame, charger_command_frame, fan_command_frame,
|
||||
heartbeat_frame, main_brush_command_frame, next_tx_sequence, sensor_sync_query_frame,
|
||||
side_brush_command_frame, subsystem_state_frame, system_mode_command_frame,
|
||||
water_pump_command_frame, wheel_command_frame,
|
||||
};
|
||||
use super::lifecycle::{Lifecycle, SAFETY_REPORT_TIMEOUT};
|
||||
use super::packet::{FrameDecoder, ReportFooter};
|
||||
use super::safety::SafetyController;
|
||||
use super::sensors::SensorGroups;
|
||||
use super::sys::MCU_BAUD;
|
||||
use super::tty::ExclusiveTty;
|
||||
use crate::error::{Error, Result};
|
||||
use crossbeam_channel::{Receiver, Sender, bounded};
|
||||
use std::cell::Cell;
|
||||
use std::collections::HashMap;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::sync::{Arc, Condvar, Mutex};
|
||||
use std::thread::{self, JoinHandle};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
const MCU_BAUD_NUMBER: u32 = 1_152_000;
|
||||
const HEARTBEAT_INTERVAL: Duration = Duration::from_secs(1);
|
||||
const ACK_TIMEOUT: Duration = Duration::from_millis(23);
|
||||
const MAX_SYNC_ATTEMPTS: u8 = 3;
|
||||
const WORKER_STARTUP_TIMEOUT: Duration = Duration::from_millis(1_500);
|
||||
const STARTUP_WAIT_TIMEOUT: Duration = Duration::from_secs(2);
|
||||
const ACK_LENGTH: u8 = 1;
|
||||
const ACK_DEDUP_WINDOW_FRAMES: u32 = 128;
|
||||
const COMMAND_QUEUE_CAPACITY: usize = 8;
|
||||
const COMMAND_TIMEOUT: Duration = Duration::from_millis(200);
|
||||
|
||||
#[derive(Debug, Clone, Copy)]
|
||||
pub(crate) enum ActuatorCommand {
|
||||
WakeMcu,
|
||||
EnableSubsystem(u32),
|
||||
Drive { linear: f32, angular: f32 },
|
||||
StopDrive,
|
||||
Fan(u8),
|
||||
MainBrush(u8),
|
||||
SideBrush(u8),
|
||||
WaterPump { on_interval: u8, off_interval: u8 },
|
||||
Charger(bool),
|
||||
EmergencyStop,
|
||||
}
|
||||
|
||||
struct CommandRequest {
|
||||
command: ActuatorCommand,
|
||||
reply: Sender<Result<Vec<u8>>>,
|
||||
}
|
||||
|
||||
struct SessionChannels {
|
||||
ready: Sender<Result<()>>,
|
||||
commands: Receiver<CommandRequest>,
|
||||
command_acks: Arc<CommandAckTracker>,
|
||||
}
|
||||
|
||||
const DOCK_STATUS_REPORT: u8 = 0x20;
|
||||
const BATTERY_STATUS_REPORT: u8 = 0x08;
|
||||
const DUSTBIN_STATUS_REPORT: u8 = 0x24;
|
||||
const DOCK_VOLTAGE_STATUS_REPORT: u8 = 0x0f;
|
||||
|
||||
#[derive(Debug)]
|
||||
struct RecentAckRequest {
|
||||
frame_index: u32,
|
||||
sequence: u8,
|
||||
payload: Vec<u8>,
|
||||
}
|
||||
|
||||
#[derive(Debug, Default)]
|
||||
struct RecentAckRequests {
|
||||
entries: Vec<RecentAckRequest>,
|
||||
}
|
||||
|
||||
impl RecentAckRequests {
|
||||
fn is_duplicate(&mut self, frame_index: u32, sequence: u8, payload: &[u8]) -> bool {
|
||||
self.entries.retain(|entry| {
|
||||
frame_index.saturating_sub(entry.frame_index) < ACK_DEDUP_WINDOW_FRAMES
|
||||
});
|
||||
if self
|
||||
.entries
|
||||
.iter()
|
||||
.any(|entry| entry.sequence == sequence && entry.payload == payload)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
self.entries.push(RecentAckRequest {
|
||||
frame_index,
|
||||
sequence,
|
||||
payload: payload.to_vec(),
|
||||
});
|
||||
false
|
||||
}
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
struct AckRetryWindow {
|
||||
attempts: u8,
|
||||
next_retry: Instant,
|
||||
}
|
||||
|
||||
#[derive(Debug, Default)]
|
||||
struct CommandAckTracker {
|
||||
states: Mutex<HashMap<u8, CommandAckState>>,
|
||||
changed: Condvar,
|
||||
}
|
||||
|
||||
#[derive(Debug)]
|
||||
struct CommandAckState {
|
||||
acknowledged: bool,
|
||||
frame: Vec<u8>,
|
||||
attempts: u8,
|
||||
next_retry: Instant,
|
||||
}
|
||||
|
||||
impl CommandAckTracker {
|
||||
fn expect(&self, sequence: u8, frame: &[u8], now: Instant) -> Result<()> {
|
||||
self.states
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max command ACK tracker".to_string()))?
|
||||
.insert(
|
||||
sequence,
|
||||
CommandAckState {
|
||||
acknowledged: false,
|
||||
frame: frame.to_vec(),
|
||||
attempts: 1,
|
||||
next_retry: now + ACK_TIMEOUT,
|
||||
},
|
||||
);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn cancel(&self, sequence: u8) {
|
||||
if let Ok(mut states) = self.states.lock() {
|
||||
states.remove(&sequence);
|
||||
}
|
||||
}
|
||||
|
||||
fn acknowledge(&self, sequence: u8) {
|
||||
if let Ok(mut states) = self.states.lock()
|
||||
&& let Some(state) = states.get_mut(&sequence)
|
||||
{
|
||||
state.acknowledged = true;
|
||||
self.changed.notify_all();
|
||||
}
|
||||
}
|
||||
|
||||
fn take_due_retries(&self, now: Instant) -> Result<Vec<(u8, Vec<u8>, u8)>> {
|
||||
let mut states = self
|
||||
.states
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max command ACK tracker".to_string()))?;
|
||||
let mut retries = Vec::new();
|
||||
for (&sequence, state) in states.iter_mut() {
|
||||
if !state.acknowledged && state.attempts < MAX_SYNC_ATTEMPTS && now >= state.next_retry
|
||||
{
|
||||
state.attempts += 1;
|
||||
state.next_retry = now + ACK_TIMEOUT;
|
||||
retries.push((sequence, state.frame.clone(), state.attempts));
|
||||
}
|
||||
}
|
||||
Ok(retries)
|
||||
}
|
||||
|
||||
fn wait_for_all(&self, sequences: &[u8], timeout: Duration) -> Result<()> {
|
||||
if sequences.is_empty() {
|
||||
return Ok(());
|
||||
}
|
||||
let states = self
|
||||
.states
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max command ACK tracker".to_string()))?;
|
||||
let (mut states, _) = self
|
||||
.changed
|
||||
.wait_timeout_while(states, timeout, |states| {
|
||||
sequences
|
||||
.iter()
|
||||
.any(|sequence| !states.get(sequence).is_some_and(|state| state.acknowledged))
|
||||
})
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max command ACK tracker".to_string()))?;
|
||||
let missing = sequences
|
||||
.iter()
|
||||
.copied()
|
||||
.filter(|sequence| !states.get(sequence).is_some_and(|state| state.acknowledged))
|
||||
.collect::<Vec<_>>();
|
||||
for sequence in sequences {
|
||||
states.remove(sequence);
|
||||
}
|
||||
if missing.is_empty() {
|
||||
Ok(())
|
||||
} else {
|
||||
Err(Error::Other(format!(
|
||||
"MCU command ACK timeout; missing sequences {missing:02x?}"
|
||||
)))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl AckRetryWindow {
|
||||
fn after_initial_send(now: Instant) -> Self {
|
||||
Self {
|
||||
attempts: 1,
|
||||
next_retry: now + ACK_TIMEOUT,
|
||||
}
|
||||
}
|
||||
|
||||
fn take_retry(&mut self, now: Instant) -> bool {
|
||||
if now < self.next_retry || self.attempts >= MAX_SYNC_ATTEMPTS {
|
||||
return false;
|
||||
}
|
||||
self.attempts += 1;
|
||||
self.next_retry = now + ACK_TIMEOUT;
|
||||
true
|
||||
}
|
||||
|
||||
fn exhausted(&self, now: Instant) -> bool {
|
||||
self.attempts == MAX_SYNC_ATTEMPTS && now >= self.next_retry
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct McuDriver {
|
||||
handle: Option<JoinHandle<Result<()>>>,
|
||||
command_tx: Sender<CommandRequest>,
|
||||
command_acks: Arc<CommandAckTracker>,
|
||||
}
|
||||
|
||||
impl McuDriver {
|
||||
pub(crate) fn start(
|
||||
path: &str,
|
||||
last_stock_sequence: u8,
|
||||
sensors: SensorGroups,
|
||||
lifecycle: Lifecycle,
|
||||
safety: SafetyController,
|
||||
shutdown: Arc<AtomicBool>,
|
||||
) -> Result<Self> {
|
||||
let tty = ExclusiveTty::open(path, MCU_BAUD, MCU_BAUD_NUMBER)?;
|
||||
let (ready_tx, ready_rx) = bounded(1);
|
||||
let (command_tx, command_rx) = bounded(COMMAND_QUEUE_CAPACITY);
|
||||
let command_acks = Arc::new(CommandAckTracker::default());
|
||||
let worker_command_acks = Arc::clone(&command_acks);
|
||||
let path = path.to_string();
|
||||
let thread_shutdown = Arc::clone(&shutdown);
|
||||
let thread_lifecycle = lifecycle.clone();
|
||||
let handle = thread::Builder::new()
|
||||
.name("s5max-mcu".to_string())
|
||||
.spawn(move || {
|
||||
let result = session_loop(
|
||||
tty,
|
||||
last_stock_sequence,
|
||||
sensors,
|
||||
lifecycle,
|
||||
safety,
|
||||
&thread_shutdown,
|
||||
SessionChannels {
|
||||
ready: ready_tx,
|
||||
commands: command_rx,
|
||||
command_acks: worker_command_acks,
|
||||
},
|
||||
);
|
||||
if let Err(error) = &result {
|
||||
log::error!("S5 Max MCU worker failed on {path}: {error}");
|
||||
thread_lifecycle.fault(format!("MCU worker: {error}"));
|
||||
thread_shutdown.store(true, Ordering::Release);
|
||||
}
|
||||
result
|
||||
})
|
||||
.map_err(|error| Error::Other(format!("spawn S5 Max MCU worker: {error}")))?;
|
||||
let mut driver = Self {
|
||||
handle: Some(handle),
|
||||
command_tx,
|
||||
command_acks,
|
||||
};
|
||||
|
||||
match ready_rx.recv_timeout(STARTUP_WAIT_TIMEOUT) {
|
||||
Ok(Ok(())) => Ok(driver),
|
||||
Ok(Err(error)) => {
|
||||
shutdown.store(true, Ordering::Release);
|
||||
let _ = driver.join();
|
||||
Err(error)
|
||||
}
|
||||
Err(error) => {
|
||||
shutdown.store(true, Ordering::Release);
|
||||
let _ = driver.join();
|
||||
Err(Error::Other(format!(
|
||||
"S5 Max MCU startup did not complete within {}ms: {error}",
|
||||
STARTUP_WAIT_TIMEOUT.as_millis()
|
||||
)))
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn execute(&self, command: ActuatorCommand) -> Result<()> {
|
||||
let (reply_tx, reply_rx) = bounded(1);
|
||||
self.command_tx
|
||||
.send_timeout(
|
||||
CommandRequest {
|
||||
command,
|
||||
reply: reply_tx,
|
||||
},
|
||||
COMMAND_TIMEOUT,
|
||||
)
|
||||
.map_err(|error| Error::Other(format!("S5 Max MCU command queue: {error}")))?;
|
||||
let acknowledged_sequences = reply_rx
|
||||
.recv_timeout(COMMAND_TIMEOUT)
|
||||
.map_err(|error| Error::Other(format!("S5 Max MCU command timeout: {error}")))??;
|
||||
self.command_acks
|
||||
.wait_for_all(&acknowledged_sequences, COMMAND_TIMEOUT)
|
||||
}
|
||||
|
||||
pub(crate) fn join(&mut self) -> Result<()> {
|
||||
let Some(handle) = self.handle.take() else {
|
||||
return Ok(());
|
||||
};
|
||||
handle.join().map_err(|_| Error::ThreadPanic)?
|
||||
}
|
||||
}
|
||||
|
||||
struct McuSafetyGuard<'a> {
|
||||
tty: &'a ExclusiveTty,
|
||||
next_sequence: Cell<u8>,
|
||||
wheel_subsystem_enabled: Cell<bool>,
|
||||
safety: SafetyController,
|
||||
command_acks: Arc<CommandAckTracker>,
|
||||
armed: bool,
|
||||
}
|
||||
|
||||
impl<'a> McuSafetyGuard<'a> {
|
||||
fn new(
|
||||
tty: &'a ExclusiveTty,
|
||||
first_sequence: u8,
|
||||
safety: SafetyController,
|
||||
command_acks: Arc<CommandAckTracker>,
|
||||
) -> Self {
|
||||
Self {
|
||||
tty,
|
||||
next_sequence: Cell::new(first_sequence),
|
||||
wheel_subsystem_enabled: Cell::new(false),
|
||||
safety,
|
||||
command_acks,
|
||||
armed: true,
|
||||
}
|
||||
}
|
||||
|
||||
fn take_sequence(&self) -> u8 {
|
||||
let sequence = self.next_sequence.get();
|
||||
self.next_sequence.set(next_tx_sequence(sequence));
|
||||
sequence
|
||||
}
|
||||
|
||||
fn send(&self, frame: std::result::Result<Vec<u8>, String>) -> Result<()> {
|
||||
self.tty.write_complete(&frame.map_err(Error::Other)?)
|
||||
}
|
||||
|
||||
fn send_requiring_ack(
|
||||
&self,
|
||||
sequence: u8,
|
||||
frame: std::result::Result<Vec<u8>, String>,
|
||||
) -> Result<u8> {
|
||||
let frame = frame.map_err(Error::Other)?;
|
||||
self.command_acks.expect(sequence, &frame, Instant::now())?;
|
||||
if let Err(error) = self.tty.write_complete(&frame) {
|
||||
self.command_acks.cancel(sequence);
|
||||
return Err(error);
|
||||
}
|
||||
Ok(sequence)
|
||||
}
|
||||
|
||||
fn forced_wheel_stop(&self) -> Result<()> {
|
||||
self.send(wheel_command_frame(self.take_sequence(), 0.0, 0.0, true))
|
||||
}
|
||||
|
||||
fn execute(&self, command: ActuatorCommand) -> Result<Vec<u8>> {
|
||||
let mut acknowledgements = Vec::new();
|
||||
match command {
|
||||
ActuatorCommand::WakeMcu => {
|
||||
self.send(system_mode_command_frame(self.take_sequence(), 0))?;
|
||||
}
|
||||
ActuatorCommand::EnableSubsystem(subsystem) => {
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
subsystem_state_frame(sequence, subsystem, true),
|
||||
)?);
|
||||
if subsystem == WHEEL_ODOMETRY_SUBSYSTEM {
|
||||
self.wheel_subsystem_enabled.set(true);
|
||||
}
|
||||
}
|
||||
ActuatorCommand::Drive { linear, angular } => {
|
||||
if (linear != 0.0 || angular != 0.0) && !self.wheel_subsystem_enabled.get() {
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
subsystem_state_frame(sequence, WHEEL_ODOMETRY_SUBSYSTEM, true),
|
||||
)?);
|
||||
// The b1 selector enables the subsystem for the MCU session;
|
||||
// periodic c0 velocity refreshes must not resend it. Doing so
|
||||
// floods the acknowledgement window and eventually wraps the
|
||||
// one-byte transmit sequence.
|
||||
self.wheel_subsystem_enabled.set(true);
|
||||
}
|
||||
self.send(wheel_command_frame(
|
||||
self.take_sequence(),
|
||||
linear,
|
||||
angular,
|
||||
false,
|
||||
))?;
|
||||
self.safety
|
||||
.arm_drive_lease(linear, angular, Instant::now())?;
|
||||
}
|
||||
ActuatorCommand::StopDrive => {
|
||||
self.forced_wheel_stop()?;
|
||||
self.safety.clear_drive_target()?;
|
||||
}
|
||||
ActuatorCommand::Fan(target) => {
|
||||
if target != 0 {
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
subsystem_state_frame(sequence, FAN_SUBSYSTEM, true),
|
||||
)?);
|
||||
}
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements
|
||||
.push(self.send_requiring_ack(sequence, fan_command_frame(sequence, target))?);
|
||||
self.safety.set_fan(target)?;
|
||||
}
|
||||
ActuatorCommand::MainBrush(target) => {
|
||||
if target != 0 {
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
subsystem_state_frame(sequence, MAIN_BRUSH_SUBSYSTEM, true),
|
||||
)?);
|
||||
}
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
main_brush_command_frame(sequence, target, false),
|
||||
)?);
|
||||
self.safety.set_main_brush(target)?;
|
||||
}
|
||||
ActuatorCommand::SideBrush(target) => {
|
||||
if target != 0 {
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
subsystem_state_frame(sequence, SIDE_BRUSH_SUBSYSTEM, true),
|
||||
)?);
|
||||
}
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(self.send_requiring_ack(
|
||||
sequence,
|
||||
side_brush_command_frame(sequence, target, false),
|
||||
)?);
|
||||
self.safety.set_side_brush(target)?;
|
||||
}
|
||||
ActuatorCommand::WaterPump {
|
||||
on_interval,
|
||||
off_interval,
|
||||
} => {
|
||||
self.send(water_pump_command_frame(
|
||||
self.take_sequence(),
|
||||
on_interval,
|
||||
off_interval,
|
||||
))?;
|
||||
self.safety.set_water_pump(on_interval)?;
|
||||
}
|
||||
ActuatorCommand::Charger(enabled) => {
|
||||
let sequence = self.take_sequence();
|
||||
acknowledgements.push(
|
||||
self.send_requiring_ack(sequence, charger_command_frame(sequence, enabled))?,
|
||||
);
|
||||
}
|
||||
ActuatorCommand::EmergencyStop => self.emit_stop_set()?,
|
||||
}
|
||||
Ok(acknowledgements)
|
||||
}
|
||||
|
||||
fn emit_stop_set(&self) -> Result<()> {
|
||||
let mut failures = Vec::new();
|
||||
let (records, next_sequence) = ordered_stop_frames(self.next_sequence.get());
|
||||
self.next_sequence.set(next_sequence);
|
||||
for (description, frame) in records {
|
||||
if let Err(error) = self.send(frame) {
|
||||
failures.push(format!("{description}: {error}"));
|
||||
}
|
||||
}
|
||||
self.safety.reset_targets();
|
||||
if failures.is_empty() {
|
||||
Ok(())
|
||||
} else {
|
||||
Err(Error::Other(failures.join("; ")))
|
||||
}
|
||||
}
|
||||
|
||||
fn stop_and_disarm(&mut self) -> Result<()> {
|
||||
self.emit_stop_set()?;
|
||||
self.armed = false;
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
type StopRecord = (&'static str, std::result::Result<Vec<u8>, String>);
|
||||
|
||||
fn ordered_stop_frames(first_sequence: u8) -> ([StopRecord; 5], u8) {
|
||||
let wheel_sequence = first_sequence;
|
||||
let fan_sequence = next_tx_sequence(wheel_sequence);
|
||||
let side_brush_sequence = next_tx_sequence(fan_sequence);
|
||||
let main_brush_sequence = next_tx_sequence(side_brush_sequence);
|
||||
let water_pump_sequence = next_tx_sequence(main_brush_sequence);
|
||||
let next_sequence = next_tx_sequence(water_pump_sequence);
|
||||
(
|
||||
[
|
||||
(
|
||||
"forced zero-wheel stop",
|
||||
wheel_command_frame(wheel_sequence, 0.0, 0.0, true),
|
||||
),
|
||||
("hard fan stop", fan_command_frame(fan_sequence, 0)),
|
||||
(
|
||||
"side-brush stop",
|
||||
side_brush_command_frame(side_brush_sequence, 0, false),
|
||||
),
|
||||
(
|
||||
"main-brush stop",
|
||||
main_brush_command_frame(main_brush_sequence, 0, false),
|
||||
),
|
||||
(
|
||||
"water-pump stop",
|
||||
water_pump_command_frame(water_pump_sequence, 0, 0),
|
||||
),
|
||||
],
|
||||
next_sequence,
|
||||
)
|
||||
}
|
||||
|
||||
impl Drop for McuSafetyGuard<'_> {
|
||||
fn drop(&mut self) {
|
||||
if self.armed
|
||||
&& let Err(error) = self.emit_stop_set()
|
||||
{
|
||||
log::error!("S5 Max emergency stop retry failed: {error}");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn contains_sensor_snapshot(frame: &super::packet::Frame) -> bool {
|
||||
let shape = frame
|
||||
.reports()
|
||||
.map(|report| (report.message, report.data.len()))
|
||||
.collect::<Vec<_>>();
|
||||
[
|
||||
(DOCK_STATUS_REPORT, 2),
|
||||
(BATTERY_STATUS_REPORT, 10),
|
||||
(DUSTBIN_STATUS_REPORT, 2),
|
||||
(DOCK_VOLTAGE_STATUS_REPORT, 2),
|
||||
]
|
||||
.into_iter()
|
||||
.all(|required| shape.contains(&required))
|
||||
}
|
||||
|
||||
fn report_requests_ack(footer: Option<ReportFooter>) -> bool {
|
||||
footer.is_some_and(|footer| footer.reports_pending)
|
||||
}
|
||||
|
||||
fn notify_startup(sender: &mut Option<Sender<Result<()>>>, result: Result<()>) {
|
||||
if let Some(sender) = sender.take() {
|
||||
let _ = sender.send(result);
|
||||
}
|
||||
}
|
||||
|
||||
fn session_loop(
|
||||
tty: ExclusiveTty,
|
||||
last_stock_sequence: u8,
|
||||
sensors: SensorGroups,
|
||||
lifecycle: Lifecycle,
|
||||
safety: SafetyController,
|
||||
shutdown: &AtomicBool,
|
||||
channels: SessionChannels,
|
||||
) -> Result<()> {
|
||||
let first_sequence = next_tx_sequence(last_stock_sequence);
|
||||
let mut guard = McuSafetyGuard::new(
|
||||
&tty,
|
||||
first_sequence,
|
||||
safety,
|
||||
Arc::clone(&channels.command_acks),
|
||||
);
|
||||
let session_result = guarded_session_loop(&guard, sensors, lifecycle, shutdown, channels);
|
||||
let stop_result = guard.stop_and_disarm();
|
||||
match (session_result, stop_result) {
|
||||
(Err(session), Err(stop)) => Err(Error::Other(format!(
|
||||
"{session}; ordered safety stop also failed: {stop}"
|
||||
))),
|
||||
(Err(error), Ok(())) | (Ok(()), Err(error)) => Err(error),
|
||||
(Ok(()), Ok(())) => Ok(()),
|
||||
}
|
||||
}
|
||||
|
||||
fn guarded_session_loop(
|
||||
guard: &McuSafetyGuard<'_>,
|
||||
sensors: SensorGroups,
|
||||
lifecycle: Lifecycle,
|
||||
shutdown: &AtomicBool,
|
||||
channels: SessionChannels,
|
||||
) -> Result<()> {
|
||||
let SessionChannels {
|
||||
ready,
|
||||
commands: command_rx,
|
||||
command_acks: _,
|
||||
} = channels;
|
||||
let mut startup_sender = Some(ready);
|
||||
let startup_deadline = Instant::now() + WORKER_STARTUP_TIMEOUT;
|
||||
|
||||
guard.send(heartbeat_frame(guard.take_sequence()))?;
|
||||
let mut next_heartbeat = Instant::now() + HEARTBEAT_INTERVAL;
|
||||
|
||||
let sync_sequence = guard.take_sequence();
|
||||
let sync_frame = sensor_sync_query_frame(sync_sequence).map_err(Error::Other)?;
|
||||
guard.tty.write_complete(&sync_frame)?;
|
||||
let mut sync_retry = AckRetryWindow::after_initial_send(Instant::now());
|
||||
let mut sync_acknowledged = false;
|
||||
let mut snapshot_received = false;
|
||||
let mut safety_report_deadline = None;
|
||||
|
||||
let mut decoder = FrameDecoder::new();
|
||||
let mut recent_ack_requests = RecentAckRequests::default();
|
||||
let mut binary_frame_index = 0u32;
|
||||
let mut buffer = [0u8; 1024];
|
||||
|
||||
while !shutdown.load(Ordering::Acquire) {
|
||||
let now = Instant::now();
|
||||
while let Ok(request) = command_rx.try_recv() {
|
||||
let result = guard.execute(request.command);
|
||||
let failed = result.is_err();
|
||||
let _ = request.reply.send(result);
|
||||
if failed {
|
||||
return Err(Error::Other("S5 Max actuator command failed".to_string()));
|
||||
}
|
||||
}
|
||||
for (sequence, frame, attempt) in guard.command_acks.take_due_retries(now)? {
|
||||
log::warn!("Retrying S5 Max MCU command sequence 0x{sequence:02x}, attempt {attempt}");
|
||||
guard.tty.write_complete(&frame)?;
|
||||
}
|
||||
if guard.safety.take_expired_drive_lease(now) {
|
||||
log::warn!("S5 Max drive lease expired; sending forced zero-wheel stop");
|
||||
guard.forced_wheel_stop()?;
|
||||
}
|
||||
if safety_report_deadline.is_some_and(|deadline| now >= deadline) {
|
||||
return Err(Error::Other(format!(
|
||||
"S5 Max safety reports stale for more than {}ms",
|
||||
SAFETY_REPORT_TIMEOUT.as_millis()
|
||||
)));
|
||||
}
|
||||
if now >= next_heartbeat {
|
||||
guard.send(heartbeat_frame(guard.take_sequence()))?;
|
||||
next_heartbeat = now + HEARTBEAT_INTERVAL;
|
||||
}
|
||||
|
||||
if !sync_acknowledged && sync_retry.take_retry(now) {
|
||||
guard.tty.write_complete(&sync_frame)?;
|
||||
}
|
||||
|
||||
if startup_sender.is_some() && !sync_acknowledged && sync_retry.exhausted(now) {
|
||||
let error = Error::Other(format!(
|
||||
"MCU did not acknowledge sensor sync after {} attempts",
|
||||
sync_retry.attempts
|
||||
));
|
||||
notify_startup(&mut startup_sender, Err(error));
|
||||
return Err(Error::Other(
|
||||
"MCU sensor-sync acknowledgement exhausted".to_string(),
|
||||
));
|
||||
}
|
||||
if startup_sender.is_some() && now >= startup_deadline {
|
||||
let missing = match (sync_acknowledged, snapshot_received) {
|
||||
(false, false) => "sync acknowledgement and sensor snapshot",
|
||||
(false, true) => "sync acknowledgement",
|
||||
(true, false) => "sensor snapshot",
|
||||
(true, true) => unreachable!(),
|
||||
};
|
||||
notify_startup(
|
||||
&mut startup_sender,
|
||||
Err(Error::Other(format!(
|
||||
"MCU startup timed out waiting for {missing}"
|
||||
))),
|
||||
);
|
||||
return Err(Error::Other(format!(
|
||||
"MCU startup timed out waiting for {missing}"
|
||||
)));
|
||||
}
|
||||
|
||||
if !guard.tty.poll_readable(Duration::from_millis(20))? {
|
||||
continue;
|
||||
}
|
||||
let count = guard.tty.read_available(&mut buffer)?;
|
||||
for frame in decoder.push(&buffer[..count]) {
|
||||
if frame.is_log {
|
||||
continue;
|
||||
}
|
||||
if !frame.crc_valid || !frame.escape_overhead_valid() {
|
||||
log::warn!("Discarding invalid S5 Max MCU frame");
|
||||
continue;
|
||||
}
|
||||
binary_frame_index = binary_frame_index.wrapping_add(1);
|
||||
|
||||
let footer = frame.report_footer();
|
||||
let duplicate = footer.is_some_and(|footer| {
|
||||
footer.reports_pending
|
||||
&& recent_ack_requests.is_duplicate(
|
||||
binary_frame_index,
|
||||
footer.sequence,
|
||||
&frame.payload,
|
||||
)
|
||||
});
|
||||
|
||||
if frame.message == ACK_REPORT
|
||||
&& frame.command == ACK_LENGTH
|
||||
&& let Some(sequence) = frame.payload.get(2).copied()
|
||||
{
|
||||
guard.command_acks.acknowledge(sequence);
|
||||
if sequence == sync_sequence {
|
||||
sync_acknowledged = true;
|
||||
}
|
||||
}
|
||||
if contains_sensor_snapshot(&frame) && !duplicate {
|
||||
snapshot_received = true;
|
||||
}
|
||||
if !duplicate {
|
||||
sensors.process_mcu_frame(&frame);
|
||||
if frame.is_state_report() {
|
||||
lifecycle.mark_safety_report(now);
|
||||
safety_report_deadline = Some(now + SAFETY_REPORT_TIMEOUT);
|
||||
}
|
||||
}
|
||||
|
||||
if report_requests_ack(footer) {
|
||||
let footer = footer.expect("ACK request requires footer");
|
||||
guard.send(acknowledgement_frame(
|
||||
guard.take_sequence(),
|
||||
footer.sequence,
|
||||
))?;
|
||||
}
|
||||
|
||||
if startup_sender.is_some() && sync_acknowledged && snapshot_received {
|
||||
notify_startup(&mut startup_sender, Ok(()));
|
||||
safety_report_deadline = Some(now + SAFETY_REPORT_TIMEOUT);
|
||||
log::info!(
|
||||
"S5 Max MCU synchronized after {} attempt(s)",
|
||||
sync_retry.attempts
|
||||
);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if startup_sender.is_some() {
|
||||
notify_startup(
|
||||
&mut startup_sender,
|
||||
Err(Error::Other("MCU shutdown during startup".to_string())),
|
||||
);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{
|
||||
ACK_TIMEOUT, AckRetryWindow, CommandAckTracker, RecentAckRequests,
|
||||
contains_sensor_snapshot, notify_startup, ordered_stop_frames,
|
||||
};
|
||||
use crate::devices::roborock_s5max::commands::{
|
||||
fan_command_frame, main_brush_command_frame, side_brush_command_frame,
|
||||
water_pump_command_frame, wheel_command_frame,
|
||||
};
|
||||
use crate::devices::roborock_s5max::packet::Frame;
|
||||
use crossbeam_channel::{TryRecvError, bounded};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
#[test]
|
||||
fn matches_command_acknowledgements_by_sequence() {
|
||||
let tracker = CommandAckTracker::default();
|
||||
let now = Instant::now();
|
||||
tracker.expect(0x42, &[0x42], now).unwrap();
|
||||
tracker.expect(0x43, &[0x43], now).unwrap();
|
||||
tracker.acknowledge(0x43);
|
||||
tracker.acknowledge(0x42);
|
||||
tracker.wait_for_all(&[0x42, 0x43], Duration::ZERO).unwrap();
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn rejects_a_missing_command_acknowledgement() {
|
||||
let tracker = CommandAckTracker::default();
|
||||
tracker.expect(0x42, &[0x42], Instant::now()).unwrap();
|
||||
let error = tracker.wait_for_all(&[0x42], Duration::ZERO).unwrap_err();
|
||||
assert!(error.to_string().contains("42"));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn retries_an_unacknowledged_command_twice() {
|
||||
let tracker = CommandAckTracker::default();
|
||||
let start = Instant::now();
|
||||
tracker.expect(0x42, &[0xaa, 0x42], start).unwrap();
|
||||
assert!(tracker.take_due_retries(start).unwrap().is_empty());
|
||||
assert_eq!(
|
||||
tracker.take_due_retries(start + ACK_TIMEOUT).unwrap(),
|
||||
vec![(0x42, vec![0xaa, 0x42], 2)]
|
||||
);
|
||||
assert_eq!(
|
||||
tracker.take_due_retries(start + ACK_TIMEOUT * 2).unwrap(),
|
||||
vec![(0x42, vec![0xaa, 0x42], 3)]
|
||||
);
|
||||
assert!(
|
||||
tracker
|
||||
.take_due_retries(start + ACK_TIMEOUT * 3)
|
||||
.unwrap()
|
||||
.is_empty()
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn deduplicates_only_exact_recent_ack_requests() {
|
||||
let mut tracker = RecentAckRequests::default();
|
||||
assert!(!tracker.is_duplicate(10, 0x44, &[0x20, 0x02, 0x01, 0x00]));
|
||||
assert!(tracker.is_duplicate(12, 0x44, &[0x20, 0x02, 0x01, 0x00]));
|
||||
assert!(!tracker.is_duplicate(13, 0x44, &[0x24, 0x02, 0x01, 0x00]));
|
||||
assert!(!tracker.is_duplicate(140, 0x44, &[0x20, 0x02, 0x01, 0x00]));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn requires_the_complete_startup_snapshot_shape() {
|
||||
let frame = |payload| Frame {
|
||||
raw: Vec::new(),
|
||||
payload,
|
||||
escape_overhead: 0,
|
||||
message: 0x0b,
|
||||
command: 0x08,
|
||||
crc: 0,
|
||||
crc_valid: true,
|
||||
is_log: false,
|
||||
};
|
||||
let complete = frame(vec![
|
||||
0x20, 2, 1, 0, 0x08, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x24, 2, 0, 0, 0x0f, 2, 0, 0,
|
||||
]);
|
||||
assert!(contains_sensor_snapshot(&complete));
|
||||
|
||||
let incomplete = frame(vec![
|
||||
0x20, 2, 1, 0, 0x08, 10, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x24, 2, 0, 0,
|
||||
]);
|
||||
assert!(!contains_sensor_snapshot(&incomplete));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn ordered_stop_set_attempts_every_actuator_and_advances_sequence() {
|
||||
let (records, next_sequence) = ordered_stop_frames(0xfd);
|
||||
assert_eq!(next_sequence, 0x03);
|
||||
assert_eq!(records[0].0, "forced zero-wheel stop");
|
||||
assert_eq!(
|
||||
records[0].1.as_ref().unwrap(),
|
||||
&wheel_command_frame(0xfd, 0.0, 0.0, true).unwrap()
|
||||
);
|
||||
assert_eq!(
|
||||
records[1].1.as_ref().unwrap(),
|
||||
&fan_command_frame(0xfe, 0).unwrap()
|
||||
);
|
||||
assert_eq!(
|
||||
records[2].1.as_ref().unwrap(),
|
||||
&side_brush_command_frame(0xff, 0, false).unwrap()
|
||||
);
|
||||
assert_eq!(
|
||||
records[3].1.as_ref().unwrap(),
|
||||
&main_brush_command_frame(0x01, 0, false).unwrap()
|
||||
);
|
||||
assert_eq!(
|
||||
records[4].1.as_ref().unwrap(),
|
||||
&water_pump_command_frame(0x02, 0, 0).unwrap()
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn exhausts_three_attempts_when_an_ack_is_lost() {
|
||||
let start = Instant::now();
|
||||
let mut retry = AckRetryWindow::after_initial_send(start);
|
||||
assert!(!retry.take_retry(start + ACK_TIMEOUT - std::time::Duration::from_millis(1)));
|
||||
assert!(retry.take_retry(start + ACK_TIMEOUT));
|
||||
assert!(retry.take_retry(start + ACK_TIMEOUT * 2));
|
||||
assert!(!retry.exhausted(start + ACK_TIMEOUT * 3 - std::time::Duration::from_millis(1)));
|
||||
assert!(retry.exhausted(start + ACK_TIMEOUT * 3));
|
||||
assert!(!retry.take_retry(start + ACK_TIMEOUT * 4));
|
||||
assert_eq!(retry.attempts, 3);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn startup_shutdown_notification_is_delivered_only_once() {
|
||||
let (sender, receiver) = bounded(1);
|
||||
let mut startup = Some(sender);
|
||||
notify_startup(&mut startup, Ok(()));
|
||||
notify_startup(&mut startup, Ok(()));
|
||||
assert!(receiver.recv().unwrap().is_ok());
|
||||
assert!(matches!(
|
||||
receiver.try_recv(),
|
||||
Err(TryRecvError::Disconnected)
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,546 @@
|
||||
//! Native Roborock S5 Max hardware driver.
|
||||
//!
|
||||
//! The driver exclusively owns the MCU and LDS UARTs, LDS motor controller,
|
||||
//! and hardware watchdog. Actuation is gated by configuration, fresh safety
|
||||
//! telemetry, bounded command leases, and ordered shutdown cleanup.
|
||||
|
||||
pub mod commands;
|
||||
pub mod frame;
|
||||
pub mod lds;
|
||||
mod lds_motor;
|
||||
mod lds_reader;
|
||||
mod lifecycle;
|
||||
mod mcu;
|
||||
pub mod packet;
|
||||
mod safety;
|
||||
mod sensors;
|
||||
mod sys;
|
||||
mod tty;
|
||||
mod watchdog;
|
||||
|
||||
use crate::config::DeviceConfig;
|
||||
use crate::core::driver::{DeviceDriver, DriverInitResult};
|
||||
use crate::core::types::{Command, ComponentAction, SensorValue};
|
||||
use crate::error::{Error, Result};
|
||||
use commands::OPERATIONAL_SUBSYSTEMS;
|
||||
use lds_motor::LdsMotorGuard;
|
||||
use lds_reader::LdsReader;
|
||||
use lifecycle::{DriverState, Lifecycle};
|
||||
use mcu::{ActuatorCommand, McuDriver};
|
||||
use safety::SafetyController;
|
||||
use sensors::SensorGroups;
|
||||
use std::sync::Arc;
|
||||
use std::sync::atomic::{AtomicBool, AtomicU16, Ordering};
|
||||
use std::thread;
|
||||
use std::time::{Duration, Instant};
|
||||
use tty::ensure_devices_unowned;
|
||||
use watchdog::HardwareWatchdog;
|
||||
|
||||
/// Read-oriented production driver for the Roborock S5 Max.
|
||||
pub struct RoborockS5MaxDriver {
|
||||
config: DeviceConfig,
|
||||
shutdown: Arc<AtomicBool>,
|
||||
mcu: Option<McuDriver>,
|
||||
lds: Option<LdsReader>,
|
||||
lds_motor: Option<LdsMotorGuard>,
|
||||
watchdog: Option<HardwareWatchdog>,
|
||||
lifecycle: Lifecycle,
|
||||
safety: SafetyController,
|
||||
dock_state: Option<Arc<AtomicU16>>,
|
||||
restore_charging_on_shutdown: bool,
|
||||
initialized: bool,
|
||||
}
|
||||
|
||||
impl RoborockS5MaxDriver {
|
||||
pub fn new(config: DeviceConfig) -> Result<Self> {
|
||||
if config.roborock_s5max.is_none() {
|
||||
return Err(Error::Config(
|
||||
"roborock_s5max device requires [device.roborock_s5max] section".to_string(),
|
||||
));
|
||||
}
|
||||
let lifecycle = Lifecycle::new();
|
||||
let safety = SafetyController::new(lifecycle.clone());
|
||||
Ok(Self {
|
||||
config,
|
||||
shutdown: Arc::new(AtomicBool::new(false)),
|
||||
mcu: None,
|
||||
lds: None,
|
||||
lds_motor: None,
|
||||
watchdog: None,
|
||||
lifecycle,
|
||||
safety,
|
||||
dock_state: None,
|
||||
restore_charging_on_shutdown: false,
|
||||
initialized: false,
|
||||
})
|
||||
}
|
||||
|
||||
fn shutdown_all(&mut self) -> Result<()> {
|
||||
let faulted = self.lifecycle.state() == DriverState::Fault;
|
||||
let mut first_error = None;
|
||||
|
||||
if self.initialized {
|
||||
if let Some(mcu) = self.mcu.as_ref()
|
||||
&& let Err(error) = mcu.execute(ActuatorCommand::EmergencyStop)
|
||||
{
|
||||
first_error.get_or_insert(error);
|
||||
}
|
||||
if let Some(motor) = self.lds_motor.as_ref()
|
||||
&& let Err(error) = motor.stop()
|
||||
{
|
||||
first_error.get_or_insert(error);
|
||||
}
|
||||
self.safety.set_lds_running(false).ok();
|
||||
if self.restore_charging_on_shutdown && !faulted {
|
||||
let restore_result = self
|
||||
.mcu
|
||||
.as_ref()
|
||||
.ok_or_else(|| Error::Other("S5 Max MCU is unavailable".to_string()))
|
||||
.and_then(|mcu| mcu.execute(ActuatorCommand::Charger(true)))
|
||||
.and_then(|()| self.wait_for_charging());
|
||||
if let Err(error) = restore_result {
|
||||
first_error.get_or_insert(error);
|
||||
} else {
|
||||
self.restore_charging_on_shutdown = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
if let Err(error) = self.lifecycle.begin_stopping() {
|
||||
self.lifecycle.fault(error.to_string());
|
||||
}
|
||||
self.shutdown.store(true, Ordering::Release);
|
||||
|
||||
if let Some(mut lds) = self.lds.take()
|
||||
&& let Err(error) = lds.join()
|
||||
{
|
||||
first_error.get_or_insert(error);
|
||||
}
|
||||
if let Some(mut mcu) = self.mcu.take()
|
||||
&& let Err(error) = mcu.join()
|
||||
{
|
||||
first_error.get_or_insert(error);
|
||||
}
|
||||
if let Some(mut lds_motor) = self.lds_motor.take()
|
||||
&& let Err(error) = lds_motor.stop_and_disarm()
|
||||
{
|
||||
first_error.get_or_insert(error);
|
||||
}
|
||||
if let Some(mut watchdog) = self.watchdog.take()
|
||||
&& let Err(error) = watchdog.join()
|
||||
{
|
||||
first_error.get_or_insert(error);
|
||||
}
|
||||
self.initialized = false;
|
||||
self.safety.reset_targets();
|
||||
|
||||
if let Some(error) = first_error {
|
||||
self.lifecycle.fault(format!("shutdown: {error}"));
|
||||
Err(error)
|
||||
} else {
|
||||
if !faulted {
|
||||
self.lifecycle.mark_stopped()?;
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
fn execute_actuator(&self, command: ActuatorCommand, nonzero: bool) -> Result<()> {
|
||||
let hardware = self.config.roborock_s5max.as_ref().ok_or_else(|| {
|
||||
Error::Config("missing Roborock S5 Max hardware configuration".to_string())
|
||||
})?;
|
||||
if nonzero {
|
||||
self.safety
|
||||
.nonzero_allowed(hardware.allow_actuation, std::time::Instant::now())?;
|
||||
}
|
||||
self.mcu
|
||||
.as_ref()
|
||||
.ok_or_else(|| Error::Other("S5 Max MCU is not initialized".to_string()))?
|
||||
.execute(command)
|
||||
}
|
||||
|
||||
fn speed(action: &ComponentAction, default: u8) -> Result<u8> {
|
||||
match action {
|
||||
ComponentAction::Disable { .. } => Ok(0),
|
||||
ComponentAction::Enable { config } => config
|
||||
.as_ref()
|
||||
.and_then(|values| values.get("speed"))
|
||||
.map(Self::speed_value)
|
||||
.transpose()
|
||||
.map(|value| value.unwrap_or(default)),
|
||||
ComponentAction::Configure { config } => config
|
||||
.get("speed")
|
||||
.ok_or_else(|| Error::InvalidParameter("speed is required".to_string()))
|
||||
.and_then(Self::speed_value),
|
||||
ComponentAction::Reset { .. } => Err(Error::InvalidParameter(
|
||||
"Reset is only supported for drive emergency stop".to_string(),
|
||||
)),
|
||||
}
|
||||
}
|
||||
|
||||
fn speed_value(value: &SensorValue) -> Result<u8> {
|
||||
let speed = match value {
|
||||
SensorValue::U8(value) => u32::from(*value),
|
||||
SensorValue::U16(value) => u32::from(*value),
|
||||
SensorValue::U32(value) => *value,
|
||||
_ => {
|
||||
return Err(Error::InvalidParameter(
|
||||
"speed must be an unsigned integer from 0 to 100".to_string(),
|
||||
));
|
||||
}
|
||||
};
|
||||
u8::try_from(speed)
|
||||
.ok()
|
||||
.filter(|speed| *speed <= 100)
|
||||
.ok_or_else(|| Error::InvalidParameter("speed must be from 0 to 100".to_string()))
|
||||
}
|
||||
|
||||
fn drive_value(
|
||||
config: &std::collections::HashMap<String, SensorValue>,
|
||||
key: &str,
|
||||
) -> Result<f32> {
|
||||
match config.get(key) {
|
||||
Some(SensorValue::F32(value)) => Ok(*value),
|
||||
Some(SensorValue::F64(value)) => Ok(*value as f32),
|
||||
_ => Err(Error::InvalidParameter(format!(
|
||||
"drive Configure requires numeric {key}"
|
||||
))),
|
||||
}
|
||||
}
|
||||
|
||||
fn wait_for_charging(&self) -> Result<()> {
|
||||
let dock_state = self
|
||||
.dock_state
|
||||
.as_ref()
|
||||
.ok_or_else(|| Error::Other("S5 Max dock state is unavailable".to_string()))?;
|
||||
let deadline = Instant::now() + Duration::from_secs(2);
|
||||
while Instant::now() < deadline {
|
||||
let state = dock_state.load(Ordering::Acquire);
|
||||
if state == 1 {
|
||||
return Ok(());
|
||||
}
|
||||
thread::sleep(Duration::from_millis(20));
|
||||
}
|
||||
Err(Error::Other(format!(
|
||||
"S5 Max charging transition was not confirmed; dock_state={}",
|
||||
dock_state.load(Ordering::Acquire)
|
||||
)))
|
||||
}
|
||||
|
||||
fn set_charging(&mut self, enabled: bool) -> Result<()> {
|
||||
let was_docked = self
|
||||
.dock_state
|
||||
.as_ref()
|
||||
.is_some_and(|state| state.load(Ordering::Acquire) != 0);
|
||||
self.execute_actuator(ActuatorCommand::Charger(enabled), true)?;
|
||||
if !enabled {
|
||||
// b7=0 controls the charger, not physical dock contact. On the
|
||||
// dock the MCU may report a very short non-charging transition
|
||||
// and then return to state 1 while the contacts remain engaged.
|
||||
// The acknowledged command is therefore the only synchronous
|
||||
// completion condition for disable.
|
||||
self.restore_charging_on_shutdown |= was_docked;
|
||||
return Ok(());
|
||||
}
|
||||
self.wait_for_charging()?;
|
||||
self.restore_charging_on_shutdown = false;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn set_lidar(&mut self, enabled: bool) -> Result<()> {
|
||||
if !enabled {
|
||||
let stop_result = self.lds_motor.as_ref().map_or(Ok(()), LdsMotorGuard::stop);
|
||||
let safety_result = self.safety.set_lds_running(false);
|
||||
return stop_result.and(safety_result);
|
||||
}
|
||||
|
||||
let hardware = self.config.roborock_s5max.as_ref().unwrap();
|
||||
self.safety
|
||||
.nonzero_allowed(hardware.allow_actuation, Instant::now())?;
|
||||
if self
|
||||
.lds_motor
|
||||
.as_ref()
|
||||
.is_some_and(LdsMotorGuard::is_running)
|
||||
{
|
||||
return Ok(());
|
||||
}
|
||||
let dock_state = self
|
||||
.dock_state
|
||||
.as_ref()
|
||||
.map_or(u16::MAX, |state| state.load(Ordering::Acquire));
|
||||
if dock_state != 0 || self.restore_charging_on_shutdown {
|
||||
return Err(Error::Other(format!(
|
||||
"S5 Max LDS start requires physical undock; dock_state={dock_state}"
|
||||
)));
|
||||
}
|
||||
// A permanent replacement can take ownership before the stock stack
|
||||
// has converted an undocked boot into the normal/discharge state. MCU
|
||||
// telemetry remains available there, but the LDS power domain does not
|
||||
// start. Reproduce the stock b7=0 discharge transition followed by its
|
||||
// b0 `sys_md\0\x00` wake before touching the motor controller. The b7
|
||||
// command is acknowledged synchronously; incoming mode confirmation is
|
||||
// handled and acknowledged by the MCU session worker.
|
||||
self.execute_actuator(ActuatorCommand::Charger(false), false)?;
|
||||
self.execute_actuator(ActuatorCommand::WakeMcu, false)?;
|
||||
self.execute_actuator(ActuatorCommand::StopDrive, false)?;
|
||||
// Submit each acknowledged transition separately so the MCU worker can
|
||||
// consume its ACK before the next subsystem record is sent.
|
||||
for &subsystem in OPERATIONAL_SUBSYSTEMS {
|
||||
self.execute_actuator(ActuatorCommand::EnableSubsystem(subsystem), false)?;
|
||||
}
|
||||
// Stock waits for the acknowledged operational subsystem transition
|
||||
// before its laser worker starts the separate Linux motor controller.
|
||||
thread::sleep(Duration::from_millis(250));
|
||||
let start_result = self
|
||||
.lds_motor
|
||||
.as_mut()
|
||||
.ok_or_else(|| Error::Other("S5 Max LDS motor is unavailable".to_string()))?
|
||||
.configure_and_start();
|
||||
if let Err(error) = start_result {
|
||||
if let Some(motor) = self.lds_motor.as_ref() {
|
||||
let _ = motor.stop();
|
||||
}
|
||||
return Err(error);
|
||||
}
|
||||
if let Err(error) = self.safety.set_lds_running(true) {
|
||||
let _ = self.set_lidar(false);
|
||||
return Err(error);
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn component_control(&mut self, id: &str, action: &ComponentAction) -> Result<()> {
|
||||
match id {
|
||||
"drive" => match action {
|
||||
ComponentAction::Disable { .. } => {
|
||||
self.execute_actuator(ActuatorCommand::StopDrive, false)
|
||||
}
|
||||
ComponentAction::Reset { .. } => {
|
||||
self.execute_actuator(ActuatorCommand::EmergencyStop, false)
|
||||
}
|
||||
ComponentAction::Configure { config } => {
|
||||
let hardware = self.config.roborock_s5max.as_ref().unwrap();
|
||||
let linear = Self::drive_value(config, "linear")?;
|
||||
let angular = Self::drive_value(config, "angular")?;
|
||||
if !linear.is_finite()
|
||||
|| !angular.is_finite()
|
||||
|| linear.abs() > 0.30
|
||||
|| angular.abs() > 1.60
|
||||
{
|
||||
return Err(Error::InvalidParameter(
|
||||
"drive exceeds S5 Max limits (0.30 m/s, 1.60 rad/s)".to_string(),
|
||||
));
|
||||
}
|
||||
// Stock c0 captures use encoder ticks/20 ms for the linear
|
||||
// field and rad/s for yaw: a commanded pi/2 turn settles
|
||||
// near 1.57 on the MCU's yaw-rate report.
|
||||
let ticks_per_20ms = linear * 20.0 / hardware.wheel_mm_per_tick;
|
||||
self.execute_actuator(
|
||||
ActuatorCommand::Drive {
|
||||
linear: ticks_per_20ms,
|
||||
angular,
|
||||
},
|
||||
linear != 0.0 || angular != 0.0,
|
||||
)
|
||||
}
|
||||
ComponentAction::Enable { .. } => Ok(()),
|
||||
},
|
||||
"vacuum" => {
|
||||
let target = Self::speed(action, 100)?;
|
||||
self.execute_actuator(ActuatorCommand::Fan(target), target != 0)
|
||||
}
|
||||
"main_brush" => {
|
||||
let target = Self::speed(action, 71)?;
|
||||
self.execute_actuator(ActuatorCommand::MainBrush(target), target != 0)
|
||||
}
|
||||
"side_brush" => {
|
||||
let target = Self::speed(action, 30)?;
|
||||
self.execute_actuator(ActuatorCommand::SideBrush(target), target != 0)
|
||||
}
|
||||
"water_pump" => {
|
||||
let target = Self::speed(action, 100)?;
|
||||
if target != 0 {
|
||||
return Err(Error::NotImplemented(
|
||||
"S5 Max water-pump duty scheduling is not implemented".to_string(),
|
||||
));
|
||||
}
|
||||
self.execute_actuator(
|
||||
ActuatorCommand::WaterPump {
|
||||
on_interval: 0,
|
||||
off_interval: 0xff,
|
||||
},
|
||||
false,
|
||||
)
|
||||
}
|
||||
"lidar" => match action {
|
||||
ComponentAction::Enable { .. } => self.set_lidar(true),
|
||||
ComponentAction::Disable { .. } => self.set_lidar(false),
|
||||
_ => Err(Error::InvalidParameter(
|
||||
"S5 Max lidar supports only Enable and Disable".to_string(),
|
||||
)),
|
||||
},
|
||||
"charger" => match action {
|
||||
ComponentAction::Enable { .. } => self.set_charging(true),
|
||||
ComponentAction::Disable { .. } => self.set_charging(false),
|
||||
_ => Err(Error::InvalidParameter(
|
||||
"S5 Max charger supports only Enable and Disable".to_string(),
|
||||
)),
|
||||
},
|
||||
_ => Err(Error::InvalidParameter(format!(
|
||||
"unsupported S5 Max component {id:?}"
|
||||
))),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl DeviceDriver for RoborockS5MaxDriver {
|
||||
fn initialize(&mut self) -> Result<DriverInitResult> {
|
||||
if self.initialized {
|
||||
return Err(Error::Other(
|
||||
"Roborock S5 Max driver is already initialized".to_string(),
|
||||
));
|
||||
}
|
||||
let hardware = self.config.roborock_s5max.clone().ok_or_else(|| {
|
||||
Error::Config(
|
||||
"roborock_s5max device requires [device.roborock_s5max] section".to_string(),
|
||||
)
|
||||
})?;
|
||||
let last_stock_sequence = hardware.last_stock_sequence.ok_or_else(|| {
|
||||
Error::Config(
|
||||
"device.roborock_s5max.last_stock_sequence is required at runtime; use 0 only for ownership from MCU reset"
|
||||
.to_string(),
|
||||
)
|
||||
})?;
|
||||
|
||||
let selected_devices = [
|
||||
hardware.mcu_port.as_str(),
|
||||
"/dev/uart_mcu",
|
||||
hardware.lds_port.as_str(),
|
||||
"/dev/uart_lds",
|
||||
hardware.lds_motor.as_str(),
|
||||
hardware.watchdog.as_str(),
|
||||
];
|
||||
ensure_devices_unowned(&selected_devices)?;
|
||||
self.lifecycle.begin_synchronizing()?;
|
||||
|
||||
self.shutdown.store(false, Ordering::Release);
|
||||
let (sensors, mut result) = SensorGroups::create(
|
||||
hardware.wheel_mm_per_tick,
|
||||
hardware.wheel_track_m,
|
||||
hardware.lds_forward_angle_deg,
|
||||
);
|
||||
self.dock_state = Some(sensors.dock_state());
|
||||
result
|
||||
.sensor_data
|
||||
.insert("driver_status".to_string(), self.lifecycle.sensor_data());
|
||||
|
||||
let watchdog = match HardwareWatchdog::start(
|
||||
&hardware.watchdog,
|
||||
self.lifecycle.clone(),
|
||||
Arc::clone(&self.shutdown),
|
||||
) {
|
||||
Ok(watchdog) => watchdog,
|
||||
Err(error) => {
|
||||
self.lifecycle.fault(format!("watchdog startup: {error}"));
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
};
|
||||
self.watchdog = Some(watchdog);
|
||||
|
||||
let lds_motor = match LdsMotorGuard::open(&hardware.lds_motor) {
|
||||
Ok(motor) => motor,
|
||||
Err(error) => {
|
||||
self.lifecycle
|
||||
.fault(format!("LDS motor ownership: {error}"));
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
};
|
||||
self.lds_motor = Some(lds_motor);
|
||||
|
||||
let mcu = match McuDriver::start(
|
||||
&hardware.mcu_port,
|
||||
last_stock_sequence,
|
||||
sensors.clone(),
|
||||
self.lifecycle.clone(),
|
||||
self.safety.clone(),
|
||||
Arc::clone(&self.shutdown),
|
||||
) {
|
||||
Ok(mcu) => mcu,
|
||||
Err(error) => {
|
||||
self.lifecycle.fault(format!("MCU startup: {error}"));
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
};
|
||||
self.mcu = Some(mcu);
|
||||
if let Err(error) = self.lifecycle.mark_read_only_ready() {
|
||||
self.lifecycle.fault(error.to_string());
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
|
||||
let feedback_motor = match self
|
||||
.lds_motor
|
||||
.as_ref()
|
||||
.expect("LDS motor guard initialized above")
|
||||
.feedback_file()
|
||||
{
|
||||
Ok(file) => file,
|
||||
Err(error) => {
|
||||
self.lifecycle
|
||||
.fault(format!("LDS feedback ownership: {error}"));
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
};
|
||||
let motor_running = self
|
||||
.lds_motor
|
||||
.as_ref()
|
||||
.expect("LDS motor guard initialized above")
|
||||
.running_flag();
|
||||
let lds = match LdsReader::start(
|
||||
&hardware.lds_port,
|
||||
hardware.lds_forward_angle_deg,
|
||||
Arc::clone(&sensors.lidar),
|
||||
self.lifecycle.clone(),
|
||||
Arc::clone(&self.shutdown),
|
||||
feedback_motor,
|
||||
motor_running,
|
||||
) {
|
||||
Ok(lds) => lds,
|
||||
Err(error) => {
|
||||
self.lifecycle.fault(format!("LDS startup: {error}"));
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
};
|
||||
self.lds = Some(lds);
|
||||
if let Err(error) = self.lifecycle.mark_operational() {
|
||||
self.lifecycle.fault(error.to_string());
|
||||
let _ = self.shutdown_all();
|
||||
return Err(error);
|
||||
}
|
||||
self.initialized = true;
|
||||
log::info!("Roborock S5 Max driver initialized: {}", self.config.name);
|
||||
Ok(result)
|
||||
}
|
||||
|
||||
fn send_command(&mut self, cmd: Command) -> Result<()> {
|
||||
match cmd {
|
||||
Command::Shutdown => self.shutdown_all(),
|
||||
Command::ComponentControl { id, action } => self.component_control(&id, &action),
|
||||
_ => Err(Error::NotImplemented(
|
||||
"unsupported Roborock S5 Max command".to_string(),
|
||||
)),
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for RoborockS5MaxDriver {
|
||||
fn drop(&mut self) {
|
||||
if let Err(error) = self.shutdown_all() {
|
||||
log::error!("Roborock S5 Max driver shutdown failed: {error}");
|
||||
}
|
||||
}
|
||||
}
|
||||
File diff suppressed because it is too large
Load Diff
@@ -0,0 +1,211 @@
|
||||
//! Actuator target state, freshness gate, and drive deadman lease.
|
||||
|
||||
use super::lifecycle::{DriverState, Lifecycle};
|
||||
use crate::error::{Error, Result};
|
||||
use std::sync::{Arc, Mutex};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
pub(crate) const DRIVE_LEASE_DURATION: Duration = Duration::from_millis(250);
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||||
pub(crate) struct ActuatorTargets {
|
||||
pub linear: f32,
|
||||
pub angular: f32,
|
||||
pub fan: u8,
|
||||
pub main_brush: u8,
|
||||
pub side_brush: u8,
|
||||
pub water_pump: u8,
|
||||
pub lds_running: bool,
|
||||
}
|
||||
|
||||
impl Default for ActuatorTargets {
|
||||
fn default() -> Self {
|
||||
Self {
|
||||
linear: 0.0,
|
||||
angular: 0.0,
|
||||
fan: 0,
|
||||
main_brush: 0,
|
||||
side_brush: 0,
|
||||
water_pump: 0,
|
||||
lds_running: false,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
struct SafetyInner {
|
||||
targets: ActuatorTargets,
|
||||
drive_lease_deadline: Option<Instant>,
|
||||
}
|
||||
|
||||
#[derive(Clone)]
|
||||
pub(crate) struct SafetyController {
|
||||
lifecycle: Lifecycle,
|
||||
inner: Arc<Mutex<SafetyInner>>,
|
||||
}
|
||||
|
||||
impl SafetyController {
|
||||
pub(crate) fn new(lifecycle: Lifecycle) -> Self {
|
||||
Self {
|
||||
lifecycle,
|
||||
inner: Arc::new(Mutex::new(SafetyInner {
|
||||
targets: ActuatorTargets::default(),
|
||||
drive_lease_deadline: None,
|
||||
})),
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn reset_targets(&self) {
|
||||
if let Ok(mut inner) = self.inner.lock() {
|
||||
inner.targets = ActuatorTargets::default();
|
||||
inner.drive_lease_deadline = None;
|
||||
} else {
|
||||
self.lifecycle
|
||||
.fault("safety mutex poisoned while resetting actuator targets");
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn nonzero_allowed(&self, allow_actuation: bool, now: Instant) -> Result<()> {
|
||||
if !allow_actuation {
|
||||
return Err(Error::InvalidParameter(
|
||||
"S5 Max actuation is disabled by configuration".to_string(),
|
||||
));
|
||||
}
|
||||
if self.lifecycle.state() != DriverState::Operational {
|
||||
return Err(Error::Other(
|
||||
"S5 Max is not in the operational lifecycle state".to_string(),
|
||||
));
|
||||
}
|
||||
if !self.lifecycle.safety_reports_fresh(now) {
|
||||
return Err(Error::Other(
|
||||
"S5 Max safety telemetry is stale; nonzero target rejected".to_string(),
|
||||
));
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn arm_drive_lease(&self, linear: f32, angular: f32, now: Instant) -> Result<()> {
|
||||
let mut inner = self
|
||||
.inner
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max safety".to_string()))?;
|
||||
inner.targets.linear = linear;
|
||||
inner.targets.angular = angular;
|
||||
inner.drive_lease_deadline = if linear == 0.0 && angular == 0.0 {
|
||||
None
|
||||
} else {
|
||||
Some(now + DRIVE_LEASE_DURATION)
|
||||
};
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn clear_drive_target(&self) -> Result<()> {
|
||||
let mut inner = self
|
||||
.inner
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max safety".to_string()))?;
|
||||
inner.targets.linear = 0.0;
|
||||
inner.targets.angular = 0.0;
|
||||
inner.drive_lease_deadline = None;
|
||||
Ok(())
|
||||
}
|
||||
|
||||
fn set_target(&self, update: impl FnOnce(&mut ActuatorTargets)) -> Result<()> {
|
||||
let mut inner = self
|
||||
.inner
|
||||
.lock()
|
||||
.map_err(|_| Error::MutexPoisoned("S5 Max safety".to_string()))?;
|
||||
update(&mut inner.targets);
|
||||
Ok(())
|
||||
}
|
||||
|
||||
pub(crate) fn set_fan(&self, target: u8) -> Result<()> {
|
||||
self.set_target(|targets| targets.fan = target)
|
||||
}
|
||||
|
||||
pub(crate) fn set_main_brush(&self, target: u8) -> Result<()> {
|
||||
self.set_target(|targets| targets.main_brush = target)
|
||||
}
|
||||
|
||||
pub(crate) fn set_side_brush(&self, target: u8) -> Result<()> {
|
||||
self.set_target(|targets| targets.side_brush = target)
|
||||
}
|
||||
|
||||
pub(crate) fn set_water_pump(&self, target: u8) -> Result<()> {
|
||||
self.set_target(|targets| targets.water_pump = target)
|
||||
}
|
||||
|
||||
pub(crate) fn set_lds_running(&self, running: bool) -> Result<()> {
|
||||
self.set_target(|targets| targets.lds_running = running)
|
||||
}
|
||||
|
||||
pub(crate) fn take_expired_drive_lease(&self, now: Instant) -> bool {
|
||||
let Ok(mut inner) = self.inner.lock() else {
|
||||
self.lifecycle
|
||||
.fault("safety mutex poisoned while checking drive lease");
|
||||
return true;
|
||||
};
|
||||
if inner
|
||||
.drive_lease_deadline
|
||||
.is_some_and(|deadline| now >= deadline)
|
||||
{
|
||||
inner.targets.linear = 0.0;
|
||||
inner.targets.angular = 0.0;
|
||||
inner.drive_lease_deadline = None;
|
||||
return true;
|
||||
}
|
||||
false
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
fn targets(&self) -> ActuatorTargets {
|
||||
self.inner.lock().unwrap().targets
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::{ActuatorTargets, DRIVE_LEASE_DURATION, SafetyController};
|
||||
use crate::devices::roborock_s5max::lifecycle::Lifecycle;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
#[test]
|
||||
fn targets_start_and_reset_to_zero() {
|
||||
let lifecycle = Lifecycle::new();
|
||||
let safety = SafetyController::new(lifecycle);
|
||||
assert_eq!(safety.targets(), ActuatorTargets::default());
|
||||
let now = Instant::now();
|
||||
safety.arm_drive_lease(0.1, -0.2, now).unwrap();
|
||||
assert!(
|
||||
!safety.take_expired_drive_lease(now + DRIVE_LEASE_DURATION - Duration::from_millis(1))
|
||||
);
|
||||
assert!(safety.take_expired_drive_lease(now + DRIVE_LEASE_DURATION));
|
||||
assert_eq!(safety.targets(), ActuatorTargets::default());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn zero_drive_target_does_not_arm_deadman() {
|
||||
let lifecycle = Lifecycle::new();
|
||||
let safety = SafetyController::new(lifecycle);
|
||||
let now = Instant::now();
|
||||
|
||||
safety.arm_drive_lease(0.0, 0.0, now).unwrap();
|
||||
|
||||
assert!(!safety.take_expired_drive_lease(now + DRIVE_LEASE_DURATION));
|
||||
assert_eq!(safety.targets(), ActuatorTargets::default());
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn nonzero_gate_requires_config_state_and_fresh_telemetry() {
|
||||
let lifecycle = Lifecycle::new();
|
||||
let safety = SafetyController::new(lifecycle.clone());
|
||||
let now = Instant::now();
|
||||
assert!(safety.nonzero_allowed(false, now).is_err());
|
||||
assert!(safety.nonzero_allowed(true, now).is_err());
|
||||
lifecycle.begin_synchronizing().unwrap();
|
||||
lifecycle.mark_read_only_ready().unwrap();
|
||||
lifecycle.mark_operational().unwrap();
|
||||
assert!(safety.nonzero_allowed(true, now).is_err());
|
||||
lifecycle.mark_safety_report(now);
|
||||
assert!(safety.nonzero_allowed(true, now).is_ok());
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,655 @@
|
||||
//! Translation from S5 Max wire reports to VacuumTiger sensor groups.
|
||||
|
||||
use super::packet::{BRUSH_REPORT_MESSAGE, FAN_REPORT_MESSAGE, Frame, SWEEP_REPORT_MESSAGE};
|
||||
use crate::core::driver::DriverInitResult;
|
||||
use crate::core::types::{SensorGroupData, SensorValue, StreamSender, create_stream_channel};
|
||||
use std::collections::HashMap;
|
||||
use std::sync::atomic::{AtomicU16, Ordering};
|
||||
use std::sync::{Arc, Mutex};
|
||||
|
||||
const KEY_REPORT: u8 = 0x06;
|
||||
const PUMP_ERROR_REPORT: u8 = 0x18;
|
||||
const DOCK_REPORT: u8 = 0x20;
|
||||
const BUMPER_REPORT: u8 = 0x21;
|
||||
const DROP_REPORT: u8 = 0x22;
|
||||
const CLIFF_REPORT: u8 = 0x23;
|
||||
const DUSTBIN_REPORT: u8 = 0x24;
|
||||
const LDS_COVER_REPORT: u8 = 0x25;
|
||||
const WATER_BOX_REPORT: u8 = 0x26;
|
||||
const MCU_VERSION_REPORT: u8 = 0x01;
|
||||
|
||||
fn group(name: &str) -> Arc<Mutex<SensorGroupData>> {
|
||||
Arc::new(Mutex::new(SensorGroupData::new(name)))
|
||||
}
|
||||
|
||||
fn little_u16(bytes: &[u8]) -> Option<u16> {
|
||||
Some(u16::from_le_bytes(bytes.get(..2)?.try_into().ok()?))
|
||||
}
|
||||
|
||||
fn radians_per_second_to_milliradians(value: f32) -> i16 {
|
||||
(value * 1000.0)
|
||||
.round()
|
||||
.clamp(i16::MIN as f32, i16::MAX as f32) as i16
|
||||
}
|
||||
|
||||
fn update_group(
|
||||
group: &Arc<Mutex<SensorGroupData>>,
|
||||
update: impl FnOnce(&mut SensorGroupData),
|
||||
) -> Option<SensorGroupData> {
|
||||
let mut data = match group.lock() {
|
||||
Ok(data) => data,
|
||||
Err(_) => {
|
||||
log::error!("S5 Max sensor-group mutex poisoned");
|
||||
return None;
|
||||
}
|
||||
};
|
||||
update(&mut data);
|
||||
data.touch();
|
||||
Some(data.clone())
|
||||
}
|
||||
|
||||
#[derive(Clone)]
|
||||
pub(crate) struct SensorGroups {
|
||||
pub sensor_status: Arc<Mutex<SensorGroupData>>,
|
||||
pub power_status: Arc<Mutex<SensorGroupData>>,
|
||||
pub attachments: Arc<Mutex<SensorGroupData>>,
|
||||
pub motor_status: Arc<Mutex<SensorGroupData>>,
|
||||
pub errors: Arc<Mutex<SensorGroupData>>,
|
||||
pub dock_ir: Arc<Mutex<SensorGroupData>>,
|
||||
pub device_version: Arc<Mutex<SensorGroupData>>,
|
||||
pub lidar: Arc<Mutex<SensorGroupData>>,
|
||||
dock_state: Arc<AtomicU16>,
|
||||
stream_tx: StreamSender,
|
||||
wheel_metres_per_tick: f32,
|
||||
}
|
||||
|
||||
impl SensorGroups {
|
||||
pub(crate) fn create(
|
||||
wheel_mm_per_tick: f32,
|
||||
wheel_track_m: f32,
|
||||
lds_forward_angle_deg: f32,
|
||||
) -> (Self, DriverInitResult) {
|
||||
let sensor_status = group("sensor_status");
|
||||
let power_status = group("power_status");
|
||||
let attachments = group("attachments");
|
||||
let motor_status = group("motor_status");
|
||||
let errors = group("errors");
|
||||
let dock_ir = group("dock_ir");
|
||||
let device_version = group("device_version");
|
||||
let lidar = group("lidar");
|
||||
let dock_state = Arc::new(AtomicU16::new(u16::MAX));
|
||||
let (stream_tx, stream_rx) = create_stream_channel();
|
||||
|
||||
update_group(&device_version, |data| {
|
||||
data.set("wheel_mm_per_tick", SensorValue::F32(wheel_mm_per_tick));
|
||||
data.set("wheel_track_m", SensorValue::F32(wheel_track_m));
|
||||
data.set(
|
||||
"lds_forward_angle_deg",
|
||||
SensorValue::F32(lds_forward_angle_deg),
|
||||
);
|
||||
});
|
||||
|
||||
let sensor_data = [
|
||||
("sensor_status", Arc::clone(&sensor_status)),
|
||||
("power_status", Arc::clone(&power_status)),
|
||||
("attachments", Arc::clone(&attachments)),
|
||||
("motor_status", Arc::clone(&motor_status)),
|
||||
("errors", Arc::clone(&errors)),
|
||||
("dock_ir", Arc::clone(&dock_ir)),
|
||||
("device_version", Arc::clone(&device_version)),
|
||||
("lidar", Arc::clone(&lidar)),
|
||||
]
|
||||
.into_iter()
|
||||
.map(|(name, data)| (name.to_string(), data))
|
||||
.collect();
|
||||
let stream_receivers = HashMap::from([("sensor_status".to_string(), stream_rx)]);
|
||||
|
||||
(
|
||||
Self {
|
||||
sensor_status,
|
||||
power_status,
|
||||
attachments,
|
||||
motor_status,
|
||||
errors,
|
||||
dock_ir,
|
||||
device_version,
|
||||
lidar,
|
||||
dock_state,
|
||||
stream_tx,
|
||||
wheel_metres_per_tick: wheel_mm_per_tick / 1000.0,
|
||||
},
|
||||
DriverInitResult {
|
||||
sensor_data,
|
||||
stream_receivers,
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
pub(crate) fn process_mcu_frame(&self, frame: &Frame) {
|
||||
if let Some(state) = frame.state_report()
|
||||
&& let Some(snapshot) = update_group(&self.sensor_status, |data| {
|
||||
data.set("acceleration", SensorValue::Vector3(state.imu.acceleration));
|
||||
data.set(
|
||||
"angular_velocity",
|
||||
SensorValue::Vector3(state.imu.angular_rate),
|
||||
);
|
||||
// Common navigation clients consume signed milli-radians per
|
||||
// second and convert them back with a 0.001 scale.
|
||||
data.set(
|
||||
"gyro_x",
|
||||
SensorValue::I16(radians_per_second_to_milliradians(
|
||||
state.imu.angular_rate[0],
|
||||
)),
|
||||
);
|
||||
data.set(
|
||||
"gyro_y",
|
||||
SensorValue::I16(radians_per_second_to_milliradians(
|
||||
state.imu.angular_rate[1],
|
||||
)),
|
||||
);
|
||||
data.set(
|
||||
"gyro_z",
|
||||
SensorValue::I16(radians_per_second_to_milliradians(
|
||||
state.imu.angular_rate[2],
|
||||
)),
|
||||
);
|
||||
data.set("orientation_x", SensorValue::F32(state.imu.quaternion[0]));
|
||||
data.set("orientation_y", SensorValue::F32(state.imu.quaternion[1]));
|
||||
data.set("orientation_z", SensorValue::F32(state.imu.quaternion[2]));
|
||||
data.set("orientation_w", SensorValue::F32(state.imu.quaternion[3]));
|
||||
data.set(
|
||||
"wheel_left_ticks",
|
||||
SensorValue::I32(state.left_odometry_ticks),
|
||||
);
|
||||
data.set(
|
||||
"wheel_right_ticks",
|
||||
SensorValue::I32(state.right_odometry_ticks),
|
||||
);
|
||||
// Preserve the common CRL-200S sensor contract for existing
|
||||
// navigation clients. Taking the low 16 bits is intentional:
|
||||
// their wrapping subtraction preserves bounded tick deltas.
|
||||
data.set(
|
||||
"wheel_left",
|
||||
SensorValue::U16(state.left_odometry_ticks as u16),
|
||||
);
|
||||
data.set(
|
||||
"wheel_right",
|
||||
SensorValue::U16(state.right_odometry_ticks as u16),
|
||||
);
|
||||
data.set(
|
||||
"wheel_left_m",
|
||||
SensorValue::F32(state.left_odometry_ticks as f32 * self.wheel_metres_per_tick),
|
||||
);
|
||||
data.set(
|
||||
"wheel_right_m",
|
||||
SensorValue::F32(
|
||||
state.right_odometry_ticks as f32 * self.wheel_metres_per_tick,
|
||||
),
|
||||
);
|
||||
data.set("forward_motion_raw", SensorValue::F32(state.forward_motion));
|
||||
data.set("mcu_timestamp_ms", SensorValue::U64(state.mcu_timestamp_ms));
|
||||
})
|
||||
{
|
||||
let _ = self.stream_tx.try_send(snapshot);
|
||||
}
|
||||
|
||||
self.process_discrete_sensors(frame);
|
||||
self.process_power(frame);
|
||||
self.process_motors(frame);
|
||||
self.process_errors(frame);
|
||||
self.process_dock_ir(frame);
|
||||
self.process_version(frame);
|
||||
}
|
||||
|
||||
pub(crate) fn dock_state(&self) -> Arc<AtomicU16> {
|
||||
Arc::clone(&self.dock_state)
|
||||
}
|
||||
|
||||
fn process_discrete_sensors(&self, frame: &Frame) {
|
||||
let reports = [
|
||||
(KEY_REPORT, "buttons_raw"),
|
||||
(BUMPER_REPORT, "bumper_raw"),
|
||||
(DROP_REPORT, "drop_raw"),
|
||||
(CLIFF_REPORT, "cliff_raw"),
|
||||
(LDS_COVER_REPORT, "lds_cover_bumper_raw"),
|
||||
];
|
||||
let values = reports
|
||||
.into_iter()
|
||||
.filter_map(|(message, name)| {
|
||||
frame.report(message).and_then(|report| {
|
||||
little_u16(report.data).map(|value| (name, SensorValue::U16(value)))
|
||||
})
|
||||
})
|
||||
.collect::<Vec<_>>();
|
||||
if !values.is_empty() {
|
||||
update_group(&self.sensor_status, |data| {
|
||||
for (name, value) in values {
|
||||
data.set(name, value);
|
||||
}
|
||||
|
||||
// The two bumper channels occupy the low bit of each byte,
|
||||
// but their physical left/right order is not yet proven.
|
||||
// Reporting any contact on both aliases is conservative and
|
||||
// keeps existing collision handling safe.
|
||||
if let Some(value) = frame
|
||||
.report(BUMPER_REPORT)
|
||||
.and_then(|report| little_u16(report.data))
|
||||
{
|
||||
let any = value & 0x0101 != 0;
|
||||
data.set("bumper_left", SensorValue::Bool(any));
|
||||
data.set("bumper_right", SensorValue::Bool(any));
|
||||
}
|
||||
|
||||
// Four cliff channels occupy the low bit of four nibbles.
|
||||
// Until their physical ordering is confirmed, expose any
|
||||
// active channel on every directional compatibility alias.
|
||||
if let Some(value) = frame
|
||||
.report(CLIFF_REPORT)
|
||||
.and_then(|report| little_u16(report.data))
|
||||
{
|
||||
let any = value & 0x1111 != 0;
|
||||
for name in [
|
||||
"cliff_left_side",
|
||||
"cliff_left_front",
|
||||
"cliff_right_front",
|
||||
"cliff_right_side",
|
||||
] {
|
||||
data.set(name, SensorValue::Bool(any));
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
let dustbin = frame
|
||||
.report(DUSTBIN_REPORT)
|
||||
.and_then(|report| little_u16(report.data));
|
||||
let water_box = frame
|
||||
.report(WATER_BOX_REPORT)
|
||||
.and_then(|report| little_u16(report.data));
|
||||
if dustbin.is_some() || water_box.is_some() {
|
||||
update_group(&self.attachments, |data| {
|
||||
if let Some(value) = dustbin {
|
||||
data.set("dustbin_raw", SensorValue::U16(value));
|
||||
data.set("dustbin_attached", SensorValue::Bool(value & 1 == 0));
|
||||
}
|
||||
if let Some(value) = water_box {
|
||||
data.set("water_box_raw", SensorValue::U16(value));
|
||||
data.set("water_box_attached", SensorValue::Bool(value & 2 == 0));
|
||||
data.set(
|
||||
"water_box_presence_bit",
|
||||
SensorValue::U8(((value >> 1) & 1) as u8),
|
||||
);
|
||||
// The S5 Max stock handler uses only bit 1 for the water
|
||||
// box. It has no independent electronic mop-cloth sensor.
|
||||
data.set("mop_detection_supported", SensorValue::Bool(false));
|
||||
}
|
||||
});
|
||||
if let Some(value) = dustbin {
|
||||
update_group(&self.sensor_status, |data| {
|
||||
data.set("dustbox_attached", SensorValue::Bool(value & 1 == 0));
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fn process_power(&self, frame: &Frame) {
|
||||
let battery = frame.battery_report();
|
||||
let dock = frame
|
||||
.report(DOCK_REPORT)
|
||||
.and_then(|report| little_u16(report.data));
|
||||
let dock_voltage = frame.dock_voltage_report();
|
||||
let capacity = frame.battery_capacity_report();
|
||||
if battery.is_none() && dock.is_none() && dock_voltage.is_none() && capacity.is_none() {
|
||||
return;
|
||||
}
|
||||
if let Some(value) = dock {
|
||||
self.dock_state.store(value, Ordering::Release);
|
||||
}
|
||||
update_group(&self.power_status, |data| {
|
||||
if let Some(battery) = battery {
|
||||
data.set(
|
||||
"battery_voltage_v",
|
||||
SensorValue::F32(battery.voltage_mv as f32 / 1000.0),
|
||||
);
|
||||
data.set(
|
||||
"battery_current_magnitude_a",
|
||||
SensorValue::F32(battery.current_ma as f32 / 1000.0),
|
||||
);
|
||||
data.set(
|
||||
"battery_level_percent",
|
||||
SensorValue::U8(battery.state_of_charge_percent),
|
||||
);
|
||||
}
|
||||
if let Some(value) = dock {
|
||||
data.set("dock_state_raw", SensorValue::U16(value));
|
||||
data.set("dock_connected", SensorValue::Bool(value != 0));
|
||||
data.set("is_charging", SensorValue::Bool(value == 1));
|
||||
}
|
||||
if let Some(report) = dock_voltage {
|
||||
data.set(
|
||||
"dock_voltage_v",
|
||||
SensorValue::F32(report.millivolts as f32 / 1000.0),
|
||||
);
|
||||
}
|
||||
if let Some(report) = capacity {
|
||||
data.set(
|
||||
"battery_capacity_mah",
|
||||
SensorValue::U16(report.stock_scaled_value()),
|
||||
);
|
||||
}
|
||||
});
|
||||
// Mirror the established navigation-facing keys into sensor_status;
|
||||
// raw and higher-resolution values remain available in power_status.
|
||||
update_group(&self.sensor_status, |data| {
|
||||
if let Some(battery) = battery {
|
||||
data.set(
|
||||
"battery_voltage",
|
||||
SensorValue::F32(battery.voltage_mv as f32 / 1000.0),
|
||||
);
|
||||
data.set(
|
||||
"battery_level",
|
||||
SensorValue::U8(battery.state_of_charge_percent),
|
||||
);
|
||||
}
|
||||
if let Some(value) = dock {
|
||||
data.set("is_dock_connected", SensorValue::Bool(value != 0));
|
||||
data.set("is_charging", SensorValue::Bool(value == 1));
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
fn process_motors(&self, frame: &Frame) {
|
||||
let wheel = frame
|
||||
.wheel_report()
|
||||
.or_else(|| frame.embedded_wheel_report());
|
||||
let fan = frame.embedded_motor_report(FAN_REPORT_MESSAGE);
|
||||
let main_brush = frame.embedded_motor_report(BRUSH_REPORT_MESSAGE);
|
||||
let side_brush = frame.embedded_motor_report(SWEEP_REPORT_MESSAGE);
|
||||
if wheel.is_none() && fan.is_none() && main_brush.is_none() && side_brush.is_none() {
|
||||
return;
|
||||
}
|
||||
update_group(&self.motor_status, |data| {
|
||||
if let Some(wheel) = wheel {
|
||||
data.set(
|
||||
"wheel_left_current_ma",
|
||||
SensorValue::U16(wheel.left_current_ma()),
|
||||
);
|
||||
data.set(
|
||||
"wheel_right_current_ma",
|
||||
SensorValue::U16(wheel.right_current_ma()),
|
||||
);
|
||||
data.set(
|
||||
"wheel_left_drive_level",
|
||||
SensorValue::I8(wheel.left_drive_level()),
|
||||
);
|
||||
data.set(
|
||||
"wheel_right_drive_level",
|
||||
SensorValue::I8(wheel.right_drive_level()),
|
||||
);
|
||||
data.set("wheel_left_fault", SensorValue::U8(wheel.left_fault()));
|
||||
data.set("wheel_right_fault", SensorValue::U8(wheel.right_fault()));
|
||||
}
|
||||
for (prefix, report) in [
|
||||
("fan", fan),
|
||||
("main_brush", main_brush),
|
||||
("side_brush", side_brush),
|
||||
] {
|
||||
if let Some(report) = report {
|
||||
data.set(
|
||||
&format!("{prefix}_current_ma"),
|
||||
SensorValue::U16(report.current_ma),
|
||||
);
|
||||
data.set(
|
||||
&format!("{prefix}_speed_raw"),
|
||||
SensorValue::U8(report.speed),
|
||||
);
|
||||
data.set(&format!("{prefix}_fault"), SensorValue::U8(report.fault));
|
||||
}
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
fn process_errors(&self, frame: &Frame) {
|
||||
let internal = frame.internal_error_report();
|
||||
let pump = frame.report(PUMP_ERROR_REPORT).map(|report| report.data);
|
||||
if internal.is_none() && pump.is_none() {
|
||||
return;
|
||||
}
|
||||
update_group(&self.errors, |data| {
|
||||
if let Some(report) = internal {
|
||||
data.set("internal_raw", SensorValue::U32(report.raw));
|
||||
data.set(
|
||||
"test_info_invalid",
|
||||
SensorValue::Bool(report.test_info_invalid()),
|
||||
);
|
||||
data.set(
|
||||
"gyro_probe_failed",
|
||||
SensorValue::Bool(report.gyro_probe_failed()),
|
||||
);
|
||||
data.set(
|
||||
"bms_communication_failed",
|
||||
SensorValue::Bool(report.bms_communication_failed()),
|
||||
);
|
||||
}
|
||||
if let Some(bytes) = pump {
|
||||
data.set("water_pump_error_raw", SensorValue::Bytes(bytes.to_vec()));
|
||||
}
|
||||
});
|
||||
}
|
||||
|
||||
fn process_dock_ir(&self, frame: &Frame) {
|
||||
let Some(report) = frame.dock_ir_report() else {
|
||||
return;
|
||||
};
|
||||
update_group(&self.dock_ir, |data| {
|
||||
data.set("left_mask", SensorValue::U8(report.left_receiver.0));
|
||||
data.set("right_mask", SensorValue::U8(report.right_receiver.0));
|
||||
data.set(
|
||||
"left_far_left",
|
||||
SensorValue::Bool(report.left_receiver.beacon_left()),
|
||||
);
|
||||
data.set(
|
||||
"left_far_right",
|
||||
SensorValue::Bool(report.left_receiver.beacon_right()),
|
||||
);
|
||||
data.set(
|
||||
"left_close_left",
|
||||
SensorValue::Bool(report.left_receiver.code_84()),
|
||||
);
|
||||
data.set(
|
||||
"left_close_right",
|
||||
SensorValue::Bool(report.left_receiver.code_88()),
|
||||
);
|
||||
data.set(
|
||||
"left_common",
|
||||
SensorValue::Bool(report.left_receiver.majority_pattern()),
|
||||
);
|
||||
data.set(
|
||||
"right_far_left",
|
||||
SensorValue::Bool(report.right_receiver.beacon_left()),
|
||||
);
|
||||
data.set(
|
||||
"right_far_right",
|
||||
SensorValue::Bool(report.right_receiver.beacon_right()),
|
||||
);
|
||||
data.set(
|
||||
"right_close_left",
|
||||
SensorValue::Bool(report.right_receiver.code_84()),
|
||||
);
|
||||
data.set(
|
||||
"right_close_right",
|
||||
SensorValue::Bool(report.right_receiver.code_88()),
|
||||
);
|
||||
data.set(
|
||||
"right_common",
|
||||
SensorValue::Bool(report.right_receiver.majority_pattern()),
|
||||
);
|
||||
});
|
||||
}
|
||||
|
||||
fn process_version(&self, frame: &Frame) {
|
||||
let version = frame.report(MCU_VERSION_REPORT).and_then(|report| {
|
||||
let end = report
|
||||
.data
|
||||
.iter()
|
||||
.position(|byte| *byte == 0)
|
||||
.unwrap_or(report.data.len());
|
||||
std::str::from_utf8(&report.data[..end]).ok()
|
||||
});
|
||||
let mode = frame.system_mode_report();
|
||||
if version.is_none() && mode.is_none() {
|
||||
return;
|
||||
}
|
||||
update_group(&self.device_version, |data| {
|
||||
if let Some(version) = version {
|
||||
data.set("mcu_version", SensorValue::String(version.to_string()));
|
||||
}
|
||||
if let Some(mode) = mode {
|
||||
data.set(
|
||||
"system_mode",
|
||||
SensorValue::String(format!("{:?}", mode.mode)),
|
||||
);
|
||||
}
|
||||
});
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::SensorGroups;
|
||||
use crate::core::types::SensorValue;
|
||||
use crate::devices::roborock_s5max::packet::{Frame, FrameDecoder};
|
||||
|
||||
fn report_frame(payload: Vec<u8>) -> Frame {
|
||||
Frame {
|
||||
raw: Vec::new(),
|
||||
payload,
|
||||
escape_overhead: 0,
|
||||
message: 0x0b,
|
||||
command: 0x08,
|
||||
crc: 0,
|
||||
crc_valid: true,
|
||||
is_log: false,
|
||||
}
|
||||
}
|
||||
|
||||
fn bool_value(value: Option<&SensorValue>) -> bool {
|
||||
let Some(SensorValue::Bool(value)) = value else {
|
||||
panic!("expected boolean sensor value");
|
||||
};
|
||||
*value
|
||||
}
|
||||
|
||||
fn bytes(hex: &str) -> Vec<u8> {
|
||||
(0..hex.len())
|
||||
.step_by(2)
|
||||
.map(|index| u8::from_str_radix(&hex[index..index + 2], 16).unwrap())
|
||||
.collect()
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn publishes_calibration_metadata() {
|
||||
let (groups, _) = SensorGroups::create(0.798, 0.229, 261.2);
|
||||
let data = groups.device_version.lock().unwrap();
|
||||
assert!(matches!(
|
||||
data.values.get("wheel_mm_per_tick"),
|
||||
Some(SensorValue::F32(value)) if (*value - 0.798).abs() < f32::EPSILON
|
||||
));
|
||||
assert!(matches!(
|
||||
data.values.get("wheel_track_m"),
|
||||
Some(SensorValue::F32(value)) if (*value - 0.229).abs() < f32::EPSILON
|
||||
));
|
||||
assert!(matches!(
|
||||
data.values.get("lds_forward_angle_deg"),
|
||||
Some(SensorValue::F32(value)) if (*value - 261.2).abs() < f32::EPSILON
|
||||
));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn maps_stock_attachment_presence_bits_without_inventing_mop_detection() {
|
||||
let (groups, _) = SensorGroups::create(0.798, 0.229, 261.2);
|
||||
groups.process_mcu_frame(&report_frame(vec![
|
||||
0x24, 0x02, 0x00, 0x00, // dustbin present: bit 0 clear
|
||||
0x26, 0x02, 0x00, 0x00, // water box present: bit 1 clear
|
||||
]));
|
||||
{
|
||||
let data = groups.attachments.lock().unwrap();
|
||||
assert!(bool_value(data.values.get("dustbin_attached")));
|
||||
assert!(bool_value(data.values.get("water_box_attached")));
|
||||
assert!(!bool_value(data.values.get("mop_detection_supported")));
|
||||
assert!(!data.values.contains_key("mop_attached"));
|
||||
}
|
||||
|
||||
groups.process_mcu_frame(&report_frame(vec![
|
||||
0x24, 0x02, 0x01, 0x00, // dustbin absent: bit 0 set
|
||||
0x26, 0x02, 0x02, 0x00, // water box absent: bit 1 set
|
||||
]));
|
||||
let data = groups.attachments.lock().unwrap();
|
||||
assert!(!bool_value(data.values.get("dustbin_attached")));
|
||||
assert!(!bool_value(data.values.get("water_box_attached")));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn publishes_navigation_compatibility_aliases() {
|
||||
let moving = bytes(
|
||||
"aa5c010740989d853d755710bf326c22416889f43bfd6b943cf8b18bbc865f753cf07d95bcf88ca2bded1e7fbfc4000000f0000000c3cfe14000000000e06e130200000000510888007400433f00005206ffff6501004b42026d00d0020015b4",
|
||||
);
|
||||
let frame = FrameDecoder::new().push(&moving).pop().unwrap();
|
||||
let (groups, _) = SensorGroups::create(0.798, 0.229, 261.2);
|
||||
groups.process_mcu_frame(&frame);
|
||||
{
|
||||
let data = groups.sensor_status.lock().unwrap();
|
||||
assert!(matches!(
|
||||
data.values.get("wheel_left"),
|
||||
Some(SensorValue::U16(196))
|
||||
));
|
||||
assert!(matches!(
|
||||
data.values.get("wheel_right"),
|
||||
Some(SensorValue::U16(240))
|
||||
));
|
||||
assert!(matches!(
|
||||
data.values.get("gyro_z"),
|
||||
Some(SensorValue::I16(_))
|
||||
));
|
||||
}
|
||||
|
||||
groups.process_mcu_frame(&report_frame(vec![
|
||||
0x21, 0x02, 0x01, 0x00, // one bumper channel active
|
||||
0x23, 0x02, 0x10, 0x00, // one cliff channel active
|
||||
0x24, 0x02, 0x00, 0x00, // dustbin present
|
||||
]));
|
||||
let data = groups.sensor_status.lock().unwrap();
|
||||
assert!(bool_value(data.values.get("bumper_left")));
|
||||
assert!(bool_value(data.values.get("bumper_right")));
|
||||
assert!(bool_value(data.values.get("cliff_left_side")));
|
||||
assert!(bool_value(data.values.get("cliff_left_front")));
|
||||
assert!(bool_value(data.values.get("cliff_right_front")));
|
||||
assert!(bool_value(data.values.get("cliff_right_side")));
|
||||
assert!(bool_value(data.values.get("dustbox_attached")));
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn labels_bms_current_as_unsigned_magnitude() {
|
||||
let input = bytes("aa1001080a06407300610000000000d002012776");
|
||||
let frame = FrameDecoder::new().push(&input).pop().unwrap();
|
||||
let (groups, _) = SensorGroups::create(0.798, 0.229, 261.2);
|
||||
groups.process_mcu_frame(&frame);
|
||||
let data = groups.power_status.lock().unwrap();
|
||||
assert!(matches!(
|
||||
data.values.get("battery_current_magnitude_a"),
|
||||
Some(SensorValue::F32(value)) if (*value - 0.115).abs() < f32::EPSILON
|
||||
));
|
||||
assert!(!data.values.contains_key("battery_current_a"));
|
||||
drop(data);
|
||||
|
||||
let data = groups.sensor_status.lock().unwrap();
|
||||
assert!(matches!(
|
||||
data.values.get("battery_level"),
|
||||
Some(SensorValue::U8(97))
|
||||
));
|
||||
assert!(matches!(
|
||||
data.values.get("battery_voltage"),
|
||||
Some(SensorValue::F32(value)) if (*value - 16.390).abs() < f32::EPSILON
|
||||
));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,18 @@
|
||||
//! Platform shims so the driver type-checks on non-Linux development hosts.
|
||||
//!
|
||||
//! The driver only runs on Linux. These aliases exist so the crate, and the
|
||||
//! driver's pure unit tests (framing, decoding, lifecycle, safety), build on
|
||||
//! macOS too.
|
||||
|
||||
/// Request-argument type for `libc::ioctl`.
|
||||
#[cfg(target_os = "linux")]
|
||||
pub(crate) type IoctlRequest = libc::Ioctl;
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub(crate) type IoctlRequest = libc::c_ulong;
|
||||
|
||||
/// MCU UART speed. Linux exposes a `B1152000` symbol; Apple termios takes the
|
||||
/// literal rate as `speed_t`.
|
||||
#[cfg(target_os = "linux")]
|
||||
pub(crate) const MCU_BAUD: libc::speed_t = libc::B1152000;
|
||||
#[cfg(not(target_os = "linux"))]
|
||||
pub(crate) const MCU_BAUD: libc::speed_t = 1_152_000;
|
||||
@@ -0,0 +1,306 @@
|
||||
//! Exclusive raw-UART ownership for the S5 Max Linux target.
|
||||
|
||||
use super::sys::IoctlRequest;
|
||||
use crate::error::{Error, Result};
|
||||
use std::fs::{self, File, OpenOptions};
|
||||
use std::io;
|
||||
use std::os::fd::{AsRawFd, RawFd};
|
||||
use std::os::unix::fs::OpenOptionsExt;
|
||||
use std::path::Path;
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
const WRITE_TIMEOUT: Duration = Duration::from_millis(100);
|
||||
|
||||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||||
pub(crate) struct DeviceOwner {
|
||||
pub pid: i32,
|
||||
pub fd: i32,
|
||||
pub process: String,
|
||||
pub device: String,
|
||||
}
|
||||
|
||||
fn process_name(proc_root: &Path, pid: i32) -> String {
|
||||
fs::read_to_string(proc_root.join(pid.to_string()).join("comm"))
|
||||
.map(|name| name.trim().to_string())
|
||||
.unwrap_or_else(|_| "unknown".to_string())
|
||||
}
|
||||
|
||||
pub(crate) fn discover_device_owners_at(
|
||||
proc_root: &Path,
|
||||
selected_devices: &[&str],
|
||||
) -> Result<Vec<DeviceOwner>> {
|
||||
let entries = fs::read_dir(proc_root)?;
|
||||
let mut owners = Vec::new();
|
||||
|
||||
for entry in entries.flatten() {
|
||||
let Some(pid) = entry
|
||||
.file_name()
|
||||
.to_str()
|
||||
.and_then(|name| name.parse::<i32>().ok())
|
||||
else {
|
||||
continue;
|
||||
};
|
||||
let Ok(fds) = fs::read_dir(entry.path().join("fd")) else {
|
||||
continue;
|
||||
};
|
||||
for fd_entry in fds.flatten() {
|
||||
let Some(fd) = fd_entry
|
||||
.file_name()
|
||||
.to_str()
|
||||
.and_then(|name| name.parse::<i32>().ok())
|
||||
else {
|
||||
continue;
|
||||
};
|
||||
let Ok(target) = fs::read_link(fd_entry.path()) else {
|
||||
continue;
|
||||
};
|
||||
let device = target.to_string_lossy();
|
||||
if selected_devices.iter().any(|selected| device == *selected) {
|
||||
owners.push(DeviceOwner {
|
||||
pid,
|
||||
fd,
|
||||
process: process_name(proc_root, pid),
|
||||
device: device.into_owned(),
|
||||
});
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
owners.sort_by_key(|owner| (owner.pid, owner.fd));
|
||||
Ok(owners)
|
||||
}
|
||||
|
||||
pub(crate) fn ensure_devices_unowned(selected_devices: &[&str]) -> Result<()> {
|
||||
let owners = discover_device_owners_at(Path::new("/proc"), selected_devices)?;
|
||||
if owners.is_empty() {
|
||||
return Ok(());
|
||||
}
|
||||
let owners = owners
|
||||
.iter()
|
||||
.map(|owner| {
|
||||
format!(
|
||||
"pid={} process={} fd={} device={}",
|
||||
owner.pid, owner.process, owner.fd, owner.device
|
||||
)
|
||||
})
|
||||
.collect::<Vec<_>>()
|
||||
.join("; ");
|
||||
Err(Error::Other(format!(
|
||||
"S5 Max hardware is already owned: {owners}"
|
||||
)))
|
||||
}
|
||||
|
||||
pub(crate) struct ExclusiveTty {
|
||||
file: Option<File>,
|
||||
original: libc::termios,
|
||||
path: String,
|
||||
}
|
||||
|
||||
impl ExclusiveTty {
|
||||
pub(crate) fn open(path: &str, baud: libc::speed_t, baud_number: u32) -> Result<Self> {
|
||||
let file = OpenOptions::new()
|
||||
.read(true)
|
||||
.write(true)
|
||||
.custom_flags(libc::O_NOCTTY | libc::O_NONBLOCK | libc::O_CLOEXEC)
|
||||
.open(path)?;
|
||||
let fd = file.as_raw_fd();
|
||||
|
||||
if unsafe { libc::ioctl(fd, libc::TIOCEXCL as IoctlRequest) } < 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"TIOCEXCL {path}: {}",
|
||||
io::Error::last_os_error()
|
||||
)));
|
||||
}
|
||||
|
||||
let mut original = std::mem::MaybeUninit::<libc::termios>::uninit();
|
||||
if unsafe { libc::tcgetattr(fd, original.as_mut_ptr()) } < 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"tcgetattr {path}: {}",
|
||||
io::Error::last_os_error()
|
||||
)));
|
||||
}
|
||||
let original = unsafe { original.assume_init() };
|
||||
let mut configured = original;
|
||||
unsafe { libc::cfmakeraw(&mut configured) };
|
||||
configured.c_cflag &= !(libc::CSIZE | libc::PARENB | libc::CSTOPB | libc::CRTSCTS);
|
||||
configured.c_cflag |= libc::CS8 | libc::CLOCAL | libc::CREAD;
|
||||
configured.c_cc[libc::VMIN] = 0;
|
||||
configured.c_cc[libc::VTIME] = 1;
|
||||
|
||||
if unsafe { libc::cfsetispeed(&mut configured, baud) } < 0
|
||||
|| unsafe { libc::cfsetospeed(&mut configured, baud) } < 0
|
||||
{
|
||||
return Err(Error::Other(format!(
|
||||
"configure {baud_number} baud for {path}: {}",
|
||||
io::Error::last_os_error()
|
||||
)));
|
||||
}
|
||||
if unsafe { libc::tcsetattr(fd, libc::TCSANOW, &configured) } < 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"tcsetattr {path}: {}",
|
||||
io::Error::last_os_error()
|
||||
)));
|
||||
}
|
||||
|
||||
Ok(Self {
|
||||
file: Some(file),
|
||||
original,
|
||||
path: path.to_string(),
|
||||
})
|
||||
}
|
||||
|
||||
pub(crate) fn fd(&self) -> RawFd {
|
||||
self.file
|
||||
.as_ref()
|
||||
.expect("exclusive tty is open")
|
||||
.as_raw_fd()
|
||||
}
|
||||
|
||||
pub(crate) fn read_available(&self, buffer: &mut [u8]) -> Result<usize> {
|
||||
let count = unsafe {
|
||||
libc::read(
|
||||
self.fd(),
|
||||
buffer.as_mut_ptr().cast::<libc::c_void>(),
|
||||
buffer.len(),
|
||||
)
|
||||
};
|
||||
if count >= 0 {
|
||||
return Ok(count as usize);
|
||||
}
|
||||
let error = io::Error::last_os_error();
|
||||
if matches!(
|
||||
error.kind(),
|
||||
io::ErrorKind::Interrupted | io::ErrorKind::WouldBlock
|
||||
) {
|
||||
Ok(0)
|
||||
} else {
|
||||
Err(error.into())
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) fn poll_readable(&self, timeout: Duration) -> Result<bool> {
|
||||
let mut poll_fd = libc::pollfd {
|
||||
fd: self.fd(),
|
||||
events: libc::POLLIN,
|
||||
revents: 0,
|
||||
};
|
||||
let timeout_ms = timeout.as_millis().min(i32::MAX as u128) as i32;
|
||||
let result = unsafe { libc::poll(&mut poll_fd, 1, timeout_ms) };
|
||||
if result < 0 {
|
||||
let error = io::Error::last_os_error();
|
||||
if error.kind() == io::ErrorKind::Interrupted {
|
||||
return Ok(false);
|
||||
}
|
||||
return Err(error.into());
|
||||
}
|
||||
if poll_fd.revents & (libc::POLLERR | libc::POLLHUP | libc::POLLNVAL) != 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"poll {} terminal revents=0x{:x}",
|
||||
self.path, poll_fd.revents
|
||||
)));
|
||||
}
|
||||
Ok(result > 0 && poll_fd.revents & libc::POLLIN != 0)
|
||||
}
|
||||
|
||||
pub(crate) fn write_complete(&self, frame: &[u8]) -> Result<()> {
|
||||
let mut written = 0;
|
||||
let deadline = Instant::now() + WRITE_TIMEOUT;
|
||||
while written < frame.len() {
|
||||
let count = unsafe {
|
||||
libc::write(
|
||||
self.fd(),
|
||||
frame[written..].as_ptr().cast::<libc::c_void>(),
|
||||
frame.len() - written,
|
||||
)
|
||||
};
|
||||
if count > 0 {
|
||||
written += count as usize;
|
||||
continue;
|
||||
}
|
||||
if count == 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"write {} returned zero after {written} bytes",
|
||||
self.path
|
||||
)));
|
||||
}
|
||||
let error = io::Error::last_os_error();
|
||||
if error.kind() == io::ErrorKind::Interrupted {
|
||||
continue;
|
||||
}
|
||||
if error.kind() == io::ErrorKind::WouldBlock {
|
||||
if Instant::now() >= deadline {
|
||||
return Err(Error::Other(format!(
|
||||
"write {} timed out after {}ms ({written}/{} bytes)",
|
||||
self.path,
|
||||
WRITE_TIMEOUT.as_millis(),
|
||||
frame.len()
|
||||
)));
|
||||
}
|
||||
let mut poll_fd = libc::pollfd {
|
||||
fd: self.fd(),
|
||||
events: libc::POLLOUT,
|
||||
revents: 0,
|
||||
};
|
||||
let result = unsafe { libc::poll(&mut poll_fd, 1, 20) };
|
||||
if result < 0 && io::Error::last_os_error().kind() == io::ErrorKind::Interrupted {
|
||||
continue;
|
||||
}
|
||||
if result < 0 {
|
||||
return Err(io::Error::last_os_error().into());
|
||||
}
|
||||
if poll_fd.revents & (libc::POLLERR | libc::POLLHUP | libc::POLLNVAL) != 0 {
|
||||
return Err(Error::Other(format!(
|
||||
"poll {} while writing returned terminal revents=0x{:x}",
|
||||
self.path, poll_fd.revents
|
||||
)));
|
||||
}
|
||||
continue;
|
||||
}
|
||||
return Err(error.into());
|
||||
}
|
||||
if unsafe { libc::tcdrain(self.fd()) } < 0 {
|
||||
return Err(io::Error::last_os_error().into());
|
||||
}
|
||||
Ok(())
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for ExclusiveTty {
|
||||
fn drop(&mut self) {
|
||||
if let Some(file) = &self.file {
|
||||
let fd = file.as_raw_fd();
|
||||
let _ = unsafe { libc::tcsetattr(fd, libc::TCSANOW, &self.original) };
|
||||
let _ = unsafe { libc::ioctl(fd, libc::TIOCNXCL as IoctlRequest) };
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::discover_device_owners_at;
|
||||
use std::fs;
|
||||
use std::os::unix::fs::symlink;
|
||||
use std::time::{SystemTime, UNIX_EPOCH};
|
||||
|
||||
#[test]
|
||||
fn reports_all_selected_device_owners() {
|
||||
let unique = SystemTime::now()
|
||||
.duration_since(UNIX_EPOCH)
|
||||
.unwrap()
|
||||
.as_nanos();
|
||||
let root = std::env::temp_dir().join(format!("s5max-driver-proc-{unique}"));
|
||||
fs::create_dir_all(root.join("490/fd")).unwrap();
|
||||
fs::write(root.join("490/comm"), "rr_loader\n").unwrap();
|
||||
symlink("/dev/ttyS2", root.join("490/fd/58")).unwrap();
|
||||
symlink("/dev/uart_mcu", root.join("490/fd/59")).unwrap();
|
||||
symlink("/dev/ttyS1", root.join("490/fd/60")).unwrap();
|
||||
|
||||
let owners = discover_device_owners_at(&root, &["/dev/ttyS2", "/dev/uart_mcu"]).unwrap();
|
||||
assert_eq!(owners.len(), 2);
|
||||
assert_eq!(owners[0].process, "rr_loader");
|
||||
assert_eq!(owners[0].fd, 58);
|
||||
assert_eq!(owners[1].device, "/dev/uart_mcu");
|
||||
|
||||
fs::remove_dir_all(root).unwrap();
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,127 @@
|
||||
//! Hardware watchdog ownership matching the stock WatchDoge contract.
|
||||
|
||||
use super::lifecycle::Lifecycle;
|
||||
use super::sys::IoctlRequest;
|
||||
use crate::error::{Error, Result};
|
||||
use std::fs::{File, OpenOptions};
|
||||
use std::io::Write;
|
||||
use std::os::fd::{AsRawFd, RawFd};
|
||||
use std::os::unix::fs::OpenOptionsExt;
|
||||
use std::sync::Arc;
|
||||
use std::sync::atomic::{AtomicBool, Ordering};
|
||||
use std::thread::{self, JoinHandle};
|
||||
use std::time::{Duration, Instant};
|
||||
|
||||
const TIMEOUT_SECONDS: libc::c_int = 16;
|
||||
const KEEPALIVE_INTERVAL: Duration = Duration::from_secs(2);
|
||||
const WDIOC_KEEPALIVE: IoctlRequest = 0x8004_5705u32 as IoctlRequest;
|
||||
const WDIOC_SETTIMEOUT: IoctlRequest = 0xc004_5706u32 as IoctlRequest;
|
||||
|
||||
struct MagicCloseWatchdog(File);
|
||||
|
||||
impl MagicCloseWatchdog {
|
||||
fn open(path: &str) -> Result<Self> {
|
||||
OpenOptions::new()
|
||||
.write(true)
|
||||
.custom_flags(libc::O_CLOEXEC)
|
||||
.open(path)
|
||||
.map(Self)
|
||||
.map_err(Error::Io)
|
||||
}
|
||||
}
|
||||
|
||||
impl AsRawFd for MagicCloseWatchdog {
|
||||
fn as_raw_fd(&self) -> RawFd {
|
||||
self.0.as_raw_fd()
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for MagicCloseWatchdog {
|
||||
fn drop(&mut self) {
|
||||
// The stock WatchDoge process uses Linux's magic-close contract too.
|
||||
// Writing V disarms the watchdog when the descriptor closes cleanly.
|
||||
let _ = self.0.write_all(b"V");
|
||||
}
|
||||
}
|
||||
|
||||
pub(crate) struct HardwareWatchdog {
|
||||
handle: Option<JoinHandle<Result<()>>>,
|
||||
shutdown: Arc<AtomicBool>,
|
||||
}
|
||||
|
||||
impl HardwareWatchdog {
|
||||
pub(crate) fn start(
|
||||
path: &str,
|
||||
lifecycle: Lifecycle,
|
||||
shutdown: Arc<AtomicBool>,
|
||||
) -> Result<Self> {
|
||||
let file = MagicCloseWatchdog::open(path).map_err(|error| {
|
||||
Error::Other(format!(
|
||||
"open S5 Max watchdog {path}: {error}; stop WatchDoge before takeover"
|
||||
))
|
||||
})?;
|
||||
let fd = file.as_raw_fd();
|
||||
let mut timeout = TIMEOUT_SECONDS;
|
||||
if unsafe { libc::ioctl(fd, WDIOC_SETTIMEOUT, &mut timeout) } < 0 {
|
||||
return Err(std::io::Error::last_os_error().into());
|
||||
}
|
||||
if unsafe { libc::ioctl(fd, WDIOC_KEEPALIVE, 0) } < 0 {
|
||||
return Err(std::io::Error::last_os_error().into());
|
||||
}
|
||||
|
||||
let path = path.to_string();
|
||||
let thread_shutdown = Arc::clone(&shutdown);
|
||||
let thread_lifecycle = lifecycle;
|
||||
let handle = thread::Builder::new()
|
||||
.name("s5max-watchdog".to_string())
|
||||
.spawn(move || {
|
||||
let file = file;
|
||||
let mut next_keepalive = Instant::now() + KEEPALIVE_INTERVAL;
|
||||
while !thread_shutdown.load(Ordering::Acquire) {
|
||||
let now = Instant::now();
|
||||
if now >= next_keepalive {
|
||||
if unsafe { libc::ioctl(file.as_raw_fd(), WDIOC_KEEPALIVE, 0) } < 0 {
|
||||
let reason = format!(
|
||||
"watchdog keepalive {path}: {}",
|
||||
std::io::Error::last_os_error()
|
||||
);
|
||||
thread_lifecycle.fault(reason.clone());
|
||||
thread_shutdown.store(true, Ordering::Release);
|
||||
return Err(Error::Other(reason));
|
||||
}
|
||||
next_keepalive = now + KEEPALIVE_INTERVAL;
|
||||
}
|
||||
thread::sleep(Duration::from_millis(20));
|
||||
}
|
||||
drop(file);
|
||||
Ok(())
|
||||
})
|
||||
.map_err(|error| Error::Other(format!("spawn S5 Max watchdog worker: {error}")))?;
|
||||
|
||||
log::info!(
|
||||
"S5 Max watchdog acquired: {}s timeout, {}ms keepalive",
|
||||
timeout,
|
||||
KEEPALIVE_INTERVAL.as_millis()
|
||||
);
|
||||
Ok(Self {
|
||||
handle: Some(handle),
|
||||
shutdown,
|
||||
})
|
||||
}
|
||||
|
||||
pub(crate) fn join(&mut self) -> Result<()> {
|
||||
let Some(handle) = self.handle.take() else {
|
||||
return Ok(());
|
||||
};
|
||||
handle.join().map_err(|_| Error::ThreadPanic)?
|
||||
}
|
||||
}
|
||||
|
||||
impl Drop for HardwareWatchdog {
|
||||
fn drop(&mut self) {
|
||||
self.shutdown.store(true, Ordering::Release);
|
||||
if let Err(error) = self.join() {
|
||||
log::error!("S5 Max watchdog shutdown failed: {error}");
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user