Telemetry Loop Example

This example demonstrates continuous polling of flight controller telemetry data.

Overview

Real-time telemetry requires:

1. Polling multiple MSP commands at a consistent rate
2. Parsing responses efficiently
3. Handling timing and connection issues
4. Processing data for display or logging

Complete Example

use std::io::{Read, Write};
use std::time::{Duration, Instant};
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::Arc;

// MSP Command Codes
const MSP_STATUS: u8 = 101;
const MSP_RAW_IMU: u8 = 102;
const MSP_MOTOR: u8 = 104;
const MSP_RC: u8 = 105;
const MSP_ATTITUDE: u8 = 108;
const MSP_ALTITUDE: u8 = 109;
const MSP_ANALOG: u8 = 110;

/// Telemetry data snapshot
#[derive(Debug, Default, Clone)]
pub struct TelemetryData {
    pub timestamp_ms: u64,
    pub attitude: Option<Attitude>,
    pub analog: Option<Analog>,
    pub rc: Option<RcChannels>,
    pub motors: Option<Motors>,
    pub imu: Option<RawImu>,
}

#[derive(Debug, Clone, Copy)]
pub struct Attitude {
    pub roll: f32,
    pub pitch: f32,
    pub yaw: f32,
}

#[derive(Debug, Clone, Copy)]
pub struct Analog {
    pub voltage: f32,
    pub current: f32,
    pub mah_drawn: u16,
    pub rssi_percent: f32,
}

#[derive(Debug, Clone)]
pub struct RcChannels {
    pub channels: Vec<u16>,
}

#[derive(Debug, Clone, Copy)]
pub struct Motors {
    pub values: [u16; 8],
}

#[derive(Debug, Clone, Copy)]
pub struct RawImu {
    pub acc: [f32; 3],
    pub gyro: [f32; 3],
}

/// Telemetry polling client
pub struct TelemetryClient<T> {
    port: T,
    buffer: [u8; 256],
    start_time: Instant,
}

impl<T: Read + Write> TelemetryClient<T> {
    pub fn new(port: T) -> Self {
        TelemetryClient {
            port,
            buffer: [0u8; 256],
            start_time: Instant::now(),
        }
    }

    /// Build and send MSP request
    fn send_request(&mut self, command: u8) -> Result<(), std::io::Error> {
        let checksum = 0 ^ command;
        let request = [b'$', b'M', b'<', 0, command, checksum];
        self.port.write_all(&request)
    }

    /// Read MSP response
    fn read_response(&mut self) -> Result<(u8, Vec<u8>), String> {
        let mut total = 0;

        // Read header (minimum 6 bytes)
        while total < 6 {
            match self.port.read(&mut self.buffer[total..]) {
                Ok(0) => return Err("Connection closed".to_string()),
                Ok(n) => total += n,
                Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => {
                    std::thread::sleep(Duration::from_micros(100));
                }
                Err(e) => return Err(format!("Read error: {}", e)),
            }
        }

        // Validate header
        if &self.buffer[0..3] != b"$M>" {
            return Err("Invalid header".to_string());
        }

        let size = self.buffer[3] as usize;
        let command = self.buffer[4];

        // Read remaining payload
        let required = 6 + size;
        while total < required {
            match self.port.read(&mut self.buffer[total..]) {
                Ok(0) => return Err("Connection closed".to_string()),
                Ok(n) => total += n,
                Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => {
                    std::thread::sleep(Duration::from_micros(100));
                }
                Err(e) => return Err(format!("Read error: {}", e)),
            }
        }

        let payload = self.buffer[5..5 + size].to_vec();
        Ok((command, payload))
    }

    /// Query single command
    fn query(&mut self, command: u8) -> Result<Vec<u8>, String> {
        self.send_request(command)
            .map_err(|e| format!("Send failed: {}", e))?;
        let (_, payload) = self.read_response()?;
        Ok(payload)
    }

    /// Parse attitude response
    fn parse_attitude(data: &[u8]) -> Option<Attitude> {
        if data.len() < 6 {
            return None;
        }
        Some(Attitude {
            roll: i16::from_le_bytes([data[0], data[1]]) as f32 / 10.0,
            pitch: i16::from_le_bytes([data[2], data[3]]) as f32 / 10.0,
            yaw: i16::from_le_bytes([data[4], data[5]]) as f32,
        })
    }

    /// Parse analog response
    fn parse_analog(data: &[u8]) -> Option<Analog> {
        if data.len() < 9 {
            return None;
        }
        Some(Analog {
            mah_drawn: u16::from_le_bytes([data[1], data[2]]),
            rssi_percent: u16::from_le_bytes([data[3], data[4]]) as f32 / 1023.0 * 100.0,
            current: i16::from_le_bytes([data[5], data[6]]) as f32 / 100.0,
            voltage: u16::from_le_bytes([data[7], data[8]]) as f32 / 100.0,
        })
    }

    /// Parse RC channels response
    fn parse_rc(data: &[u8]) -> Option<RcChannels> {
        let count = data.len() / 2;
        let channels: Vec<u16> = (0..count)
            .map(|i| u16::from_le_bytes([data[i * 2], data[i * 2 + 1]]))
            .collect();
        Some(RcChannels { channels })
    }

    /// Parse motors response
    fn parse_motors(data: &[u8]) -> Option<Motors> {
        if data.len() < 16 {
            return None;
        }
        let mut values = [0u16; 8];
        for i in 0..8 {
            values[i] = u16::from_le_bytes([data[i * 2], data[i * 2 + 1]]);
        }
        Some(Motors { values })
    }

    /// Parse raw IMU response
    fn parse_imu(data: &[u8]) -> Option<RawImu> {
        if data.len() < 18 {
            return None;
        }
        let read_i16 = |off: usize| i16::from_le_bytes([data[off], data[off + 1]]);
        Some(RawImu {
            acc: [
                read_i16(0) as f32 / 512.0,
                read_i16(2) as f32 / 512.0,
                read_i16(4) as f32 / 512.0,
            ],
            gyro: [
                read_i16(6) as f32 / 4.096,
                read_i16(8) as f32 / 4.096,
                read_i16(10) as f32 / 4.096,
            ],
        })
    }

    /// Poll all telemetry once
    pub fn poll(&mut self) -> TelemetryData {
        let timestamp_ms = self.start_time.elapsed().as_millis() as u64;

        let attitude = self.query(MSP_ATTITUDE)
            .ok()
            .and_then(|d| Self::parse_attitude(&d));

        let analog = self.query(MSP_ANALOG)
            .ok()
            .and_then(|d| Self::parse_analog(&d));

        let rc = self.query(MSP_RC)
            .ok()
            .and_then(|d| Self::parse_rc(&d));

        let motors = self.query(MSP_MOTOR)
            .ok()
            .and_then(|d| Self::parse_motors(&d));

        let imu = self.query(MSP_RAW_IMU)
            .ok()
            .and_then(|d| Self::parse_imu(&d));

        TelemetryData {
            timestamp_ms,
            attitude,
            analog,
            rc,
            motors,
            imu,
        }
    }

    /// Run continuous telemetry loop
    pub fn run_loop<F>(
        &mut self,
        rate_hz: u32,
        running: Arc<AtomicBool>,
        mut callback: F,
    ) where
        F: FnMut(&TelemetryData),
    {
        let interval = Duration::from_micros(1_000_000 / rate_hz as u64);

        while running.load(Ordering::Relaxed) {
            let start = Instant::now();

            let data = self.poll();
            callback(&data);

            let elapsed = start.elapsed();
            if elapsed < interval {
                std::thread::sleep(interval - elapsed);
            }
        }
    }
}

/// Format telemetry for display
fn format_telemetry(data: &TelemetryData) -> String {
    let mut output = format!("[{:8}ms] ", data.timestamp_ms);

    if let Some(att) = &data.attitude {
        output.push_str(&format!(
            "R:{:+6.1}° P:{:+6.1}° Y:{:5.0}° ",
            att.roll, att.pitch, att.yaw
        ));
    }

    if let Some(analog) = &data.analog {
        output.push_str(&format!(
            "| {:.2}V {:.1}A {}mAh ",
            analog.voltage, analog.current, analog.mah_drawn
        ));
    }

    if let Some(rc) = &data.rc {
        if rc.channels.len() >= 4 {
            output.push_str(&format!(
                "| T:{:4} ",
                rc.channels[2]
            ));
        }
    }

    output
}

fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Open serial port
    let port = std::fs::OpenOptions::new()
        .read(true)
        .write(true)
        .open("/dev/ttyUSB0")?;

    let mut client = TelemetryClient::new(port);

    // Setup Ctrl+C handler
    let running = Arc::new(AtomicBool::new(true));
    let r = running.clone();
    ctrlc::set_handler(move || {
        r.store(false, Ordering::Relaxed);
    })?;

    println!("Starting telemetry loop at 20Hz (Ctrl+C to stop)...\n");

    // Run telemetry loop
    client.run_loop(20, running, |data| {
        println!("{}", format_telemetry(data));
    });

    println!("\nTelemetry loop stopped.");
    Ok(())
}

Expected Output

Starting telemetry loop at 20Hz (Ctrl+C to stop)...

[       0ms] R:  +0.5° P:  -1.2° Y:   45° | 14.82V 0.5A 0mAh | T:1000
[      50ms] R:  +0.4° P:  -1.1° Y:   45° | 14.81V 0.5A 0mAh | T:1000
[     100ms] R:  +0.6° P:  -1.3° Y:   44° | 14.82V 0.4A 0mAh | T:1000
[     150ms] R:  +0.5° P:  -1.2° Y:   45° | 14.81V 0.5A 1mAh | T:1000
...

Optimized Polling

For higher performance, batch requests:

/// Optimized batch polling
impl<T: Read + Write> TelemetryClient<T> {
    /// Send multiple requests, then read all responses
    pub fn poll_batch(&mut self, commands: &[u8]) -> Vec<(u8, Vec<u8>)> {
        let mut results = Vec::with_capacity(commands.len());

        // Send all requests
        for &cmd in commands {
            if self.send_request(cmd).is_err() {
                break;
            }
        }

        // Read all responses
        for _ in 0..commands.len() {
            match self.read_response() {
                Ok((cmd, payload)) => results.push((cmd, payload)),
                Err(_) => break,
            }
        }

        results
    }

    /// High-frequency minimal polling (attitude only)
    pub fn poll_attitude_fast(&mut self) -> Option<Attitude> {
        self.query(MSP_ATTITUDE)
            .ok()
            .and_then(|d| Self::parse_attitude(&d))
    }
}

Data Logging

use std::fs::File;
use std::io::BufWriter;

/// CSV telemetry logger
pub struct TelemetryLogger {
    writer: BufWriter<File>,
}

impl TelemetryLogger {
    pub fn new(path: &str) -> std::io::Result<Self> {
        let file = File::create(path)?;
        let mut writer = BufWriter::new(file);

        // Write CSV header
        writeln!(
            writer,
            "timestamp_ms,roll,pitch,yaw,voltage,current,mah,throttle"
        )?;

        Ok(TelemetryLogger { writer })
    }

    pub fn log(&mut self, data: &TelemetryData) -> std::io::Result<()> {
        let roll = data.attitude.map(|a| a.roll).unwrap_or(0.0);
        let pitch = data.attitude.map(|a| a.pitch).unwrap_or(0.0);
        let yaw = data.attitude.map(|a| a.yaw).unwrap_or(0.0);
        let voltage = data.analog.map(|a| a.voltage).unwrap_or(0.0);
        let current = data.analog.map(|a| a.current).unwrap_or(0.0);
        let mah = data.analog.map(|a| a.mah_drawn).unwrap_or(0);
        let throttle = data.rc.as_ref()
            .and_then(|rc| rc.channels.get(2).copied())
            .unwrap_or(0);

        writeln!(
            self.writer,
            "{},{:.2},{:.2},{:.1},{:.2},{:.2},{},{}",
            data.timestamp_ms, roll, pitch, yaw, voltage, current, mah, throttle
        )
    }

    pub fn flush(&mut self) -> std::io::Result<()> {
        use std::io::Write;
        self.writer.flush()
    }
}

Rate Recommendations

Use Case	Rate	Commands
Display only	10-20 Hz	ATTITUDE, ANALOG
Ground station	20-50 Hz	All telemetry
Data logging	50-100 Hz	ATTITUDE, IMU
Real-time control	50-100 Hz	ATTITUDE, RC

Error Recovery

/// Telemetry with automatic reconnection
pub struct RobustTelemetry<F>
where
    F: Fn() -> Result<Box<dyn Read + Write>, std::io::Error>,
{
    connect_fn: F,
    client: Option<TelemetryClient<Box<dyn Read + Write>>>,
    consecutive_errors: u32,
}

impl<F> RobustTelemetry<F>
where
    F: Fn() -> Result<Box<dyn Read + Write>, std::io::Error>,
{
    pub fn new(connect_fn: F) -> Self {
        RobustTelemetry {
            connect_fn,
            client: None,
            consecutive_errors: 0,
        }
    }

    pub fn poll(&mut self) -> Option<TelemetryData> {
        // Reconnect if needed
        if self.client.is_none() || self.consecutive_errors > 5 {
            match (self.connect_fn)() {
                Ok(port) => {
                    self.client = Some(TelemetryClient::new(port));
                    self.consecutive_errors = 0;
                }
                Err(_) => {
                    std::thread::sleep(Duration::from_secs(1));
                    return None;
                }
            }
        }

        // Poll telemetry
        if let Some(client) = &mut self.client {
            let data = client.poll();
            if data.attitude.is_some() {
                self.consecutive_errors = 0;
                return Some(data);
            } else {
                self.consecutive_errors += 1;
            }
        }

        None
    }
}