From 4a301af6aec92a17f53ce8c4fa5664c7347f1482 Mon Sep 17 00:00:00 2001
From: fly <merspieler@alwaysdata.net>
Date: Mon, 23 Sep 2024 23:22:32 +0200
Subject: [PATCH] Fox timer in Pedestal Box firmware

Signed-off-by: fly <merspieler@alwaysdata.net>
---
 .../PedestalConnectBox/firmware/src/main.rs   | 45 +++++++++----------
 1 file changed, 20 insertions(+), 25 deletions(-)

diff --git a/Pedestal/PedestalConnectBox/firmware/src/main.rs b/Pedestal/PedestalConnectBox/firmware/src/main.rs
index cac008c..6f28c3d 100644
--- a/Pedestal/PedestalConnectBox/firmware/src/main.rs
+++ b/Pedestal/PedestalConnectBox/firmware/src/main.rs
@@ -11,6 +11,8 @@ mod device;
 
 use panic_halt as _;
 
+use nb::block;
+
 use cortex_m::asm::delay;
 use cortex_m_rt::entry;
 use stm32f1xx_hal::{
@@ -18,7 +20,7 @@ use stm32f1xx_hal::{
 	gpio::{Pin, Input, Analog, PullDown, Output, PushPull},
 	pac,
 	prelude::*,
-	timer::{Tim2FullRemap},
+	timer::{Tim2FullRemap, Timer},
 	flash::{FlashWriter, FlashSize, SectorSize},
 	usb::{Peripheral, UsbBus},
 };
@@ -176,6 +178,7 @@ 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();
@@ -294,8 +297,6 @@ fn main() -> ! {
 		pe13: gpioe.pe13.into_push_pull_output(&mut gpioe.crh),
 	};
 
-//	let mut last = get_report(&mut io_pins, &mut adc1, &cal);
-
 	// ====================== PWM setup =================
 	let c1 = pa15.into_alternate_push_pull(&mut gpioa.crh);
 	let c3 = gpiob.pb10.into_alternate_push_pull(&mut gpiob.crh);
@@ -303,37 +304,31 @@ fn main() -> ! {
 		.TIM2
 		.pwm_hz::<Tim2FullRemap, _, _>((c1, c3), &mut afio.mapr, 1.kHz(), &clocks).split();
 	integ_lt_pwm.enable();
-//	let mut rudder_trim_display_pwm = p
-//		.TIM2
-//		.pwm_hz::<Tim2FullRemap, _, _>(c3, &mut afio.mapr, 1.kHz(), &clocks);
 	rudder_trim_display_pwm.enable();
 	let pwm_max = integ_lt_pwm.get_max_duty(); //48000 in our case
 
 	// ====================== Timer setup ===============
-//	let timer = Instant;
-//	let mut last_report_sent = timer.elapsed();
+	let mut timer = Timer::syst(cp.SYST, &clocks).counter_hz();
+	timer.start(1.kHz()).unwrap();
 
 	// ====================== Main loop =================
 	loop {
+		block!(timer.wait()).unwrap();
 		let report = get_report(&mut io_pins, &mut adc1, &cal);
-// TODO figure out timer and only send in like 1ms intervals or on change
-//		if report != last {
-			match consumer.device().write_report(&report) {
-				Err(UsbHidError::WouldBlock) => {}
-				Err(UsbHidError::UsbError(usb_device::UsbError::BufferOverflow)) => {
-					core::panic!("Failed to write consumer report, report is too big")
-				}
-				Ok(_) => {
-//					last = report;
-				}
-				Err(e) => {
-					// set as indicator that this has happened
-//					io_pins.pe1.set_high();
-//					io_pins.pe4.set_high();
-					core::panic!("Failed to write consumer report: {:?}", e)
-				}
+		match consumer.device().write_report(&report) {
+			Err(UsbHidError::WouldBlock) => {}
+			Err(UsbHidError::UsbError(usb_device::UsbError::BufferOverflow)) => {
+				core::panic!("Failed to write consumer report, report is too big")
 			}
-//		}
+			Ok(_) => {
+			}
+			Err(e) => {
+				// set as indicator that this has happened
+				io_pins.pe1.set_high();
+				io_pins.pe4.set_high();
+				core::panic!("Failed to write consumer report: {:?}", e)
+			}
+		}
 
 		if usb_dev.poll(&mut [&mut consumer]) {
 			match consumer.device().read_report() {