Enuxia-Public/enuxia-vacuum
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(s5max): address portability and integration review

Browse Source
This commit is contained in:
Fabien Carrion
2026-09-12 23:13:02 -06:00
parent 0c5d173bb8
commit 667a00e795
11 changed files with 328 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sangam-io/README.md
+132 -2
View File
@@ -85,6 +85,11 @@ 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 |
@@ -113,12 +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
@@ -327,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) |
sangam-io/src/bin/roborock_s5max_sequence.rs
+4
View File
@@ -10,6 +10,10 @@ 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> {
sangam-io/src/devices/roborock_s5max/lds.rs
+1 -1
View File
@@ -81,7 +81,7 @@ impl LdsPacket {
/// 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_exact(2) {
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
sangam-io/src/devices/roborock_s5max/lds_motor.rs
+12 -6
View File
@@ -1,5 +1,6 @@
//! 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;
@@ -10,11 +11,11 @@ use std::sync::atomic::{AtomicBool, Ordering};
use std::thread;
use std::time::Duration;
const LDS_MOTOR_SET_SPEED: libc::Ioctl = 0x4004_f810u32 as libc::Ioctl;
pub(crate) const LDS_MOTOR_SET_CURRENT_SPEED: libc::Ioctl = 0x4004_f812u32 as libc::Ioctl;
const LDS_MOTOR_START: libc::Ioctl = 0x4004_f813u32 as libc::Ioctl;
const LDS_MOTOR_STOP: libc::Ioctl = 0x4004_f814u32 as libc::Ioctl;
const LDS_MOTOR_SET_PRODUCT_ID: libc::Ioctl = 0x4004_f826u32 as libc::Ioctl;
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;
@@ -43,7 +44,12 @@ impl LdsMotorGuard {
})
}
fn ioctl_value(&self, request: libc::Ioctl, value: libc::c_int, operation: &str) -> Result<()> {
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!(
sangam-io/src/devices/roborock_s5max/mcu.rs
+1 -1
View File
@@ -11,6 +11,7 @@ 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};
@@ -21,7 +22,6 @@ use std::sync::{Arc, Condvar, Mutex};
use std::thread::{self, JoinHandle};
use std::time::{Duration, Instant};
const MCU_BAUD: libc::speed_t = libc::B1152000;
const MCU_BAUD_NUMBER: u32 = 1_152_000;
const HEARTBEAT_INTERVAL: Duration = Duration::from_secs(1);
const ACK_TIMEOUT: Duration = Duration::from_millis(23);
sangam-io/src/devices/roborock_s5max/mod.rs
+4
View File
@@ -14,6 +14,7 @@ mod mcu;
pub mod packet;
mod safety;
mod sensors;
mod sys;
mod tty;
mod watchdog;
@@ -329,6 +330,9 @@ impl RoborockS5MaxDriver {
"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 {
sangam-io/src/devices/roborock_s5max/packet.rs
+1 -1
View File
@@ -790,7 +790,7 @@ impl Frame {
drop,
bumper,
dock,
cliff: cliff as u32,
cliff,
})
}
sangam-io/src/devices/roborock_s5max/sensors.rs
+149 -10
View File
@@ -26,6 +26,12 @@ 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),
@@ -127,6 +133,26 @@ impl SensorGroups {
"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]));
@@ -139,6 +165,17 @@ impl SensorGroups {
"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),
@@ -189,6 +226,37 @@ impl SensorGroups {
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));
}
}
});
}
@@ -216,6 +284,11 @@ impl SensorGroups {
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));
});
}
}
}
@@ -265,6 +338,24 @@ impl SensorGroups {
);
}
});
// 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) {
@@ -449,6 +540,13 @@ mod tests {
*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);
@@ -491,18 +589,48 @@ mod tests {
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 = (0.."aa1001080a06407300610000000000d002012776".len())
.step_by(2)
.map(|index| {
u8::from_str_radix(
&"aa1001080a06407300610000000000d002012776"[index..index + 2],
16,
)
.unwrap()
})
.collect::<Vec<_>>();
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);
@@ -512,5 +640,16 @@ mod tests {
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
));
}
}
sangam-io/src/devices/roborock_s5max/sys.rs
+18
View File
@@ -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;
sangam-io/src/devices/roborock_s5max/tty.rs
+3 -2
View File
@@ -1,5 +1,6 @@
//! 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;
@@ -104,7 +105,7 @@ impl ExclusiveTty {
.open(path)?;
let fd = file.as_raw_fd();
if unsafe { libc::ioctl(fd, libc::TIOCEXCL as libc::Ioctl) } < 0 {
if unsafe { libc::ioctl(fd, libc::TIOCEXCL as IoctlRequest) } < 0 {
return Err(Error::Other(format!(
"TIOCEXCL {path}: {}",
io::Error::last_os_error()
@@ -269,7 +270,7 @@ impl Drop for ExclusiveTty {
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 libc::Ioctl) };
let _ = unsafe { libc::ioctl(fd, libc::TIOCNXCL as IoctlRequest) };
}
}
}
sangam-io/src/devices/roborock_s5max/watchdog.rs
+3 -2
View File
@@ -1,6 +1,7 @@
//! 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;
@@ -13,8 +14,8 @@ use std::time::{Duration, Instant};
const TIMEOUT_SECONDS: libc::c_int = 16;
const KEEPALIVE_INTERVAL: Duration = Duration::from_secs(2);
const WDIOC_KEEPALIVE: libc::Ioctl = 0x8004_5705u32 as libc::Ioctl;
const WDIOC_SETTIMEOUT: libc::Ioctl = 0xc004_5706u32 as libc::Ioctl;
const WDIOC_KEEPALIVE: IoctlRequest = 0x8004_5705u32 as IoctlRequest;
const WDIOC_SETTIMEOUT: IoctlRequest = 0xc004_5706u32 as IoctlRequest;
struct MagicCloseWatchdog(File);