From 016e37e009fa1c7d010f9c818466911b3f9458a6 Mon Sep 17 00:00:00 2001
From: Gregor Riepl <onitake@gmail.com>
Date: Mon, 8 Feb 2021 16:14:56 +0100
Subject: [PATCH] Implement glitch smoothing

---
 feedback/firmware/main.c | 22 ++++++++++++++++++++--
 1 file changed, 20 insertions(+), 2 deletions(-)

diff --git a/feedback/firmware/main.c b/feedback/firmware/main.c
index 3cbce06..992aa09 100644
--- a/feedback/firmware/main.c
+++ b/feedback/firmware/main.c
@@ -7,6 +7,7 @@
 
 static bool report_change = false;
 static uint16_t saved_status;
+static uint16_t status_hysteresis[2];
 
 static void init() {
 	// Set PB as input, pull-up off
@@ -58,6 +59,21 @@ static uint16_t get_status() {
 	return ((uint16_t) statush << 8) | ((uint16_t) statusl);
 }
 
+static uint16_t graded_status() {
+	uint16_t status = get_status();
+
+	// combine the bits of the last 3 states into one: "at least 2 of 3"
+	uint16_t graded01 = status_hysteresis[0] & status_hysteresis[1];
+	uint16_t graded1n = status_hysteresis[1] & status;
+	uint16_t gradedn0 = status & status_hysteresis[0];
+	uint16_t graded = graded01 | graded1n | gradedn0;
+
+	status_hysteresis[0] = status_hysteresis[1];
+	status_hysteresis[1] = status;
+
+	return graded;
+}
+
 static void send_status(uint16_t status) {
 	if (can_check_free_buffer()) {
 		can_t msg = {
@@ -74,7 +90,7 @@ static void send_status(uint16_t status) {
 
 static void loop() {
 	// Check for status changes
-	uint16_t new_status = get_status();
+	uint16_t new_status = graded_status();
 	if (new_status != saved_status) {
 		if (report_change) {
 			send_status(new_status);
@@ -89,7 +105,7 @@ static void loop() {
 		if (status != 0) {
 			switch (msg.id) {
 			case CAN_MSG_FEEDBACK_STATUS:
-				send_status(get_status());
+				send_status(new_status);
 				break;
 			case CAN_MSG_AUTO_STATUS:
 				if (msg.length == 1) {
@@ -99,6 +115,8 @@ static void loop() {
 			}
 		}
 	}
+
+	_delay_us(100);
 }
 
 int main() {