From 2f5956f3e324eb12fa3c20f0f4383a1e745bbe78 Mon Sep 17 00:00:00 2001
From: "Jan \"Yenya\" Kasprzak" <kas@fi.muni.cz>
Date: Fri, 22 May 2015 23:10:43 +0200
Subject: [PATCH] Three-state LED: display errors if present

Also support remote status update via modbus (untested yet)
---
 firmware/firmware.c | 70 ++++++++++++++++++++++++++++++++++++---------
 1 file changed, 57 insertions(+), 13 deletions(-)

diff --git a/firmware/firmware.c b/firmware/firmware.c
index eb064cd..bbba987 100644
--- a/firmware/firmware.c
+++ b/firmware/firmware.c
@@ -88,24 +88,63 @@ static void do_measurement(unsigned char trig)
 static void led_set(uint8_t led, uint8_t state)
 {
 	if (led == 0) {
-		if (state) {
-			PORTC |= _BV(PC5);
-			led_bitmap |= 1;
-		} else {
-			PORTC &= ~_BV(PC5);
+		switch (state) {
+		case 0:
 			led_bitmap &= ~1;
+			led_bitmap &= ~2;
+			break;
+		case 1:
+			led_bitmap |= 1;
+			led_bitmap &= ~2;
+			break;
+		default: // error
+			led_bitmap |= 2;
+			break;
 		}
 	} else {
-		if (state) {
-			PORTB |= _BV(PB5);
-			led_bitmap |= 2;
-		} else {
-			PORTB &= ~_BV(PB5);
-			led_bitmap &= ~2;
+		switch (state) {
+		case 0:
+			led_bitmap &= ~4;
+			led_bitmap &= ~8;
+			break;
+		case 1:
+			led_bitmap |= 4;
+			led_bitmap &= ~8;
+			break;
+		default:
+			led_bitmap |= 8;
+			break;
 		}
 	}
 }
 
+static void leds_update()
+{
+	if (led_bitmap & 1) {
+		PORTC |= _BV(PC5);
+	} else {
+		PORTC &= ~_BV(PC5);
+	}
+
+	if (led_bitmap & 2) {
+		DDRC &= ~_BV(PC5);
+	} else {
+		DDRC |= _BV(PC5);
+	}
+
+	if (led_bitmap & 4) {
+		PORTB |= _BV(PB5);
+	} else {
+		PORTB &= ~_BV(PB5);
+	}
+
+	if (led_bitmap & 8) {
+		DDRB |= _BV(PB5);
+	} else {
+		DDRB &= ~_BV(PB5);
+	}
+}
+
 static void eval_bitmaps()
 {
 	uint16_t free_b = 0, err_b = 0, mask;
@@ -127,7 +166,9 @@ static void eval_bitmaps()
 	err_bitmap  = err_b;
 
 	if (led1_sensors) {
-		if (led1_sensors & free_bitmap) {
+		if (led1_sensors & err_bitmap) {
+			led_set(0, 2);
+		} else if (led1_sensors & free_bitmap) {
 			led_set(0, 1);
 		} else {
 			led_set(0, 0);
@@ -135,7 +176,9 @@ static void eval_bitmaps()
 	}
 
 	if (led2_sensors) {
-		if (led2_sensors & free_bitmap) {
+		if (led2_sensors & err_bitmap) {
+			led_set(1, 2);
+		} else if (led2_sensors & free_bitmap) {
 			led_set(1, 1);
 		} else {
 			led_set(1, 0);
@@ -178,6 +221,7 @@ int main()
 		}
 
 		eval_bitmaps();
+		leds_update(); // might be written from modbus
 //		led_set(0,
 //			distances[4] > 100 || distances[11] > 100);
 	}
-- 
2.43.0