dtourolleandClaude Opus 5
7c17ca6158
Adds backing state for Resistance and Erg control modes, which had no value to hold and so could never satisfy FR-4.3/FR-4.6. Co-Authored-By: Claude Opus 5 <noreply@anthropic.com>
489 lines
17 KiB
Rust
489 lines
17 KiB
Rust
//! Encoders and decoders for the FTMS **Fitness Machine Control Point**
|
||
//! (`0x2AD9`).
|
||
//!
|
||
//! The control point is a request/response characteristic: the central writes a
|
||
//! procedure (with response), and the fitness machine replies with an
|
||
//! **indication** of the form `[0x80, request_op_code, result_code, ...params]`.
|
||
//! Treating writes as fire-and-forget is a bug (FR-2.7) — a trainer that has
|
||
//! not granted control will silently ignore everything until `RequestControl`
|
||
//! succeeds, and there is no other way to find that out.
|
||
//!
|
||
//! The op-code table and the sint16 encodings for inclination, resistance and
|
||
//! target power were ported from `obostjancic/smart-trainer-control`
|
||
//! (`src/lib/bike/ftms-control.ts`), MIT licensed, Copyright (c) 2025 Ogi —
|
||
//! a working Van Rysel D100 client. See REQUIREMENTS.md §3.2.
|
||
|
||
/// FTMS Fitness Machine Control Point op codes.
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq, PartialOrd, Ord, Hash)]
|
||
#[repr(u8)]
|
||
pub enum OpCode {
|
||
RequestControl = 0x00,
|
||
Reset = 0x01,
|
||
SetTargetSpeed = 0x02,
|
||
SetTargetInclination = 0x03,
|
||
SetTargetResistanceLevel = 0x04,
|
||
SetTargetPower = 0x05,
|
||
SetTargetHeartRate = 0x06,
|
||
StartOrResume = 0x07,
|
||
StopOrPause = 0x08,
|
||
SetTargetedExpendedEnergy = 0x09,
|
||
SetTargetedNumberOfSteps = 0x0A,
|
||
SetTargetedNumberOfStrides = 0x0B,
|
||
SetTargetedDistance = 0x0C,
|
||
SetTargetedTrainingTime = 0x0D,
|
||
SetTargetedTimeInTwoHeartRateZones = 0x0E,
|
||
SetTargetedTimeInThreeHeartRateZones = 0x0F,
|
||
SetTargetedTimeInFiveHeartRateZones = 0x10,
|
||
SetIndoorBikeSimulationParameters = 0x11,
|
||
SetWheelCircumference = 0x12,
|
||
SetSpinDownControl = 0x13,
|
||
SetTargetedCadence = 0x14,
|
||
}
|
||
|
||
impl OpCode {
|
||
pub fn from_u8(v: u8) -> Option<Self> {
|
||
use OpCode::*;
|
||
Some(match v {
|
||
0x00 => RequestControl,
|
||
0x01 => Reset,
|
||
0x02 => SetTargetSpeed,
|
||
0x03 => SetTargetInclination,
|
||
0x04 => SetTargetResistanceLevel,
|
||
0x05 => SetTargetPower,
|
||
0x06 => SetTargetHeartRate,
|
||
0x07 => StartOrResume,
|
||
0x08 => StopOrPause,
|
||
0x09 => SetTargetedExpendedEnergy,
|
||
0x0A => SetTargetedNumberOfSteps,
|
||
0x0B => SetTargetedNumberOfStrides,
|
||
0x0C => SetTargetedDistance,
|
||
0x0D => SetTargetedTrainingTime,
|
||
0x0E => SetTargetedTimeInTwoHeartRateZones,
|
||
0x0F => SetTargetedTimeInThreeHeartRateZones,
|
||
0x10 => SetTargetedTimeInFiveHeartRateZones,
|
||
0x11 => SetIndoorBikeSimulationParameters,
|
||
0x12 => SetWheelCircumference,
|
||
0x13 => SetSpinDownControl,
|
||
0x14 => SetTargetedCadence,
|
||
_ => return None,
|
||
})
|
||
}
|
||
|
||
pub fn as_u8(self) -> u8 {
|
||
self as u8
|
||
}
|
||
}
|
||
|
||
impl std::fmt::Display for OpCode {
|
||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||
write!(f, "{self:?} (0x{:02x})", self.as_u8())
|
||
}
|
||
}
|
||
|
||
/// The first byte of every control point indication.
|
||
pub const RESPONSE_CODE: u8 = 0x80;
|
||
|
||
/// Result codes carried in a control point indication.
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||
pub enum ResultCode {
|
||
Success,
|
||
OpCodeNotSupported,
|
||
InvalidParameter,
|
||
OperationFailed,
|
||
ControlNotPermitted,
|
||
/// Reserved or vendor-specific.
|
||
Unknown(u8),
|
||
}
|
||
|
||
impl ResultCode {
|
||
pub fn from_u8(v: u8) -> Self {
|
||
match v {
|
||
0x01 => ResultCode::Success,
|
||
0x02 => ResultCode::OpCodeNotSupported,
|
||
0x03 => ResultCode::InvalidParameter,
|
||
0x04 => ResultCode::OperationFailed,
|
||
0x05 => ResultCode::ControlNotPermitted,
|
||
other => ResultCode::Unknown(other),
|
||
}
|
||
}
|
||
|
||
pub fn as_u8(self) -> u8 {
|
||
match self {
|
||
ResultCode::Success => 0x01,
|
||
ResultCode::OpCodeNotSupported => 0x02,
|
||
ResultCode::InvalidParameter => 0x03,
|
||
ResultCode::OperationFailed => 0x04,
|
||
ResultCode::ControlNotPermitted => 0x05,
|
||
ResultCode::Unknown(v) => v,
|
||
}
|
||
}
|
||
|
||
pub fn is_success(self) -> bool {
|
||
matches!(self, ResultCode::Success)
|
||
}
|
||
}
|
||
|
||
impl std::fmt::Display for ResultCode {
|
||
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
|
||
match self {
|
||
ResultCode::Success => write!(f, "Success"),
|
||
ResultCode::OpCodeNotSupported => write!(f, "Op Code not supported"),
|
||
ResultCode::InvalidParameter => write!(f, "Invalid parameter"),
|
||
ResultCode::OperationFailed => write!(f, "Operation failed"),
|
||
ResultCode::ControlNotPermitted => {
|
||
write!(f, "Control not permitted (RequestControl first)")
|
||
}
|
||
ResultCode::Unknown(v) => write!(f, "Unknown result code 0x{v:02x}"),
|
||
}
|
||
}
|
||
}
|
||
|
||
/// A decoded control point indication.
|
||
#[derive(Debug, Clone, PartialEq, Eq)]
|
||
pub struct ControlPointResponse {
|
||
/// The op code this response refers to, if it is one we know.
|
||
pub request_op_code: Option<OpCode>,
|
||
/// The raw op code byte, even when unrecognised.
|
||
pub raw_request_op_code: u8,
|
||
pub result: ResultCode,
|
||
/// Response parameters, present only for some procedures.
|
||
pub parameters: Vec<u8>,
|
||
}
|
||
|
||
impl ControlPointResponse {
|
||
pub fn is_success(&self) -> bool {
|
||
self.result.is_success()
|
||
}
|
||
}
|
||
|
||
/// Why a control point indication could not be decoded.
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq, thiserror::Error)]
|
||
pub enum ResponseError {
|
||
#[error("control point indication is {len} bytes; at least 3 are required")]
|
||
TooShort { len: usize },
|
||
#[error("control point indication started with 0x{first:02x}, expected 0x80")]
|
||
NotAResponse { first: u8 },
|
||
}
|
||
|
||
/// Decode a control point indication. Pure function; no hardware needed.
|
||
pub fn decode_response(data: &[u8]) -> Result<ControlPointResponse, ResponseError> {
|
||
if data.len() < 3 {
|
||
return Err(ResponseError::TooShort { len: data.len() });
|
||
}
|
||
if data[0] != RESPONSE_CODE {
|
||
return Err(ResponseError::NotAResponse { first: data[0] });
|
||
}
|
||
Ok(ControlPointResponse {
|
||
request_op_code: OpCode::from_u8(data[1]),
|
||
raw_request_op_code: data[1],
|
||
result: ResultCode::from_u8(data[2]),
|
||
parameters: data[3..].to_vec(),
|
||
})
|
||
}
|
||
|
||
// ---------------------------------------------------------------------------
|
||
// Request encoders
|
||
// ---------------------------------------------------------------------------
|
||
|
||
/// `StopOrPause` (`0x08`) takes a one-byte control parameter.
|
||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||
pub enum StopOrPause {
|
||
Stop = 0x01,
|
||
Pause = 0x02,
|
||
}
|
||
|
||
/// Parameters for `SetIndoorBikeSimulationParameters` (`0x11`).
|
||
///
|
||
/// Whether the D100 accepts this at all is open question **A-1** — the MIT
|
||
/// reference drives grade with `SetTargetInclination` (`0x03`) instead. The
|
||
/// `probe set` subcommand exists to resolve it against real hardware.
|
||
#[derive(Debug, Clone, Copy, PartialEq)]
|
||
pub struct SimulationParameters {
|
||
/// Wind speed, m/s. Encoded as sint16 with 0.001 resolution.
|
||
pub wind_speed_mps: f32,
|
||
/// Grade, percent. Encoded as sint16 with 0.01 resolution.
|
||
pub grade_percent: f32,
|
||
/// Coefficient of rolling resistance. Encoded as uint8 with 0.0001
|
||
/// resolution (so the representable range is 0.0000 – 0.0255).
|
||
pub crr: f32,
|
||
/// Wind resistance coefficient, kg/m. Encoded as uint8 with 0.01
|
||
/// resolution (representable range 0.00 – 2.55).
|
||
pub wind_resistance_coefficient: f32,
|
||
}
|
||
|
||
impl Default for SimulationParameters {
|
||
fn default() -> Self {
|
||
Self {
|
||
wind_speed_mps: 0.0,
|
||
grade_percent: 0.0,
|
||
crr: 0.004,
|
||
wind_resistance_coefficient: 0.51,
|
||
}
|
||
}
|
||
}
|
||
|
||
/// `[0x00]` — take control of the machine. Must succeed before any target
|
||
/// setting procedure is accepted (FR-2.1).
|
||
pub fn request_control() -> Vec<u8> {
|
||
vec![OpCode::RequestControl.as_u8()]
|
||
}
|
||
|
||
/// `[0x01]` — reset the machine to its default state. Part of the shutdown
|
||
/// safety sequence (SAF-2).
|
||
pub fn reset() -> Vec<u8> {
|
||
vec![OpCode::Reset.as_u8()]
|
||
}
|
||
|
||
/// `[0x07]` — start or resume.
|
||
pub fn start_or_resume() -> Vec<u8> {
|
||
vec![OpCode::StartOrResume.as_u8()]
|
||
}
|
||
|
||
/// `[0x08, param]` — stop or pause.
|
||
pub fn stop_or_pause(what: StopOrPause) -> Vec<u8> {
|
||
vec![OpCode::StopOrPause.as_u8(), what as u8]
|
||
}
|
||
|
||
/// `[0x03, sint16 LE]` — target inclination in percent, 0.1 resolution.
|
||
///
|
||
/// Values outside the sint16 range after scaling are saturated rather than
|
||
/// wrapped: wrapping a +400% mistake into a large negative grade would be a
|
||
/// safety hazard.
|
||
pub fn set_target_inclination(percent: f32) -> Vec<u8> {
|
||
let raw = scale_to_i16(percent, 10.0);
|
||
let mut v = vec![OpCode::SetTargetInclination.as_u8()];
|
||
v.extend_from_slice(&raw.to_le_bytes());
|
||
v
|
||
}
|
||
|
||
/// `[0x04, sint16 LE]` — target resistance level, in the trainer's own units.
|
||
///
|
||
/// Note: the FTMS specification defines this parameter as a *uint8* with 0.1
|
||
/// resolution, but the working D100 reference implementation (§3.2) sends a
|
||
/// sint16 and the trainer accepts it. We follow the reference, since it is the
|
||
/// only behaviour confirmed on this hardware. **Needs hardware confirmation**
|
||
/// if a different trainer is ever targeted.
|
||
pub fn set_target_resistance(level: i16) -> Vec<u8> {
|
||
let mut v = vec![OpCode::SetTargetResistanceLevel.as_u8()];
|
||
v.extend_from_slice(&level.to_le_bytes());
|
||
v
|
||
}
|
||
|
||
/// `[0x05, sint16 LE]` — target power in watts.
|
||
pub fn set_target_power(watts: i16) -> Vec<u8> {
|
||
let mut v = vec![OpCode::SetTargetPower.as_u8()];
|
||
v.extend_from_slice(&watts.to_le_bytes());
|
||
v
|
||
}
|
||
|
||
/// `[0x11, sint16 wind, sint16 grade, uint8 crr, uint8 cw]` — indoor bike
|
||
/// simulation parameters. See [`SimulationParameters`] and open question A-1.
|
||
pub fn set_simulation_parameters(p: SimulationParameters) -> Vec<u8> {
|
||
let wind = scale_to_i16(p.wind_speed_mps, 1000.0);
|
||
let grade = scale_to_i16(p.grade_percent, 100.0);
|
||
let crr = scale_to_u8(p.crr, 10_000.0);
|
||
let cw = scale_to_u8(p.wind_resistance_coefficient, 100.0);
|
||
|
||
let mut v = vec![OpCode::SetIndoorBikeSimulationParameters.as_u8()];
|
||
v.extend_from_slice(&wind.to_le_bytes());
|
||
v.extend_from_slice(&grade.to_le_bytes());
|
||
v.push(crr);
|
||
v.push(cw);
|
||
v
|
||
}
|
||
|
||
/// Scale a physical value by `factor` and saturate into sint16.
|
||
fn scale_to_i16(value: f32, factor: f32) -> i16 {
|
||
if !value.is_finite() {
|
||
return 0;
|
||
}
|
||
let scaled = (value * factor).round();
|
||
scaled.clamp(i16::MIN as f32, i16::MAX as f32) as i16
|
||
}
|
||
|
||
/// Scale a physical value by `factor` and saturate into uint8.
|
||
fn scale_to_u8(value: f32, factor: f32) -> u8 {
|
||
if !value.is_finite() {
|
||
return 0;
|
||
}
|
||
let scaled = (value * factor).round();
|
||
scaled.clamp(0.0, u8::MAX as f32) as u8
|
||
}
|
||
|
||
#[cfg(test)]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
#[test]
|
||
fn simple_procedures_are_single_bytes() {
|
||
assert_eq!(request_control(), vec![0x00]);
|
||
assert_eq!(reset(), vec![0x01]);
|
||
assert_eq!(start_or_resume(), vec![0x07]);
|
||
assert_eq!(stop_or_pause(StopOrPause::Stop), vec![0x08, 0x01]);
|
||
assert_eq!(stop_or_pause(StopOrPause::Pause), vec![0x08, 0x02]);
|
||
}
|
||
|
||
#[test]
|
||
fn inclination_is_sint16_tenths_of_a_percent_little_endian() {
|
||
// +5.0% -> 50 -> 0x0032
|
||
assert_eq!(set_target_inclination(5.0), vec![0x03, 0x32, 0x00]);
|
||
// 0% -> 0
|
||
assert_eq!(set_target_inclination(0.0), vec![0x03, 0x00, 0x00]);
|
||
// -7.5% -> -75 -> 0xFFB5
|
||
assert_eq!(set_target_inclination(-7.5), vec![0x03, 0xb5, 0xff]);
|
||
// Rounding, not truncation.
|
||
assert_eq!(set_target_inclination(1.26), vec![0x03, 0x0d, 0x00]);
|
||
}
|
||
|
||
#[test]
|
||
fn inclination_saturates_instead_of_wrapping() {
|
||
// 5000% * 10 = 50000, past i16::MAX. Must clamp to +32767, not wrap to
|
||
// a large negative grade.
|
||
assert_eq!(set_target_inclination(5000.0), vec![0x03, 0xff, 0x7f]);
|
||
assert_eq!(set_target_inclination(-5000.0), vec![0x03, 0x00, 0x80]);
|
||
assert_eq!(set_target_inclination(f32::NAN), vec![0x03, 0x00, 0x00]);
|
||
assert_eq!(set_target_inclination(f32::INFINITY), vec![0x03, 0x00, 0x00]);
|
||
}
|
||
|
||
#[test]
|
||
fn resistance_is_sint16_little_endian() {
|
||
assert_eq!(set_target_resistance(0), vec![0x04, 0x00, 0x00]);
|
||
assert_eq!(set_target_resistance(50), vec![0x04, 0x32, 0x00]);
|
||
assert_eq!(set_target_resistance(100), vec![0x04, 0x64, 0x00]);
|
||
assert_eq!(set_target_resistance(-1), vec![0x04, 0xff, 0xff]);
|
||
}
|
||
|
||
#[test]
|
||
fn power_is_sint16_watts_little_endian() {
|
||
assert_eq!(set_target_power(200), vec![0x05, 0xc8, 0x00]);
|
||
assert_eq!(set_target_power(600), vec![0x05, 0x58, 0x02]);
|
||
// Negative target power is nonsense but must still encode as sint16
|
||
// rather than panic; the safety gate is what prevents it being sent.
|
||
assert_eq!(set_target_power(-10), vec![0x05, 0xf6, 0xff]);
|
||
}
|
||
|
||
#[test]
|
||
fn simulation_parameters_layout() {
|
||
let p = SimulationParameters {
|
||
wind_speed_mps: 0.0,
|
||
grade_percent: 4.5, // * 100 = 450 = 0x01C2
|
||
crr: 0.004, // * 10000 = 40 = 0x28
|
||
wind_resistance_coefficient: 0.51, // * 100 = 51 = 0x33
|
||
};
|
||
assert_eq!(
|
||
set_simulation_parameters(p),
|
||
vec![0x11, 0x00, 0x00, 0xc2, 0x01, 0x28, 0x33]
|
||
);
|
||
assert_eq!(set_simulation_parameters(p).len(), 7);
|
||
}
|
||
|
||
#[test]
|
||
fn simulation_parameters_negative_grade_and_wind() {
|
||
let p = SimulationParameters {
|
||
wind_speed_mps: -1.5, // * 1000 = -1500 = 0xFA24
|
||
grade_percent: -8.0, // * 100 = -800 = 0xFCE0
|
||
crr: 0.0,
|
||
wind_resistance_coefficient: 0.0,
|
||
};
|
||
assert_eq!(
|
||
set_simulation_parameters(p),
|
||
vec![0x11, 0x24, 0xfa, 0xe0, 0xfc, 0x00, 0x00]
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn simulation_parameter_bytes_saturate() {
|
||
let p = SimulationParameters {
|
||
wind_speed_mps: 0.0,
|
||
grade_percent: 0.0,
|
||
crr: 1.0, // * 10000 = 10000, way past u8
|
||
wind_resistance_coefficient: -5.0, // negative, clamps to 0
|
||
};
|
||
let b = set_simulation_parameters(p);
|
||
assert_eq!(b[5], 0xff);
|
||
assert_eq!(b[6], 0x00);
|
||
}
|
||
|
||
#[test]
|
||
fn response_decoding() {
|
||
let r = decode_response(&[0x80, 0x05, 0x01]).unwrap();
|
||
assert_eq!(r.request_op_code, Some(OpCode::SetTargetPower));
|
||
assert_eq!(r.result, ResultCode::Success);
|
||
assert!(r.is_success());
|
||
assert!(r.parameters.is_empty());
|
||
}
|
||
|
||
#[test]
|
||
fn response_decoding_reports_every_error_code() {
|
||
for (byte, expect) in [
|
||
(0x02u8, ResultCode::OpCodeNotSupported),
|
||
(0x03, ResultCode::InvalidParameter),
|
||
(0x04, ResultCode::OperationFailed),
|
||
(0x05, ResultCode::ControlNotPermitted),
|
||
(0x77, ResultCode::Unknown(0x77)),
|
||
] {
|
||
let r = decode_response(&[0x80, 0x11, byte]).unwrap();
|
||
assert_eq!(r.result, expect);
|
||
assert!(!r.is_success());
|
||
}
|
||
}
|
||
|
||
/// A-1: this is exactly the byte sequence that tells us the D100 rejects
|
||
/// simulation mode.
|
||
#[test]
|
||
fn sim_mode_rejection_is_recognisable() {
|
||
let r = decode_response(&[0x80, 0x11, 0x02]).unwrap();
|
||
assert_eq!(
|
||
r.request_op_code,
|
||
Some(OpCode::SetIndoorBikeSimulationParameters)
|
||
);
|
||
assert_eq!(r.result, ResultCode::OpCodeNotSupported);
|
||
}
|
||
|
||
#[test]
|
||
fn response_with_parameters() {
|
||
let r = decode_response(&[0x80, 0x13, 0x01, 0xaa, 0xbb]).unwrap();
|
||
assert_eq!(r.parameters, vec![0xaa, 0xbb]);
|
||
}
|
||
|
||
#[test]
|
||
fn unknown_op_code_in_response_is_preserved_not_dropped() {
|
||
let r = decode_response(&[0x80, 0xfe, 0x02]).unwrap();
|
||
assert_eq!(r.request_op_code, None);
|
||
assert_eq!(r.raw_request_op_code, 0xfe);
|
||
}
|
||
|
||
#[test]
|
||
fn malformed_responses_are_errors() {
|
||
assert_eq!(
|
||
decode_response(&[]).unwrap_err(),
|
||
ResponseError::TooShort { len: 0 }
|
||
);
|
||
assert_eq!(
|
||
decode_response(&[0x80, 0x05]).unwrap_err(),
|
||
ResponseError::TooShort { len: 2 }
|
||
);
|
||
assert_eq!(
|
||
decode_response(&[0x01, 0x05, 0x01]).unwrap_err(),
|
||
ResponseError::NotAResponse { first: 0x01 }
|
||
);
|
||
}
|
||
|
||
#[test]
|
||
fn op_code_round_trips() {
|
||
for v in 0x00u8..=0x14 {
|
||
let op = OpCode::from_u8(v).expect("all of 0x00..=0x14 are defined");
|
||
assert_eq!(op.as_u8(), v);
|
||
}
|
||
assert_eq!(OpCode::from_u8(0x15), None);
|
||
assert_eq!(OpCode::from_u8(0xff), None);
|
||
}
|
||
|
||
#[test]
|
||
fn result_code_round_trips() {
|
||
for v in 0x00u8..=0xff {
|
||
assert_eq!(ResultCode::from_u8(v).as_u8(), v);
|
||
}
|
||
}
|
||
}
|