Configuration Write Example

This example demonstrates the proper sequence for reading, modifying, and writing flight controller configuration.

Overview

The safe configuration write sequence:

1. Read current configuration
2. Modify desired values
3. Write updated configuration
4. Verify write succeeded
5. Save to EEPROM
6. Optionally reboot

Complete Example

use std::io::{Read, Write};
use std::time::Duration;

// MSP Command Codes
const MSP_PID: u8 = 112;
const MSP_SET_PID: u8 = 202;
const MSP_RC_TUNING: u8 = 111;
const MSP_SET_RC_TUNING: u8 = 204;
const MSP_FEATURE_CONFIG: u8 = 36;
const MSP_SET_FEATURE_CONFIG: u8 = 37;
const MSP_EEPROM_WRITE: u8 = 250;
const MSP_REBOOT: u8 = 68;

/// PID configuration
#[derive(Debug, Clone)]
pub struct PidConfig {
    pub roll: PidValues,
    pub pitch: PidValues,
    pub yaw: PidValues,
    pub raw_data: Vec<u8>,  // Preserve unknown fields
}

#[derive(Debug, Clone, Copy)]
pub struct PidValues {
    pub p: u8,
    pub i: u8,
    pub d: u8,
}

/// Rate configuration
#[derive(Debug, Clone)]
pub struct RateConfig {
    pub rc_rate: u8,
    pub rc_expo: u8,
    pub roll_rate: u8,
    pub pitch_rate: u8,
    pub yaw_rate: u8,
    pub raw_data: Vec<u8>,
}

/// Configuration manager
pub struct ConfigManager<T> {
    port: T,
    buffer: [u8; 256],
}

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

    /// Build MSP request
    fn build_request(&self, command: u8, payload: &[u8]) -> Vec<u8> {
        let size = payload.len() as u8;
        let mut frame = vec![b'$', b'M', b'<', size, command];
        frame.extend_from_slice(payload);

        let mut checksum = size ^ command;
        for byte in payload {
            checksum ^= byte;
        }
        frame.push(checksum);

        frame
    }

    /// Send request and get response
    fn query(&mut self, command: u8, payload: &[u8]) -> Result<Vec<u8>, String> {
        let request = self.build_request(command, payload);
        self.port.write_all(&request)
            .map_err(|e| format!("Write failed: {}", e))?;

        // Read response
        std::thread::sleep(Duration::from_millis(50));

        let mut total = 0;
        loop {
            match self.port.read(&mut self.buffer[total..]) {
                Ok(0) => break,
                Ok(n) => total += n,
                Err(e) if e.kind() == std::io::ErrorKind::WouldBlock => break,
                Err(e) => return Err(format!("Read failed: {}", e)),
            }
        }

        if total < 6 {
            return Err("Response too short".to_string());
        }

        if &self.buffer[0..3] != b"$M>" {
            if self.buffer[2] == b'!' {
                return Err("Command failed".to_string());
            }
            return Err("Invalid response".to_string());
        }

        let size = self.buffer[3] as usize;
        Ok(self.buffer[5..5 + size].to_vec())
    }

    /// Read current PID configuration
    pub fn read_pid(&mut self) -> Result<PidConfig, String> {
        let data = self.query(MSP_PID, &[])?;

        if data.len() < 9 {
            return Err("PID response too short".to_string());
        }

        Ok(PidConfig {
            roll: PidValues { p: data[0], i: data[1], d: data[2] },
            pitch: PidValues { p: data[3], i: data[4], d: data[5] },
            yaw: PidValues { p: data[6], i: data[7], d: data[8] },
            raw_data: data,
        })
    }

    /// Write PID configuration
    pub fn write_pid(&mut self, config: &PidConfig) -> Result<(), String> {
        let mut payload = config.raw_data.clone();

        // Update the values we changed
        payload[0] = config.roll.p;
        payload[1] = config.roll.i;
        payload[2] = config.roll.d;
        payload[3] = config.pitch.p;
        payload[4] = config.pitch.i;
        payload[5] = config.pitch.d;
        payload[6] = config.yaw.p;
        payload[7] = config.yaw.i;
        payload[8] = config.yaw.d;

        let _ = self.query(MSP_SET_PID, &payload)?;
        Ok(())
    }

    /// Read rate configuration
    pub fn read_rates(&mut self) -> Result<RateConfig, String> {
        let data = self.query(MSP_RC_TUNING, &[])?;

        if data.len() < 10 {
            return Err("Rate response too short".to_string());
        }

        Ok(RateConfig {
            rc_rate: data[0],
            rc_expo: data[1],
            roll_rate: data[2],
            pitch_rate: data[3],
            yaw_rate: data[4],
            raw_data: data,
        })
    }

    /// Write rate configuration
    pub fn write_rates(&mut self, config: &RateConfig) -> Result<(), String> {
        let mut payload = config.raw_data.clone();

        payload[0] = config.rc_rate;
        payload[1] = config.rc_expo;
        payload[2] = config.roll_rate;
        payload[3] = config.pitch_rate;
        payload[4] = config.yaw_rate;

        let _ = self.query(MSP_SET_RC_TUNING, &payload)?;
        Ok(())
    }

    /// Read feature flags
    pub fn read_features(&mut self) -> Result<u32, String> {
        let data = self.query(MSP_FEATURE_CONFIG, &[])?;

        if data.len() < 4 {
            return Err("Feature response too short".to_string());
        }

        Ok(u32::from_le_bytes([data[0], data[1], data[2], data[3]]))
    }

    /// Write feature flags
    pub fn write_features(&mut self, features: u32) -> Result<(), String> {
        let payload = features.to_le_bytes();
        let _ = self.query(MSP_SET_FEATURE_CONFIG, &payload)?;
        Ok(())
    }

    /// Save configuration to EEPROM
    pub fn save_to_eeprom(&mut self) -> Result<(), String> {
        let _ = self.query(MSP_EEPROM_WRITE, &[])?;
        // EEPROM write takes some time
        std::thread::sleep(Duration::from_millis(500));
        Ok(())
    }

    /// Reboot flight controller
    pub fn reboot(&mut self) -> Result<(), String> {
        let request = self.build_request(MSP_REBOOT, &[]);
        self.port.write_all(&request)
            .map_err(|e| format!("Write failed: {}", e))?;
        Ok(())
    }
}

/// Feature flags
pub mod features {
    pub const RX_MSP: u32 = 1 << 14;
    pub const LED_STRIP: u32 = 1 << 16;
    pub const OSD: u32 = 1 << 18;
    pub const AIRMODE: u32 = 1 << 22;
    pub const ANTI_GRAVITY: u32 = 1 << 28;
}

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 config = ConfigManager::new(port);

    println!("=== PID Configuration ===\n");

    // Step 1: Read current PID values
    println!("Reading current PID values...");
    let mut pid = config.read_pid()?;
    println!("  Roll:  P={} I={} D={}", pid.roll.p, pid.roll.i, pid.roll.d);
    println!("  Pitch: P={} I={} D={}", pid.pitch.p, pid.pitch.i, pid.pitch.d);
    println!("  Yaw:   P={} I={} D={}", pid.yaw.p, pid.yaw.i, pid.yaw.d);

    // Step 2: Modify values
    println!("\nModifying PID values...");
    pid.roll.p = 45;
    pid.roll.i = 40;
    pid.roll.d = 25;
    pid.pitch.p = 47;
    pid.pitch.i = 42;
    pid.pitch.d = 27;

    // Step 3: Write new values
    println!("Writing new PID values...");
    config.write_pid(&pid)?;

    // Step 4: Verify write
    println!("Verifying...");
    let verify = config.read_pid()?;
    if verify.roll.p != pid.roll.p || verify.pitch.p != pid.pitch.p {
        return Err("Verification failed!".into());
    }
    println!("  Verification successful!");

    // Step 5: Save to EEPROM
    println!("\nSaving to EEPROM...");
    config.save_to_eeprom()?;
    println!("  Configuration saved!");

    println!("\n=== Feature Configuration ===\n");

    // Read features
    println!("Reading features...");
    let mut feat = config.read_features()?;
    println!("  Current features: 0x{:08X}", feat);
    println!("  AIRMODE: {}", if feat & features::AIRMODE != 0 { "ON" } else { "OFF" });
    println!("  OSD: {}", if feat & features::OSD != 0 { "ON" } else { "OFF" });

    // Enable AIRMODE if not already
    if feat & features::AIRMODE == 0 {
        println!("\nEnabling AIRMODE...");
        feat |= features::AIRMODE;
        config.write_features(feat)?;
        config.save_to_eeprom()?;
        println!("  AIRMODE enabled and saved!");
    }

    println!("\n=== Configuration Complete ===");
    println!("\nReboot required for some changes to take effect.");
    println!("Reboot now? (y/n)");

    let mut input = String::new();
    std::io::stdin().read_line(&mut input)?;

    if input.trim().to_lowercase() == "y" {
        println!("Rebooting...");
        config.reboot()?;
        println!("Reboot command sent.");
    }

    Ok(())
}

Expected Output

=== PID Configuration ===

Reading current PID values...
  Roll:  P=42 I=40 D=20
  Pitch: P=46 I=45 D=22
  Yaw:   P=80 I=45 D=0

Modifying PID values...
Writing new PID values...
Verifying...
  Verification successful!

Saving to EEPROM...
  Configuration saved!

=== Feature Configuration ===

Reading features...
  Current features: 0x20440000
  AIRMODE: ON
  OSD: ON

=== Configuration Complete ===

Reboot required for some changes to take effect.
Reboot now? (y/n)

Backup and Restore

use std::fs::File;
use std::io::{BufReader, BufWriter};

/// Configuration backup
#[derive(Debug, serde::Serialize, serde::Deserialize)]
pub struct ConfigBackup {
    pub pid: Vec<u8>,
    pub rates: Vec<u8>,
    pub features: u32,
    pub timestamp: String,
}

impl<T: Read + Write> ConfigManager<T> {
    /// Create backup of current configuration
    pub fn backup(&mut self) -> Result<ConfigBackup, String> {
        let pid = self.query(MSP_PID, &[])?;
        let rates = self.query(MSP_RC_TUNING, &[])?;
        let features = self.read_features()?;

        Ok(ConfigBackup {
            pid,
            rates,
            features,
            timestamp: chrono::Utc::now().to_rfc3339(),
        })
    }

    /// Restore configuration from backup
    pub fn restore(&mut self, backup: &ConfigBackup) -> Result<(), String> {
        // Restore PID
        self.query(MSP_SET_PID, &backup.pid)?;

        // Restore rates
        self.query(MSP_SET_RC_TUNING, &backup.rates)?;

        // Restore features
        self.write_features(backup.features)?;

        // Save to EEPROM
        self.save_to_eeprom()?;

        Ok(())
    }
}

/// Save backup to file
pub fn save_backup(backup: &ConfigBackup, path: &str) -> std::io::Result<()> {
    let file = File::create(path)?;
    let writer = BufWriter::new(file);
    serde_json::to_writer_pretty(writer, backup)?;
    Ok(())
}

/// Load backup from file
pub fn load_backup(path: &str) -> std::io::Result<ConfigBackup> {
    let file = File::open(path)?;
    let reader = BufReader::new(file);
    let backup = serde_json::from_reader(reader)?;
    Ok(backup)
}

Batch Configuration

/// Apply multiple configuration changes atomically
pub fn apply_tuning_preset<T: Read + Write>(
    config: &mut ConfigManager<T>,
    preset: &TuningPreset,
) -> Result<(), String> {
    println!("Applying preset: {}", preset.name);

    // Read current config
    let mut pid = config.read_pid()?;
    let mut rates = config.read_rates()?;

    // Apply preset
    pid.roll = preset.roll_pid;
    pid.pitch = preset.pitch_pid;
    pid.yaw = preset.yaw_pid;

    rates.roll_rate = preset.roll_rate;
    rates.pitch_rate = preset.pitch_rate;
    rates.yaw_rate = preset.yaw_rate;
    rates.rc_expo = preset.expo;

    // Write all changes
    config.write_pid(&pid)?;
    config.write_rates(&rates)?;

    // Save once at the end
    config.save_to_eeprom()?;

    println!("Preset applied successfully!");
    Ok(())
}

/// Tuning preset definition
pub struct TuningPreset {
    pub name: String,
    pub roll_pid: PidValues,
    pub pitch_pid: PidValues,
    pub yaw_pid: PidValues,
    pub roll_rate: u8,
    pub pitch_rate: u8,
    pub yaw_rate: u8,
    pub expo: u8,
}

/// Preset: Freestyle
pub fn freestyle_preset() -> TuningPreset {
    TuningPreset {
        name: "Freestyle".to_string(),
        roll_pid: PidValues { p: 45, i: 40, d: 25 },
        pitch_pid: PidValues { p: 47, i: 42, d: 27 },
        yaw_pid: PidValues { p: 85, i: 45, d: 0 },
        roll_rate: 70,
        pitch_rate: 70,
        yaw_rate: 65,
        expo: 20,
    }
}

/// Preset: Racing
pub fn racing_preset() -> TuningPreset {
    TuningPreset {
        name: "Racing".to_string(),
        roll_pid: PidValues { p: 50, i: 50, d: 28 },
        pitch_pid: PidValues { p: 52, i: 52, d: 30 },
        yaw_pid: PidValues { p: 90, i: 50, d: 0 },
        roll_rate: 80,
        pitch_rate: 80,
        yaw_rate: 70,
        expo: 15,
    }
}

Safety Considerations

1. Always verify writes - Read back after writing to confirm.

2. Use EEPROM sparingly - EEPROM has limited write cycles (~100,000).

3. Backup before changes - Save current config before modifications.

4. Test incrementally - Make small changes and test between each.

5. Keep FC disarmed - Never write configuration while armed.

6. Reboot when required - Some settings require reboot to take effect:

   • Feature flags
   • Serial port configuration
   • Motor protocol changes