Btw, here's an example file using ADC readings. It assumes they've already been loaded into a buffer: ```rust //! This module handles voltage, current, and related measurements. use core::{ cell::RefCell, sync::atomic::{AtomicBool, Ordering}, }; // use cortex_m::{self, interrupt::Mutex}; use critical_section::Mutex; use defmt::println; use dronecan::broadcast::{BatteryInUse, CircuitStatusErrorFlag, PowerStats}; use idsp::iir::IIR; use hal::{ adc::Adc, pac::{ADC1, ADC2}, }; use crate::{setup, system_status::StatusLed, BROADCAST_RATIO_READINGS, NODE_ID, USB_CONFIG_MODE}; const CURRENT_AMP_FACTOR: f32 = 50.; // ie INA199_1; const CURRENT_FACTOR_5V: f32 = 1. / (0.02 * CURRENT_AMP_FACTOR); // 20mΩ shunt const CURRENT_FACTOR_7V: f32 = 1. / (0.02 * CURRENT_AMP_FACTOR); // 20mΩ shunt // const CURRENT_FACTOR_BATT: f32 = 1. / (0.0007 * CURRENT_AMP_FACTOR); // 0.7mΩ shunt const CURRENT_FACTOR_BATT_BACKUP: f32 = 1. / (0.02 * CURRENT_AMP_FACTOR); // 20mΩ shunt // Cells are wired in series, so we need incrementally more aggressive dividers. const DIV_CELL1: f32 = 1.4545; // Min 1.27:1 const DIV_CELL2: f32 = 2.65; // Min 2.54:1 const DIV_CELL3: f32 = 4.03; // Min 3.82:1 const DIV_CELL4: f32 = 5.25; // Min 5.09:1 const DIV_CELL5: f32 = 7.666; // Min 6.36:1 const DIV_CELL6: f32 = 7.666; // Min 7.63:1 const DIV_BATT: f32 = 15.6666; // Min 16:1 to support 50v (12S) battery. const DIV_BATT_BACKUP: f32 = 11.; // 11:1 divider (Up 30 36V / 8S) const DIV_7V: f32 = 4.03; // Min 3.64:1 (for 12V) const DIV_5V: f32 = 1.682; // Min 1.52:1 // Measured voltage on these lines must be in these ranges to consider the line healthy. const FIVE_V_HEALTHY_RANGE: (f32, f32) = (4.8, 5.2); const SEVEN_V_HEALTHY_RANGE: (f32, f32) = (7.2, 7.6); const TWELVE_V_HEALTHY_RANGE: (f32, f32) = (11.7, 12.3); const CELL_HEALTHY_RANGE: (f32, f32) = (3.5, 4.3); // If current, in amps, through the backup battery is greater than this, indicate we're using // the backup battery. const ON_BACKUP_CUR_THRESH: f32 = 0.004; // filter_ = signal.iirfilter(1, 1., btype="lowpass", ftype="bessel", output="sos", fs=40) // coeffs = [] // for row in filter_: // coeffs.extend([row[0] / row[3], row[1] / row[3], row[2] / row[3], -row[4] / row[3], -row[5] / row[3]]) static IIR_COEFFS: [f32; 5] = [ 0.07295965726826667, 0.07295965726826667, 0.0, 0.8540806854634666, -0.0, ]; pub static FILTER: Mutex>> = Mutex::new(RefCell::new(IIR { ba: IIR_COEFFS, y_offset: 0., y_min: 0., y_max: 10000., })); // static mut FILTER_STATE_CURR_BATT: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CURR_BATT_BACKUP: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CURR_5V: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CURR_7V: [f32; 5] = [0.; 5]; static mut FILTER_STATE_V_BATT: [f32; 5] = [0.; 5]; static mut FILTER_STATE_V_BATT_BACKUP: [f32; 5] = [0.; 5]; static mut FILTER_STATE_V_5V: [f32; 5] = [0.; 5]; static mut FILTER_STATE_V_7V: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CELL1: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CELL2: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CELL3: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CELL4: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CELL5: [f32; 5] = [0.; 5]; static mut FILTER_STATE_CELL6: [f32; 5] = [0.; 5]; pub static mut ADC1_READ_BUF: [u16; 6] = [0; 6]; pub static mut ADC2_READ_BUF: [u16; 7] = [0; 7]; // We use these atomics to know when both ADC transfers are complete for a given read. pub static ADC1_READ_COMPLETE: AtomicBool = AtomicBool::new(false); pub static ADC2_READ_COMPLETE: AtomicBool = AtomicBool::new(false); fn print_stats(stats: &PowerStats) { println!( "\n Voltages: Bat: {}, backup: {}, 5V: {} 7V: {}", stats.voltage_batt, stats.voltage_batt_backup, stats.voltage_5v, stats.voltage_7v ); println!( "cells: 1: {}, 2: {}, 3: {} 4: {} 5: {} 6: {}", stats.voltage_cell1, stats.voltage_cell2, stats.voltage_cell3, stats.voltage_cell4, stats.voltage_cell5, stats.voltage_cell6 ); println!( "Current: Backup: {}, 5v: {} 7v: {}", stats.current_batt_backup, stats.current_5v, stats.current_7v ); println!( "Portion through: {} time: {} in use: {}", stats.estimated_portion_through, stats.estimated_time_remaining, stats.battery_in_use as u8 ); } #[repr(u8)] #[derive(Clone, Copy)] /// All of these are on ADC1 (or ADC12) pub enum Adc1Channel { Cell3 = 4, Cell4 = 3, Cell5 = 2, Cell6 = 1, Current5v = 5, CurrentBattBackup = 12, } #[repr(u8)] #[derive(Clone, Copy)] pub enum Adc2Channel { Cell1 = 13, Cell2 = 17, FiveV = 3, SevenV = 4, Current7v = 15, BattTotal = 5, BattBackup = 14, // Note: This is available on ADC1 or 2. We use ADC2 to even the count. } pub fn handle_readings( adc1: &Adc, adc2: &Adc, filter: &mut IIR, can: &mut setup::Can_, fd_mode: bool, stats: &mut PowerStats, ) { static mut I: u32 = 0; unsafe { I += 1 }; // Coolect readings, and send over CAN. let buf1 = unsafe { &ADC1_READ_BUF }; let buf2 = unsafe { &ADC2_READ_BUF }; let mut v_cell3_raw = adc1.reading_to_voltage(buf1[0]) * DIV_CELL3; let mut v_cell4_raw = adc1.reading_to_voltage(buf1[1]) * DIV_CELL4; let mut v_cell5_raw = adc1.reading_to_voltage(buf1[2]) * DIV_CELL5; let mut v_cell6_raw = adc1.reading_to_voltage(buf1[3]) * DIV_CELL6; let mut current_5v = adc1.reading_to_voltage(buf1[4]) * CURRENT_FACTOR_5V; // let mut current_batt = adc1.reading_to_voltage(buf1[5]) * CURRENT_FACTOR_BATT; let mut current_batt_backup = adc1.reading_to_voltage(buf1[5]) * CURRENT_FACTOR_BATT_BACKUP; let mut v_cell1_raw = adc2.reading_to_voltage(buf2[0]) * DIV_CELL1; let mut v_cell2_raw = adc2.reading_to_voltage(buf2[1]) * DIV_CELL2; let mut v_5 = adc2.reading_to_voltage(buf2[2]) * DIV_5V; let mut v_7 = adc2.reading_to_voltage(buf2[3]) * DIV_7V; let mut current_7v = adc2.reading_to_voltage(buf2[4]) * CURRENT_FACTOR_7V; let mut v_batt = adc2.reading_to_voltage(buf2[5]) * DIV_BATT; let mut v_batt_backup = adc2.reading_to_voltage(buf2[6]) * DIV_BATT_BACKUP; // Flgs are for the dronecan circuit status. let mut flags_5v = 0; let mut flags_7v = 0; if v_5 < FIVE_V_HEALTHY_RANGE.0 { StatusLed::FiveVHealthy.turn_off(); flags_5v |= CircuitStatusErrorFlag::Undervoltage as u8; } else if v_5 > FIVE_V_HEALTHY_RANGE.1 { StatusLed::FiveVHealthy.turn_off(); flags_5v |= CircuitStatusErrorFlag::Overvoltage as u8; } else { StatusLed::FiveVHealthy.turn_on(); } // Accept a 7v healthy range, or 12v healthy range. if v_7 < SEVEN_V_HEALTHY_RANGE.0 { StatusLed::SevenVHealthy.turn_off(); flags_7v |= CircuitStatusErrorFlag::Undervoltage as u8; } else if v_7 > SEVEN_V_HEALTHY_RANGE.0 && v_7 < SEVEN_V_HEALTHY_RANGE.1 { StatusLed::SevenVHealthy.turn_on(); } else if v_7 > SEVEN_V_HEALTHY_RANGE.1 && v_7 < TWELVE_V_HEALTHY_RANGE.0 { flags_7v |= CircuitStatusErrorFlag::Overvoltage as u8; // todo: Or 12v undervoltage. StatusLed::SevenVHealthy.turn_off(); } else if v_7 > TWELVE_V_HEALTHY_RANGE.0 && v_7 < TWELVE_V_HEALTHY_RANGE.1 { StatusLed::SevenVHealthy.turn_on(); } else { StatusLed::SevenVHealthy.turn_off(); flags_7v |= CircuitStatusErrorFlag::Overvoltage as u8; } v_batt = filter.update(unsafe { &mut FILTER_STATE_V_BATT }, v_batt, false); v_batt_backup = filter.update( unsafe { &mut FILTER_STATE_V_BATT_BACKUP }, v_batt_backup, false, ); v_5 = filter.update(unsafe { &mut FILTER_STATE_V_5V }, v_5, false); v_7 = filter.update(unsafe { &mut FILTER_STATE_V_7V }, v_7, false); v_cell1_raw = filter.update(unsafe { &mut FILTER_STATE_CELL1 }, v_cell1_raw, false); v_cell2_raw = filter.update(unsafe { &mut FILTER_STATE_CELL2 }, v_cell2_raw, false); v_cell3_raw = filter.update(unsafe { &mut FILTER_STATE_CELL3 }, v_cell3_raw, false); v_cell4_raw = filter.update(unsafe { &mut FILTER_STATE_CELL4 }, v_cell4_raw, false); v_cell5_raw = filter.update(unsafe { &mut FILTER_STATE_CELL5 }, v_cell5_raw, false); v_cell6_raw = filter.update(unsafe { &mut FILTER_STATE_CELL6 }, v_cell6_raw, false); // todo: You could use a similar system for cell voltage. let battery_in_use = if current_batt_backup > ON_BACKUP_CUR_THRESH { StatusLed::UsingBackupBatt.turn_on(); BatteryInUse::Backup } else if v_batt > 0.1 { StatusLed::UsingBackupBatt.turn_off(); BatteryInUse::Main } else { StatusLed::UsingBackupBatt.turn_off(); BatteryInUse::None }; // current_batt = filter.update(unsafe { &mut FILTER_STATE_CURR_BATT }, current_batt, false); current_batt_backup = filter.update( unsafe { &mut FILTER_STATE_CURR_BATT_BACKUP }, current_batt_backup, false, ); current_5v = filter.update(unsafe { &mut FILTER_STATE_CURR_5V }, current_5v, false); current_7v = filter.update(unsafe { &mut FILTER_STATE_CURR_7V }, current_7v, false); // todo: If using neither (eg on USB or CAN power), set batt in use to that. if unsafe { I } % BROADCAST_RATIO_READINGS != 0 { return; } // The battery cells are wired in series; subtract in sequence to find individual cell voltage. let v_cell2 = v_cell2_raw - v_cell1_raw; let v_cell3 = v_cell3_raw - v_cell2_raw; let v_cell4 = v_cell4_raw - v_cell3_raw; let v_cell5 = v_cell5_raw - v_cell4_raw; let v_cell6 = v_cell6_raw - v_cell5_raw; let power_stats = PowerStats { voltage_batt: v_batt, voltage_batt_backup: v_batt_backup, voltage_5v: v_5, voltage_7v: v_7, // voltage_cell1: v_cell1_raw, voltage_cell2: v_cell2, voltage_cell3: v_cell3, voltage_cell4: v_cell4, voltage_cell5: v_cell5, voltage_cell6: v_cell6, voltage_cell7: 0., voltage_cell8: 0., // current_batt: 0., current_batt_backup, current_5v, current_7v, // estimated_portion_through: 0., // Fraction of 1. Serialized as a u8, fraction of 255 estimated_time_remaining: 0., // In seconds, serialized as u16 (seconds). battery_in_use, }; *stats = power_stats.clone(); let node_id = NODE_ID.load(Ordering::Acquire); if USB_CONFIG_MODE.load(Ordering::Acquire) { return; } // Our custom format. dronecan::publish_power_stats(can, &power_stats, fd_mode, node_id).ok(); // Donrecan official formats dronecan::publish_power_supply_status( can, power_stats.estimated_time_remaining / 1_000., 0., false, (1. - power_stats.estimated_portion_through * 100.) as u8, 0, fd_mode, node_id, ) .ok(); for data in [ (0, power_stats.voltage_batt, power_stats.current_batt, 0), ( 1, power_stats.voltage_batt_backup, power_stats.current_batt_backup, 0, ), (2, power_stats.voltage_5v, power_stats.current_5v, flags_5v), (3, power_stats.voltage_7v, power_stats.current_7v, flags_7v), (4, power_stats.voltage_cell1, 0., 0), (5, power_stats.voltage_cell2, 0., 0), (6, power_stats.voltage_cell3, 0., 0), (7, power_stats.voltage_cell4, 0., 0), (8, power_stats.voltage_cell5, 0., 0), (9, power_stats.voltage_cell6, 0., 0), ] { dronecan::publish_circuit_status(can, data.0, data.1, data.2, data.3, fd_mode, node_id) .ok(); } } ```