pixracerpro/
battery.rs

1//! Pixracer Pro analog battery monitor.
2//!
3//! The board routes battery voltage and current from its power-monitor input to
4//! ADC1 channels 14 (PA2) and 15 (PA3). ADC3's internal VREFINT channel is read
5//! alongside them so conversion uses measured VDDA, matching ROSflight C.
6
7use stm_32::embassy_stm32::{
8    Peri,
9    adc::{Adc, AdcChannel, AdcConfig, AnyAdcChannel, Resolution, SampleTime},
10    pac,
11    peripherals::{ADC1, ADC3, DMA2_CH0, DMA2_CH1, PA2, PA3},
12};
13use stm_32::embassy_sync::{blocking_mutex::raw::CriticalSectionRawMutex, signal::Signal};
14use stm_32::embassy_time::{Duration, Instant, Timer, block_for, with_timeout};
15use veloxity_core::{
16    battery::BatteryMonitorCalibration,
17    errors::SensorError,
18    packets::{BatteryPacket, RosflightPacketHeader},
19};
20
21#[cfg(feature = "runtime-diagnostics")]
22use core::sync::atomic::{AtomicU32, Ordering};
23
24#[cfg(feature = "runtime-diagnostics")]
25#[unsafe(no_mangle)]
26pub static VELOXITY_DIAG_BATTERY_SAMPLE_COUNT: AtomicU32 = AtomicU32::new(0);
27#[cfg(feature = "runtime-diagnostics")]
28#[unsafe(no_mangle)]
29pub static VELOXITY_DIAG_BATTERY_SAMPLE_SUM_US: AtomicU32 = AtomicU32::new(0);
30#[cfg(feature = "runtime-diagnostics")]
31#[unsafe(no_mangle)]
32pub static VELOXITY_DIAG_BATTERY_SAMPLE_MAX_US: AtomicU32 = AtomicU32::new(0);
33#[cfg(feature = "runtime-diagnostics")]
34#[unsafe(no_mangle)]
35pub static VELOXITY_DIAG_BATTERY_PUBLISH: AtomicU32 = AtomicU32::new(0);
36#[cfg(feature = "runtime-diagnostics")]
37#[unsafe(no_mangle)]
38pub static VELOXITY_DIAG_BATTERY_ERROR_PUBLISH: AtomicU32 = AtomicU32::new(0);
39#[cfg(feature = "runtime-diagnostics")]
40#[unsafe(no_mangle)]
41pub static VELOXITY_DIAG_BATTERY_SIGNAL_OVERWRITE: AtomicU32 = AtomicU32::new(0);
42
43const SAMPLE_PERIOD: Duration = Duration::from_millis(100);
44const ADC_CONVERSION_TIMEOUT: Duration = Duration::from_millis(5);
45const ADC_SAMPLE_TIME: SampleTime = SampleTime::CYCLES810_5;
46
47// Values from the ROSflight Pixracer Pro BoardConfig.h ADC channel table.
48const BOARD_CALIBRATION: BatteryMonitorCalibration =
49    BatteryMonitorCalibration::new(12.62, 0.0, 60.5, 0.0747);
50
51// STM32H743/H753 factory VREFINT calibration: 16-bit ADC count measured with
52// VDDA = 3.3 V. This is the address and reference used by the STM32 HAL and the
53// ROSflight C firmware for this board.
54const VREFINT_CAL_ADDR: *const u16 = 0x1FF1_E860 as *const u16;
55const VREFINT_CAL_VDDA: f32 = 3.3;
56#[derive(Debug, Clone, Copy)]
57pub struct BatteryMultipliers {
58    pub voltage: f32,
59    pub current: f32,
60}
61
62pub static BATTERY_SIGNAL: Signal<CriticalSectionRawMutex, Result<BatteryPacket, SensorError>> =
63    Signal::new();
64
65static MULTIPLIER_SIGNAL: Signal<CriticalSectionRawMutex, BatteryMultipliers> = Signal::new();
66
67pub fn configure_multipliers(voltage: f32, current: f32) {
68    MULTIPLIER_SIGNAL.signal(BatteryMultipliers { voltage, current });
69}
70
71pub struct PixracerBatteryMonitor {
72    adc1: Adc<'static, ADC1>,
73    adc3: Adc<'static, ADC3>,
74    adc1_dma: Peri<'static, DMA2_CH0>,
75    adc3_dma: Peri<'static, DMA2_CH1>,
76    voltage_channel: AnyAdcChannel<'static, ADC1>,
77    current_channel: AnyAdcChannel<'static, ADC1>,
78    vref_channel: AnyAdcChannel<'static, ADC3>,
79    calibration: BatteryMonitorCalibration,
80}
81
82impl PixracerBatteryMonitor {
83    pub fn new(
84        adc1: Peri<'static, ADC1>,
85        adc3: Peri<'static, ADC3>,
86        voltage_pin: Peri<'static, PA2>,
87        current_pin: Peri<'static, PA3>,
88        adc1_dma: Peri<'static, DMA2_CH0>,
89        adc3_dma: Peri<'static, DMA2_CH1>,
90    ) -> Self {
91        let config = AdcConfig {
92            resolution: Some(Resolution::BITS16),
93            ..Default::default()
94        };
95        let adc1 = Adc::new_with_config(adc1, config);
96        let adc3 = Adc::new_with_config(
97            adc3,
98            AdcConfig {
99                resolution: Some(Resolution::BITS16),
100                ..Default::default()
101            },
102        );
103
104        configure_adc_hardware();
105        let voltage_channel: AnyAdcChannel<'static, ADC1> = voltage_pin.degrade_adc();
106        let current_channel: AnyAdcChannel<'static, ADC1> = current_pin.degrade_adc();
107        // Embassy's STM32H7 ADC v4 mapping correctly selects ADC3 channel 19.
108        // Keep VREFINT on ADC3, as required by the STM32H753/Pixracer Pro.
109        let vref_channel: AnyAdcChannel<'static, ADC3> = adc3.enable_vrefint().degrade_adc();
110
111        Self {
112            adc1,
113            adc3,
114            adc1_dma,
115            adc3_dma,
116            voltage_channel,
117            current_channel,
118            vref_channel,
119            calibration: BOARD_CALIBRATION,
120        }
121    }
122
123    async fn sample(&mut self) -> Result<BatteryPacket, SensorError> {
124        if let Some(multipliers) = MULTIPLIER_SIGNAL.try_take() {
125            self.calibration
126                .apply_multipliers(multipliers.voltage, multipliers.current);
127        }
128
129        let started_us = Instant::now().as_micros();
130        let mut vref_reading = [0u16; 1];
131        with_timeout(
132            ADC_CONVERSION_TIMEOUT,
133            self.adc3.read(
134                self.adc3_dma.reborrow(),
135                crate::board::BoardIrqs,
136                [(&mut self.vref_channel, ADC_SAMPLE_TIME)].into_iter(),
137                &mut vref_reading,
138            ),
139        )
140        .await
141        .map_err(|_| SensorError::GenericSensorError("ADC VREFINT DMA conversion timed out"))?;
142
143        let mut battery_readings = [0u16; 2];
144        with_timeout(
145            ADC_CONVERSION_TIMEOUT,
146            self.adc1.read(
147                self.adc1_dma.reborrow(),
148                crate::board::BoardIrqs,
149                [
150                    (&mut self.voltage_channel, ADC_SAMPLE_TIME),
151                    (&mut self.current_channel, ADC_SAMPLE_TIME),
152                ]
153                .into_iter(),
154                &mut battery_readings,
155            ),
156        )
157        .await
158        .map_err(|_| SensorError::GenericSensorError("ADC1 DMA conversion timed out"))?;
159        let completed_us = Instant::now().as_micros();
160
161        let vref_count = vref_reading[0];
162        let voltage_count = battery_readings[0];
163        let current_count = battery_readings[1];
164
165        let vdda = measured_vdda(vref_count).ok_or(SensorError::GenericSensorError(
166            "ADC VREFINT conversion failed",
167        ))?;
168        Ok(BatteryPacket {
169            header: RosflightPacketHeader {
170                timestamp: started_us + completed_us.saturating_sub(started_us) / 2,
171                status: 0,
172            },
173            voltage: self
174                .calibration
175                .voltage
176                .convert_16_bit_sample(voltage_count, vdda),
177            current: self
178                .calibration
179                .current
180                .convert_16_bit_sample(current_count, vdda),
181        })
182    }
183
184    async fn run(&mut self) -> ! {
185        loop {
186            Timer::at(stm_32::synch_at(SAMPLE_PERIOD)).await;
187            #[cfg(feature = "runtime-diagnostics")]
188            let started = Instant::now();
189            let sample = self.sample().await;
190            #[cfg(feature = "runtime-diagnostics")]
191            {
192                let elapsed_us = started.elapsed().as_micros().min(u32::MAX as u64) as u32;
193                VELOXITY_DIAG_BATTERY_SAMPLE_COUNT.fetch_add(1, Ordering::Relaxed);
194                VELOXITY_DIAG_BATTERY_SAMPLE_SUM_US.fetch_add(elapsed_us, Ordering::Relaxed);
195                VELOXITY_DIAG_BATTERY_SAMPLE_MAX_US.fetch_max(elapsed_us, Ordering::Relaxed);
196                VELOXITY_DIAG_BATTERY_PUBLISH.fetch_add(1, Ordering::Relaxed);
197                if sample.is_err() {
198                    VELOXITY_DIAG_BATTERY_ERROR_PUBLISH.fetch_add(1, Ordering::Relaxed);
199                }
200                if BATTERY_SIGNAL.signaled() {
201                    VELOXITY_DIAG_BATTERY_SIGNAL_OVERWRITE.fetch_add(1, Ordering::Relaxed);
202                }
203            }
204            BATTERY_SIGNAL.signal(sample);
205        }
206    }
207}
208
209fn configure_adc_hardware() {
210    use pac::adc::vals::Boost;
211    use pac::adccommon::vals::Presc;
212
213    // ROSflight C uses DIV32 for the Pixracer Pro external ADC. Use the same
214    // conservative clock for ADC3 as well; conversion ratios are unaffected,
215    // and VREFINT is sampled far below its maximum rate.
216    pac::ADC12_COMMON
217        .ccr()
218        .modify(|register| register.set_presc(Presc::DIV32));
219    pac::ADC3_COMMON.ccr().modify(|register| {
220        register.set_presc(Presc::DIV32);
221        register.set_vrefen(true);
222    });
223    pac::ADC1
224        .cr()
225        .modify(|register| register.set_boost(Boost::LT6_25));
226    pac::ADC3
227        .cr()
228        .modify(|register| register.set_boost(Boost::LT6_25));
229
230    // STM32H753 datasheet maximum VREFINT startup time is below 15 us.
231    block_for(Duration::from_micros(15));
232}
233
234fn measured_vdda(vref_count: u16) -> Option<f32> {
235    if vref_count == 0 {
236        return None;
237    }
238    // SAFETY: this read-only system-memory address is defined by STM32H743/H753
239    // and contains ST's factory-programmed VREFINT calibration value.
240    let calibrated_count = unsafe { core::ptr::read_volatile(VREFINT_CAL_ADDR) };
241    if calibrated_count == 0 {
242        return None;
243    }
244    Some(VREFINT_CAL_VDDA * f32::from(calibrated_count) / f32::from(vref_count))
245}
246
247#[embassy_executor::task]
248pub async fn task(mut monitor: PixracerBatteryMonitor) {
249    monitor.run().await;
250}
pixracerpro/battery.rs

pixracerpro/
battery.rs
