From b350f20a3685ff39d049ae6cb3558eecdd6b13f8 Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Wed, 24 Jun 2026 13:41:40 +0200 Subject: [PATCH 1/4] Made the written lh data persist in permanent storage. --- src/modules/lighthouse.rs | 90 +++++++++++++++++++++++++-------------- 1 file changed, 59 insertions(+), 31 deletions(-) diff --git a/src/modules/lighthouse.rs b/src/modules/lighthouse.rs index 39b5497..9048931 100644 --- a/src/modules/lighthouse.rs +++ b/src/modules/lighthouse.rs @@ -9,7 +9,7 @@ use crazyflie_lib::{ LighthouseBsCalibration, LighthouseBsGeometry, LighthouseCalibrationSweep, LighthouseMemory, MemoryType, }, - Crazyflie, + Crazyflie, Error, }; use serde::{Deserialize, Serialize}; use std::collections::HashMap; @@ -374,47 +374,75 @@ pub async fn write( None => bail!("Failed to open lighthouse memory"), }; + let num_bs = LighthouseMemory::MAX_BASE_STATIONS as u8; + // Upload geometry data - if !config.geos.is_empty() { - let geometries: HashMap = config + let progress_bar = make_progress(num_bs as usize, "Geometry", non_interactive); + for id in 0..num_bs { + let geo = config .geos - .iter() - .map(|(&id, entry)| (id, LighthouseBsGeometry::from(entry))) - .collect(); - - let progress_bar = make_progress(geometries.len(), "Geometry", non_interactive); - let pb = progress_bar.clone(); - lighthouse_mem - .write_geometries_with_progress(&geometries, move |completed, _total| { - pb.set_position(completed as u64); - }) - .await - .context("Failed to write geometries")?; - progress_bar.finish_with_message(format!("Uploaded {} geometries", geometries.len())); + .get(&id) + .map(LighthouseBsGeometry::from) + .unwrap_or_default(); + match lighthouse_mem.write_geometry(id, &geo).await { + Ok(()) => {} + Err(Error::MemoryError(_)) => { + // Base station not supported by this firmware build, skip it. + } + Err(e) => { + return Err(e) + .with_context(|| format!("Failed to write geometry for base station {}", id)) + } + } + progress_bar.set_position((id + 1) as u64); } + progress_bar.finish_with_message(format!( + "Uploaded geometry for {} base station(s), invalidated the rest", + config.geos.len() + )); // Upload calibration data - if !config.calibs.is_empty() { - let calibrations: HashMap = config + let progress_bar = make_progress(num_bs as usize, "Calibration", non_interactive); + for id in 0..num_bs { + let calib = config .calibs - .iter() - .map(|(&id, entry)| (id, LighthouseBsCalibration::from(entry))) - .collect(); - - let progress_bar = make_progress(calibrations.len(), "Calibration", non_interactive); - let pb = progress_bar.clone(); - lighthouse_mem - .write_calibrations_with_progress(&calibrations, move |completed, _total| { - pb.set_position(completed as u64); - }) - .await - .context("Failed to write calibrations")?; - progress_bar.finish_with_message(format!("Uploaded {} calibrations", calibrations.len())); + .get(&id) + .map(LighthouseBsCalibration::from) + .unwrap_or_default(); + match lighthouse_mem.write_calibration(id, &calib).await { + Ok(()) => {} + Err(Error::MemoryError(_)) => { + // Base station not supported by this firmware build, skip it. + } + Err(e) => { + return Err(e).with_context(|| { + format!("Failed to write calibration for base station {}", id) + }) + } + } + progress_bar.set_position((id + 1) as u64); } + progress_bar.finish_with_message(format!( + "Uploaded calibration for {} base station(s), invalidated the rest", + config.calibs.len() + )); // Close memory cf.memory.close_memory(lighthouse_mem).await?; + // Persist the written data to permanent storage + let all_ids: Vec = (0..num_bs).collect(); + let persisted = cf + .localization + .lighthouse + .persist_lighthouse_data(&all_ids, &all_ids) + .await + .context("Failed to persist lighthouse configuration to flash")?; + + if !persisted { + bail!("Crazyflie reported failure while persisting lighthouse configuration to flash"); + } + println!(); println!("Lighthouse configuration uploaded successfully!"); From ca9a8b624536339dd51d61a85e017856c46690cf Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Wed, 24 Jun 2026 14:15:10 +0200 Subject: [PATCH 2/4] Added BS ID check to make sure they are inside the 0-max_bs_id range --- src/modules/lighthouse.rs | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/modules/lighthouse.rs b/src/modules/lighthouse.rs index 9048931..b71e379 100644 --- a/src/modules/lighthouse.rs +++ b/src/modules/lighthouse.rs @@ -356,6 +356,17 @@ pub async fn write( let config: LighthouseConfigFile = serde_yaml::from_str(&yaml_content) .with_context(|| "Failed to parse lighthouse config YAML")?; + let max_bs_id = LighthouseMemory::MAX_BASE_STATIONS as u8 - 1; + for &id in config.geos.keys().chain(config.calibs.keys()) { + if id > max_bs_id { + bail!( + "Base station ID {} in config is out of range (0-{})", + id, + max_bs_id + ); + } + } + // Find lighthouse memory let memories = cf.memory.get_memories(Some(MemoryType::Lighthouse)); let lighthouse_mem_device = match memories.first() { From 12c858eea280b81328dd97e616df3688d5d7caa6 Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Wed, 24 Jun 2026 14:26:19 +0200 Subject: [PATCH 3/4] Added hard errors for BS that are not supported by the firmware --- src/modules/lighthouse.rs | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/modules/lighthouse.rs b/src/modules/lighthouse.rs index b71e379..dd8b9ae 100644 --- a/src/modules/lighthouse.rs +++ b/src/modules/lighthouse.rs @@ -397,8 +397,9 @@ pub async fn write( .unwrap_or_default(); match lighthouse_mem.write_geometry(id, &geo).await { Ok(()) => {} - Err(Error::MemoryError(_)) => { - // Base station not supported by this firmware build, skip it. + Err(Error::MemoryError(_)) if !config.geos.contains_key(&id) => { + // Padding entry for a base station not supported by this + // firmware build, skip it. } Err(e) => { return Err(e) @@ -422,8 +423,9 @@ pub async fn write( .unwrap_or_default(); match lighthouse_mem.write_calibration(id, &calib).await { Ok(()) => {} - Err(Error::MemoryError(_)) => { - // Base station not supported by this firmware build, skip it. + Err(Error::MemoryError(_)) if !config.calibs.contains_key(&id) => { + // Padding entry for a base station not supported by this + // firmware build, skip it. } Err(e) => { return Err(e).with_context(|| { From 06afb0944876e198e8f8825ee3c8afd1a10c64b1 Mon Sep 17 00:00:00 2001 From: ArisMorgens Date: Thu, 25 Jun 2026 12:06:42 +0200 Subject: [PATCH 4/4] Updated the lighthouse doc --- docs/lighthouse.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/lighthouse.md b/docs/lighthouse.md index 3b3035c..db6dcf4 100644 --- a/docs/lighthouse.md +++ b/docs/lighthouse.md @@ -179,9 +179,9 @@ Options: - `-i, --input ` — read YAML from a file. If omitted, YAML is read from stdin. -Only the slots present in the YAML are written; existing slots not mentioned -in the YAML are left untouched. To force a slot to be cleared, omit it from -the YAML and re-flash, or use a separate erase tool. +All base station slots are written. Slots present in the YAML are uploaded as +valid, while slots omitted from the YAML are written as invalid to clear +stale configuration. The resulting configuration is then persisted to flash. #### Write Examples