diff --git a/Pedestal/firmware/src/main.rs b/Pedestal/firmware/src/main.rs index d83499e..39eaf92 100644 --- a/Pedestal/firmware/src/main.rs +++ b/Pedestal/firmware/src/main.rs @@ -18,7 +18,7 @@ use stm32f1xx_hal::{ pac, prelude::*, timer::{Channel, Tim2NoRemap}, - flash::{FlashWriter, FLASH_START, FlashSize, SectorSize}, + flash::{FlashWriter, FlashSize, SectorSize}, usb::{Peripheral, UsbBus}, }; @@ -49,10 +49,6 @@ struct CalibrationData { impl CalibrationData { const ADC_MAX: u16 = 4095; - const _dummy: () = { - let size = core::mem::size_of::(); - assert!(size <= 1021, "CalibrationData too big for flash size!"); - }; fn new (min: u16, max: u16) -> CalibrationData { return CalibrationData {min, max, factor: calculate_factor(min, max)}; } @@ -64,15 +60,17 @@ impl CalibrationData { #[derive(Clone)] #[derive(Zeroable)] struct Calibration { - integ_lt: CalibrationData, -// flood_lt: CalibrationData, + data: [CalibrationData; 2], } impl Calibration { + const _dummy: () = { + let size = core::mem::size_of::(); + assert!(size <= 1021, "Calibration too big for flash size!"); + }; fn new () -> Calibration { return Calibration { - integ_lt: CalibrationData::new(0, CalibrationData::ADC_MAX), -// flood_lt: CalibrationData::new(0, CalibrationData::ADC_MAX), + data: [CalibrationData::new(0, CalibrationData::ADC_MAX); 2], }; } } @@ -82,7 +80,6 @@ impl Calibration { fn main() -> ! { // ====================== general setup ================= // Acquire peripherals - // let cp = cortex_m::Peripherals::take().unwrap(); let p = pac::Peripherals::take().unwrap(); let mut flash = p.FLASH.constrain(); let rcc = p.RCC.constrain(); @@ -163,6 +160,7 @@ fn main() -> ! { } } + // TODO figure out why we only land here after we've sent a report if usb_dev.poll(&mut [&mut consumer]) { match consumer.device().read_report() { Err(UsbHidError::WouldBlock) => {} @@ -182,20 +180,30 @@ fn main() -> ! { if output.generic & 0x1 == 0x1 { calibration_active = true; if !calibration_min_done && output.generic & 0x2 == 0x2 { - cal.integ_lt.min = adc1.read(&mut input_pins.pa1).unwrap(); + cal.data[0].min = adc1.read(&mut input_pins.pa1).unwrap(); + cal.data[1].min = adc1.read(&mut input_pins.pa2).unwrap(); calibration_min_done = true; } } else { if calibration_active { - let max = adc1.read(&mut input_pins.pa1).unwrap(); - if max > cal.integ_lt.min { - cal.integ_lt.max = max; + let mut values: [u16; 2] = [0; 2]; + values[0] = adc1.read(&mut input_pins.pa1).unwrap(); + values[1] = adc1.read(&mut input_pins.pa2).unwrap(); + let mut i = 0; + loop { + if values[i] > cal.data[i].min { + cal.data[i].max = values[i]; + } + else { + cal.data[i].max = CalibrationData::ADC_MAX; + } + cal.data[i].factor = calculate_factor(cal.data[i].min, cal.data[i].max); + i += 1; + if i == values.len() { + break; + } } - else { - cal.integ_lt.max = CalibrationData::ADC_MAX; - } - cal.integ_lt.factor = calculate_factor(cal.integ_lt.min, cal.integ_lt.max); let save_success = save_calibration(&mut flash_writer, &cal); if save_success { pwm.set_duty(Channel::C1, pwm_max); @@ -221,24 +229,32 @@ fn calculate_factor(min: u16, max: u16) -> f32 { // Returns a CustomInputReport from the inputs given fn get_report(pins: &mut MyPins, adc1: &mut adc::Adc, cal: &Calibration) -> CustomInputReport { - let integ_lt: u16 = adc1.read(&mut pins.pa1).unwrap(); - let flood_lt: u16 = adc1.read(&mut pins.pa2).unwrap(); + let mut values: [u16; 2] = [0; 2]; + values[0] = adc1.read(&mut pins.pa1).unwrap(); + values[1] = adc1.read(&mut pins.pa2).unwrap(); let buttons: u16 = 0; - let integ_lt_norm: u16; - if integ_lt < cal.integ_lt.min { - integ_lt_norm = 0; - } - else if integ_lt > cal.integ_lt.max { - integ_lt_norm = CalibrationData::ADC_MAX; - } - else { - integ_lt_norm = ((integ_lt - cal.integ_lt.min) as f32 * cal.integ_lt.factor) as u16; + let mut values_norm: [u16; 2] = [0; 2]; + let mut i = 0; + loop { + if values[i] < cal.data[i].min { + values_norm[i] = 0; + } + else if values[i] > cal.data[i].max { + values_norm[i] = CalibrationData::ADC_MAX; + } + else { + values_norm[i] = ((values[i] - cal.data[i].min) as f32 * cal.data[i].factor) as u16; + } + i += 1; + if i == values_norm.len() { + break; + } } CustomInputReport { - x: integ_lt_norm, - y: flood_lt, + x: values_norm[0], + y: values_norm[1], buttons, } } @@ -288,7 +304,7 @@ fn load_calibration(flash: &mut FlashWriter) -> Calibration { // Load calibration data let dummy = bytes_of(&cal); let dummy2 = &data[3..][..dummy.len()]; - let mut dummy3 = bytes_of_mut(&mut cal); + let dummy3 = bytes_of_mut(&mut cal); dummy3.copy_from_slice(&dummy2); return cal; },