moteus_protocol/
query.rs

1// Copyright 2026 mjbots Robotic Systems, LLC.  info@mjbots.com
2//
3// Licensed under the Apache License, Version 2.0 (the "License");
4// you may not use this file except in compliance with the License.
5// You may obtain a copy of the License at
6//
7//     http://www.apache.org/licenses/LICENSE-2.0
8//
9// Unless required by applicable law or agreed to in writing, software
10// distributed under the License is distributed on an "AS IS" BASIS,
11// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12// See the License for the specific language governing permissions and
13// limitations under the License.
14
15//! Query types for reading telemetry from moteus controllers.
16//!
17//! Queries request register values from the controller and parse the response.
18
19use crate::frame::CanFdFrame;
20use crate::mode::{HomeState, Mode};
21use crate::multiplex::{parse_frame, Subframe, Value, WriteCanData, WriteCombiner};
22use crate::register::Register;
23use crate::resolution::Resolution;
24use crate::scaling;
25
26/// Maximum number of extra (custom) registers that can be queried.
27pub const MAX_EXTRA: usize = 16;
28
29/// Format configuration for queries.
30///
31/// Specifies which registers to query and at what resolution.
32/// Setting a field to `Resolution::Ignore` means that register will not be queried.
33///
34/// # Examples
35///
36/// ```
37/// use moteus_protocol::Resolution;
38/// use moteus_protocol::query::QueryFormat;
39///
40/// // Default format queries position, velocity, torque, and q_current
41/// let default = QueryFormat::default();
42///
43/// // Comprehensive format includes all standard registers
44/// let full = QueryFormat::comprehensive();
45///
46/// // Or customize individual registers
47/// let mut custom = QueryFormat::default();
48/// custom.abs_position = Resolution::Float;
49/// custom.voltage = Resolution::Int16;
50/// ```
51#[non_exhaustive]
52#[derive(Debug, Clone)]
53pub struct QueryFormat {
54    pub mode: Resolution,
55    pub position: Resolution,
56    pub velocity: Resolution,
57    pub torque: Resolution,
58    pub q_current: Resolution,
59    pub d_current: Resolution,
60    pub abs_position: Resolution,
61    pub power: Resolution,
62    pub motor_temperature: Resolution,
63    pub trajectory_complete: Resolution,
64    pub home_state: Resolution,
65    pub voltage: Resolution,
66    pub temperature: Resolution,
67    pub fault: Resolution,
68
69    pub aux1_gpio: Resolution,
70    pub aux2_gpio: Resolution,
71
72    pub aux1_pwm_input_period_us: Resolution,
73    pub aux1_pwm_input_duty_cycle: Resolution,
74    pub aux2_pwm_input_period_us: Resolution,
75    pub aux2_pwm_input_duty_cycle: Resolution,
76
77    /// Extra registers to query, as (register_number, resolution) pairs.
78    /// Entries with `Resolution::Ignore` are unused. Active entries must be
79    /// sorted by register number (maintained by `add_extra`).
80    pub extra: [(u16, Resolution); MAX_EXTRA],
81}
82
83impl Default for QueryFormat {
84    fn default() -> Self {
85        QueryFormat {
86            mode: Resolution::Int8,
87            position: Resolution::Float,
88            velocity: Resolution::Float,
89            torque: Resolution::Float,
90            q_current: Resolution::Ignore,
91            d_current: Resolution::Ignore,
92            abs_position: Resolution::Ignore,
93            power: Resolution::Ignore,
94            motor_temperature: Resolution::Ignore,
95            trajectory_complete: Resolution::Ignore,
96            home_state: Resolution::Ignore,
97            voltage: Resolution::Int8,
98            temperature: Resolution::Int8,
99            fault: Resolution::Int8,
100            aux1_gpio: Resolution::Ignore,
101            aux2_gpio: Resolution::Ignore,
102            aux1_pwm_input_period_us: Resolution::Ignore,
103            aux1_pwm_input_duty_cycle: Resolution::Ignore,
104            aux2_pwm_input_period_us: Resolution::Ignore,
105            aux2_pwm_input_duty_cycle: Resolution::Ignore,
106            extra: [(u16::MAX, Resolution::Ignore); MAX_EXTRA],
107        }
108    }
109}
110
111impl QueryFormat {
112    /// Creates a minimal query format (mode, position, velocity, torque only).
113    pub fn minimal() -> Self {
114        QueryFormat {
115            voltage: Resolution::Ignore,
116            temperature: Resolution::Ignore,
117            fault: Resolution::Ignore,
118            ..Default::default()
119        }
120    }
121
122    /// Creates a comprehensive query format with all common fields.
123    pub fn comprehensive() -> Self {
124        QueryFormat {
125            q_current: Resolution::Float,
126            d_current: Resolution::Float,
127            abs_position: Resolution::Float,
128            power: Resolution::Float,
129            motor_temperature: Resolution::Float,
130            trajectory_complete: Resolution::Int8,
131            home_state: Resolution::Int8,
132            ..Default::default()
133        }
134    }
135
136    /// Returns the number of active extra register entries.
137    pub fn extra_count(&self) -> usize {
138        self.extra
139            .iter()
140            .filter(|(_, r)| *r != Resolution::Ignore)
141            .count()
142    }
143
144    /// Adds an extra register to query.
145    pub fn add_extra(&mut self, register: u16, resolution: Resolution) {
146        // Find the first unused slot
147        let slot = self
148            .extra
149            .iter()
150            .position(|(_, r)| *r == Resolution::Ignore);
151        if let Some(idx) = slot {
152            self.extra[idx] = (register, resolution);
153            let count = idx + 1;
154            // Sort active entries by register number using insertion sort
155            for i in (1..count).rev() {
156                if self.extra[i].0 < self.extra[i - 1].0 {
157                    self.extra.swap(i, i - 1);
158                } else {
159                    break;
160                }
161            }
162        }
163    }
164
165    /// Serializes the query to a CAN frame and returns expected reply size.
166    pub fn serialize(&self, frame: &mut CanFdFrame) -> u8 {
167        let mut writer = WriteCanData::new(frame);
168        let mut reply_size: u8 = 0;
169
170        // First block: Mode through Power (registers 0x000-0x007)
171        {
172            let resolutions = [
173                self.mode,
174                self.position,
175                self.velocity,
176                self.torque,
177                self.q_current,
178                self.d_current,
179                self.abs_position,
180                self.power,
181            ];
182            let mut combiner = WriteCombiner::new(
183                0x10, // Read base
184                Register::Mode.address(),
185                &resolutions,
186            );
187            for _ in 0..resolutions.len() {
188                combiner.maybe_write(&mut writer);
189            }
190            reply_size += combiner.reply_size();
191        }
192
193        // Second block: MotorTemperature through Fault (registers 0x00a-0x00f)
194        {
195            let resolutions = [
196                self.motor_temperature,
197                self.trajectory_complete,
198                self.home_state,
199                self.voltage,
200                self.temperature,
201                self.fault,
202            ];
203            let mut combiner =
204                WriteCombiner::new(0x10, Register::MotorTemperature.address(), &resolutions);
205            for _ in 0..resolutions.len() {
206                combiner.maybe_write(&mut writer);
207            }
208            reply_size += combiner.reply_size();
209        }
210
211        // Third block: GPIO status (registers 0x05e-0x05f)
212        {
213            let resolutions = [self.aux1_gpio, self.aux2_gpio];
214            let mut combiner =
215                WriteCombiner::new(0x10, Register::Aux1GpioStatus.address(), &resolutions);
216            for _ in 0..resolutions.len() {
217                combiner.maybe_write(&mut writer);
218            }
219            reply_size += combiner.reply_size();
220        }
221
222        // Fourth block: PWM input (registers 0x072-0x075)
223        {
224            let resolutions = [
225                self.aux1_pwm_input_period_us,
226                self.aux1_pwm_input_duty_cycle,
227                self.aux2_pwm_input_period_us,
228                self.aux2_pwm_input_duty_cycle,
229            ];
230            let mut combiner =
231                WriteCombiner::new(0x10, Register::Aux1PwmInputPeriod.address(), &resolutions);
232            for _ in 0..resolutions.len() {
233                combiner.maybe_write(&mut writer);
234            }
235            reply_size += combiner.reply_size();
236        }
237
238        // Extra registers — group nearby registers into spans of up to
239        // MAX_GROUP_SPAN so the resolution array stays on the stack.
240        let extra_count = self.extra_count();
241        if extra_count > 0 {
242            const MAX_GROUP_SPAN: usize = 64;
243            let mut group_start = 0;
244
245            while group_start < extra_count {
246                let base_reg = self.extra[group_start].0;
247                let mut group_end = group_start + 1;
248
249                while group_end < extra_count
250                    && (self.extra[group_end].0 - base_reg) < MAX_GROUP_SPAN as u16
251                {
252                    group_end += 1;
253                }
254
255                let last_reg = self.extra[group_end - 1].0;
256                let span = (last_reg - base_reg + 1) as usize;
257
258                let mut resolutions = [Resolution::Ignore; MAX_GROUP_SPAN];
259                for i in group_start..group_end {
260                    let (reg, res) = self.extra[i];
261                    resolutions[(reg - base_reg) as usize] = res;
262                }
263
264                let mut combiner = WriteCombiner::new(0x10, base_reg, &resolutions[..span]);
265                for _ in 0..span {
266                    combiner.maybe_write(&mut writer);
267                }
268                reply_size += combiner.reply_size();
269
270                group_start = group_end;
271            }
272        }
273
274        reply_size
275    }
276}
277
278/// Extra register value pair.
279#[derive(Debug, Clone, Copy, Default)]
280pub struct ExtraValue {
281    /// Register number
282    pub register: u16,
283    /// Register value
284    pub value: f32,
285}
286
287/// Result of parsing a query response.
288///
289/// # Examples
290///
291/// ```
292/// use moteus_protocol::CanFdFrame;
293/// use moteus_protocol::query::QueryResult;
294///
295/// // After receiving a response frame from the device...
296/// fn handle_response(frame: &CanFdFrame) {
297///     let result = QueryResult::parse(frame);
298///     if result.mode.is_error() {
299///         eprintln!("Fault code: {}", result.fault);
300///     }
301/// }
302/// ```
303#[non_exhaustive]
304#[derive(Debug, Clone, Default)]
305pub struct QueryResult {
306    pub mode: Mode,
307    pub position: f32,
308    pub velocity: f32,
309    pub torque: f32,
310    pub q_current: f32,
311    pub d_current: f32,
312    pub abs_position: f32,
313    pub power: f32,
314    pub motor_temperature: f32,
315    pub trajectory_complete: bool,
316    pub home_state: HomeState,
317    pub voltage: f32,
318    pub temperature: f32,
319    pub fault: i8,
320
321    pub aux1_gpio: i8,
322    pub aux2_gpio: i8,
323
324    pub aux1_pwm_input_period_us: i32,
325    pub aux1_pwm_input_duty_cycle: f32,
326    pub aux2_pwm_input_period_us: i32,
327    pub aux2_pwm_input_duty_cycle: f32,
328
329    /// Extra register values. `None` entries are unused.
330    pub extra: [Option<ExtraValue>; MAX_EXTRA],
331}
332
333impl QueryResult {
334    /// Creates a new QueryResult with NaN values.
335    pub fn new() -> Self {
336        QueryResult {
337            mode: Mode::Stopped,
338            position: f32::NAN,
339            velocity: f32::NAN,
340            torque: f32::NAN,
341            q_current: f32::NAN,
342            d_current: f32::NAN,
343            abs_position: f32::NAN,
344            power: f32::NAN,
345            motor_temperature: f32::NAN,
346            trajectory_complete: false,
347            home_state: HomeState::Relative,
348            voltage: f32::NAN,
349            temperature: f32::NAN,
350            fault: 0,
351            aux1_gpio: 0,
352            aux2_gpio: 0,
353            aux1_pwm_input_period_us: 0,
354            aux1_pwm_input_duty_cycle: 0.0,
355            aux2_pwm_input_period_us: 0,
356            aux2_pwm_input_duty_cycle: 0.0,
357            extra: [None; MAX_EXTRA],
358        }
359    }
360
361    /// Parses a query response from a CAN frame.
362    pub fn parse(frame: &CanFdFrame) -> Self {
363        Self::parse_data(&frame.data[..frame.size as usize])
364    }
365
366    /// Parses a query response from raw bytes.
367    pub fn parse_data(data: &[u8]) -> Self {
368        let mut result = QueryResult::new();
369
370        for subframe in parse_frame(data) {
371            let (register, value) = match subframe {
372                Subframe::Register {
373                    register,
374                    value: Some(value),
375                    ..
376                } => (register, value),
377                _ => continue,
378            };
379
380            match register {
381                r if r == Register::Mode.address() => {
382                    result.mode = Mode::try_from(value.to_i32() as u8).unwrap_or(Mode::Stopped);
383                }
384                r if r == Register::Position.address() => {
385                    result.position = value.to_f32(&scaling::POSITION);
386                }
387                r if r == Register::Velocity.address() => {
388                    result.velocity = value.to_f32(&scaling::VELOCITY);
389                }
390                r if r == Register::Torque.address() => {
391                    result.torque = value.to_f32(&scaling::TORQUE);
392                }
393                r if r == Register::QCurrent.address() => {
394                    result.q_current = value.to_f32(&scaling::CURRENT);
395                }
396                r if r == Register::DCurrent.address() => {
397                    result.d_current = value.to_f32(&scaling::CURRENT);
398                }
399                r if r == Register::AbsPosition.address() => {
400                    result.abs_position = value.to_f32(&scaling::POSITION);
401                }
402                r if r == Register::Power.address() => {
403                    result.power = value.to_f32(&scaling::POWER);
404                }
405                r if r == Register::MotorTemperature.address() => {
406                    result.motor_temperature = value.to_f32(&scaling::TEMPERATURE);
407                }
408                r if r == Register::TrajectoryComplete.address() => {
409                    result.trajectory_complete = value.to_i32() != 0;
410                }
411                r if r == Register::HomeState.address() => {
412                    result.home_state =
413                        HomeState::try_from(value.to_i32() as u8).unwrap_or(HomeState::Relative);
414                }
415                r if r == Register::Voltage.address() => {
416                    result.voltage = value.to_f32(&scaling::VOLTAGE);
417                }
418                r if r == Register::Temperature.address() => {
419                    result.temperature = value.to_f32(&scaling::TEMPERATURE);
420                }
421                r if r == Register::Fault.address() => {
422                    result.fault = value.to_i32() as i8;
423                }
424                r if r == Register::Aux1GpioStatus.address() => {
425                    result.aux1_gpio = value.to_i32() as i8;
426                }
427                r if r == Register::Aux2GpioStatus.address() => {
428                    result.aux2_gpio = value.to_i32() as i8;
429                }
430                r if r == Register::Aux1PwmInputPeriod.address() => {
431                    result.aux1_pwm_input_period_us = value.to_i32();
432                }
433                r if r == Register::Aux1PwmInputDutyCycle.address() => {
434                    result.aux1_pwm_input_duty_cycle = value.to_f32(&scaling::PWM);
435                }
436                r if r == Register::Aux2PwmInputPeriod.address() => {
437                    result.aux2_pwm_input_period_us = value.to_i32();
438                }
439                r if r == Register::Aux2PwmInputDutyCycle.address() => {
440                    result.aux2_pwm_input_duty_cycle = value.to_f32(&scaling::PWM);
441                }
442                _ => {
443                    if let Some(slot) = result.extra.iter().position(|e| e.is_none()) {
444                        result.extra[slot] = Some(ExtraValue {
445                            register,
446                            value: parse_generic(register, value),
447                        });
448                    } else {
449                        debug_assert!(
450                            false,
451                            "MAX_EXTRA ({}) exceeded, register 0x{:x} dropped",
452                            MAX_EXTRA, register
453                        );
454                    }
455                }
456            }
457        }
458
459        result
460    }
461
462    /// Gets an extra value by register number.
463    pub fn get_extra(&self, register: u16) -> Option<f32> {
464        for entry in &self.extra {
465            match entry {
466                Some(ev) if ev.register == register => return Some(ev.value),
467                None => return None,
468                _ => {}
469            }
470        }
471        None
472    }
473}
474
475/// Parses a generic register value based on register number.
476fn parse_generic(register: u16, value: Value) -> f32 {
477    use crate::scaling;
478
479    // Determine scaling based on register
480    let reg = Register::from_address(register);
481
482    let scaling = match reg {
483        Some(Register::Position)
484        | Some(Register::AbsPosition)
485        | Some(Register::CommandPosition)
486        | Some(Register::CommandStopPosition)
487        | Some(Register::CommandStayWithinLowerBound)
488        | Some(Register::CommandStayWithinUpperBound)
489        | Some(Register::ControlPosition)
490        | Some(Register::ControlPositionError)
491        | Some(Register::Encoder0Position)
492        | Some(Register::Encoder1Position)
493        | Some(Register::Encoder2Position) => &scaling::POSITION,
494
495        Some(Register::Velocity)
496        | Some(Register::CommandVelocity)
497        | Some(Register::CommandVelocityLimit)
498        | Some(Register::ControlVelocity)
499        | Some(Register::ControlVelocityError)
500        | Some(Register::Encoder0Velocity)
501        | Some(Register::Encoder1Velocity)
502        | Some(Register::Encoder2Velocity) => &scaling::VELOCITY,
503
504        Some(Register::Torque)
505        | Some(Register::CommandFeedforwardTorque)
506        | Some(Register::CommandPositionMaxTorque)
507        | Some(Register::ControlTorque)
508        | Some(Register::ControlTorqueError)
509        | Some(Register::PositionKp)
510        | Some(Register::PositionKi)
511        | Some(Register::PositionKd)
512        | Some(Register::PositionFeedforward)
513        | Some(Register::PositionCommand) => &scaling::TORQUE,
514
515        Some(Register::QCurrent)
516        | Some(Register::DCurrent)
517        | Some(Register::CommandQCurrent)
518        | Some(Register::CommandDCurrent)
519        | Some(Register::CommandFixedCurrentOverride) => &scaling::CURRENT,
520
521        Some(Register::Voltage)
522        | Some(Register::VoltagePhaseA)
523        | Some(Register::VoltagePhaseB)
524        | Some(Register::VoltagePhaseC)
525        | Some(Register::VFocVoltage)
526        | Some(Register::VoltageDqD)
527        | Some(Register::VoltageDqQ)
528        | Some(Register::CommandFixedVoltageOverride) => &scaling::VOLTAGE,
529
530        Some(Register::Temperature) | Some(Register::MotorTemperature) => &scaling::TEMPERATURE,
531
532        Some(Register::Power) => &scaling::POWER,
533
534        Some(Register::CommandTimeout) | Some(Register::CommandStayWithinTimeout) => &scaling::TIME,
535
536        Some(Register::CommandAccelLimit) => &scaling::ACCELERATION,
537
538        Some(Register::CommandKpScale)
539        | Some(Register::CommandKdScale)
540        | Some(Register::CommandIlimitScale)
541        | Some(Register::PwmPhaseA)
542        | Some(Register::PwmPhaseB)
543        | Some(Register::PwmPhaseC)
544        | Some(Register::Aux1PwmInputDutyCycle)
545        | Some(Register::Aux2PwmInputDutyCycle) => &scaling::PWM,
546
547        _ => &scaling::INT,
548    };
549
550    value.to_f32(scaling)
551}
552
553#[cfg(test)]
554mod tests {
555    use super::*;
556
557    #[test]
558    fn test_query_format_default() {
559        let format = QueryFormat::default();
560        assert_eq!(format.mode, Resolution::Int8);
561        assert_eq!(format.position, Resolution::Float);
562        assert_eq!(format.q_current, Resolution::Ignore);
563    }
564
565    fn bytes(frame: &CanFdFrame) -> &[u8] {
566        &frame.data[..frame.size as usize]
567    }
568
569    #[test]
570    fn test_query_format_serialize() {
571        let format = QueryFormat::default();
572        let mut frame = CanFdFrame::new();
573        let reply_size = format.serialize(&mut frame);
574
575        assert_eq!(reply_size, 22);
576        assert_eq!(bytes(&frame), &[0x11, 0x00, 0x1f, 0x01, 0x13, 0x0d]);
577    }
578
579    #[test]
580    fn test_query_format_serialize_with_extras() {
581        let mut format = QueryFormat::minimal();
582        format.add_extra(0x100, Resolution::Float);
583        format.add_extra(0x102, Resolution::Int16);
584        let mut frame = CanFdFrame::new();
585        let reply_size = format.serialize(&mut frame);
586
587        assert_eq!(reply_size, 29);
588        assert_eq!(
589            bytes(&frame),
590            &[0x11, 0x00, 0x1f, 0x01, 0x1d, 0x80, 0x02, 0x15, 0x82, 0x02]
591        );
592    }
593
594    #[test]
595    fn test_query_format_serialize_with_distant_extras() {
596        // Extras spanning more than 64 registers get separate groups.
597        let mut format = QueryFormat::minimal();
598        format.add_extra(0x010, Resolution::Int32);
599        format.add_extra(0x100, Resolution::Float);
600        let mut frame = CanFdFrame::new();
601        let reply_size = format.serialize(&mut frame);
602
603        assert_eq!(reply_size, 30);
604        assert_eq!(
605            bytes(&frame),
606            &[0x11, 0x00, 0x1f, 0x01, 0x19, 0x10, 0x1d, 0x80, 0x02]
607        );
608    }
609
610    #[test]
611    fn test_query_result_parse() {
612        // Build a simple reply: mode=10, position=0.5
613        let data = [
614            0x21, // Reply Int8, count=1
615            0x00, // Register 0 (MODE)
616            0x0a, // Mode::Position (10)
617            0x2d, // Reply Float, count=1
618            0x01, // Register 1 (POSITION)
619            0x00, 0x00, 0x00, 0x3f, // 0.5 as f32
620        ];
621
622        let result = QueryResult::parse_data(&data);
623        assert_eq!(result.mode, Mode::Position);
624        assert!((result.position - 0.5).abs() < 0.001);
625    }
626
627    #[test]
628    fn test_query_result_parse_extras() {
629        // Reply with mode + two extra registers (0x100 as Float, 0x102 as Int16)
630        let data = [
631            0x21, // Reply Int8, count=1
632            0x00, // Register 0 (MODE)
633            0x0a, // Mode::Position (10)
634            0x2d, // Reply Float, count=1
635            0x80, 0x02, // Register 0x100 (varuint)
636            0x00, 0x00, 0xc8, 0x42, // 100.0 as f32
637            0x25, // Reply Int16, count=1
638            0x82, 0x02, // Register 0x102 (varuint)
639            0x39, 0x30, // 12345 as i16
640        ];
641
642        let result = QueryResult::parse_data(&data);
643        assert_eq!(result.mode, Mode::Position);
644        assert!(result.extra[0].is_some());
645        let e0 = result.extra[0].unwrap();
646        assert_eq!(e0.register, 0x100);
647        assert!((e0.value - 100.0).abs() < 0.001);
648        assert!(result.extra[1].is_some());
649        let e1 = result.extra[1].unwrap();
650        assert_eq!(e1.register, 0x102);
651        assert_eq!(e1.value as i32, 12345);
652        assert!(result.extra[2].is_none());
653
654        assert!((result.get_extra(0x100).unwrap() - 100.0).abs() < 0.001);
655        assert_eq!(result.get_extra(0x102).unwrap() as i32, 12345);
656        assert!(result.get_extra(0x999).is_none());
657    }
658}
moteus_protocol/query.rs

moteus_protocol/
query.rs
