Bugfixes and speed improvements

Software/VNA_embedded/Application/Drivers/stm.cpp

@@ -0,0 +1,50 @@
+#include "stm.hpp"
+
+using Callback = void(*)(void);
+static constexpr uint8_t numCallbacks = 10;
+static Callback callbacks[numCallbacks];
+uint8_t read_index, write_index;
+
+static void increment(uint8_t &index) {
+	if(index < numCallbacks - 1) {
+		index++;
+	} else {
+		index = 0;
+	}
+}
+static uint8_t callbackFifoLevel() {
+	int8_t level = (int8_t) write_index - (int8_t) read_index;
+	if(level < 0) {
+		level += numCallbacks;
+	}
+	return (uint8_t) level;
+}
+
+void STM::Init() {
+	read_index = write_index = 0;
+	HAL_NVIC_SetPriority(COMP4_IRQn, 6, 0);
+	HAL_NVIC_EnableIRQ(COMP4_IRQn);
+}
+
+bool STM::DispatchToInterrupt(void (*cb)(void)) {
+	if(callbackFifoLevel() < numCallbacks - 1) {
+		callbacks[write_index] = cb;
+		increment(write_index);
+		HAL_NVIC_SetPendingIRQ(COMP4_IRQn);
+		return true;
+	} else {
+		// already at limit
+		return false;
+	}
+}
+
+extern "C" {
+void COMP4_IRQHandler() {
+	while(callbackFifoLevel() > 0) {
+		if (callbacks[read_index]) {
+			callbacks[read_index]();
+		}
+		increment(read_index);
+	}
+}
+}