From de552cad42af0802f67525f6e68e18687fd93261 Mon Sep 17 00:00:00 2001
From: fly <merspieler@alwaysdata.com>
Date: Tue, 30 Apr 2024 22:06:07 +0200
Subject: [PATCH] Add calibration

Signed-off-by: fly <merspieler@alwaysdata.com>
---
 Pedestal/firmware/src/main.rs | 47 +++++++++++++++++++++++++++++++----
 1 file changed, 42 insertions(+), 5 deletions(-)

diff --git a/Pedestal/firmware/src/main.rs b/Pedestal/firmware/src/main.rs
index 6512ce3..976278e 100644
--- a/Pedestal/firmware/src/main.rs
+++ b/Pedestal/firmware/src/main.rs
@@ -21,12 +21,25 @@ use usb_device::prelude::*;
 use usbd_human_interface_device::prelude::*;
 
 use crate::device::{CustomConfig, CustomInputReport, CustomOutputReport};
+use core::cell::OnceCell;
 
 struct MyPins {
 	pa1: Pin<'A', 1, Analog>,
 	pa2: Pin<'A', 2, Analog>,
 }
 
+struct CalibrationData {
+	min: u16,
+	max: u16,
+	factor: OnceCell<f32>,
+}
+
+struct Calibration {
+	integ_lt: CalibrationData,
+	flood_lt: CalibrationData,
+}
+
+
 #[entry]
 fn main() -> ! {
 	// ====================== general setup =================
@@ -74,13 +87,19 @@ fn main() -> ! {
 	// Setup ADC
 	let mut adc1 = adc::Adc::adc1(p.ADC1, clocks);
 
+	// ====================== Calibration ===============
+	let cal = Calibration {
+		integ_lt: CalibrationData {min: 100, max: 4000, factor: OnceCell::new()},
+		flood_lt: CalibrationData {min: 0, max: 0, factor: OnceCell::new()},
+	};
+
 	// ====================== Pin setup =================
 	let mut input_pins = MyPins {
 		pa1: gpioa.pa1.into_analog(&mut gpioa.crl),
 		pa2: gpioa.pa2.into_analog(&mut gpioa.crl),
 	};
 
-	let mut last = get_report(&mut input_pins, &mut adc1);
+	let mut last = get_report(&mut input_pins, &mut adc1, &cal);
 
 	// ====================== PWM setup =================
 	let mut afio = p.AFIO.constrain();
@@ -93,7 +112,7 @@ fn main() -> ! {
 
 	// ====================== Main loop =================
 	loop {
-		let report = get_report(&mut input_pins, &mut adc1);
+		let report = get_report(&mut input_pins, &mut adc1, &cal);
 		if report != last {
 			match consumer.device().write_report(&report) {
 				Err(UsbHidError::WouldBlock) => {}
@@ -129,15 +148,33 @@ fn main() -> ! {
 }
 
 // Returns a CustomInputReport from the inputs given
-fn get_report(pins: &mut MyPins, adc1: &mut adc::Adc<pac::ADC1>) -> CustomInputReport {
+fn get_report(pins: &mut MyPins, adc1: &mut adc::Adc<pac::ADC1>, cal: &Calibration) -> CustomInputReport {
 	let integ_lt: u16 = adc1.read(&mut pins.pa1).unwrap();
 	let flood_lt: u16 = 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 = 4095;
+	}
+	else {
+		let factor: f32 = *cal.integ_lt.factor.get_or_init(|| cal.integ_lt.get_scale_factor());
+		integ_lt_norm = ((integ_lt - cal.integ_lt.min) as f32 * factor) as u16;
+	}
+
 	CustomInputReport {
-		x: integ_lt.into(),
+		x: integ_lt_norm.into(),
 		y: flood_lt.into(),
 		buttons,
 	}
 }
+
+impl CalibrationData {
+	const ADC_MAX: u16 = 4095;
+	fn get_scale_factor (&self) -> f32 {
+		return (Self::ADC_MAX / (Self::ADC_MAX - self.min - (Self::ADC_MAX - self.max))) as f32;
+	}
+}