1 files changed, 14 insertions, 1 deletions

diff --git a/main/i2c.c b/main/i2c.c
index 2615d0a..e42e0b1 100644
--- a/main/i2c.c
+++ b/main/i2c.c
@@ -7,6 +7,11 @@
 #include <pico/stdlib.h>
 #include <hardware/i2c.h>
 
+#include <FreeRTOS.h>
+#include <queue.h>
+
+extern QueueHandle_t queue;
+
 void init_i2c() {
 	i2c_init(I2C_PORT, 100 * 1000); // currently at 100kHz
 
@@ -77,11 +82,12 @@ void bus_task() {
 		// printf("Bus scan!");
 		scan_bus(found);
 
+		uint8_t data;
 		for(int i = 0; i < MAX_SLAVES; i++){
 			if( found[i] == 0x00 )
 				break;
 			
-			uint8_t data = 0x01;
+			data = 0x01;
 			// send data to found slave address
 			write_i2c(found[i], &data, 1);
 
@@ -90,5 +96,12 @@ void bus_task() {
 			// request update from slave addr at found[i]
 			//write_i2c();
 		}
+
+		uint8_t rcvData[2];
+		if(xQueueReceive(queue, &rcvData, portMAX_DELAY) == pdPASS){
+			write_i2c(rcvData[0], &rcvData[1], sizeof(rcvData[1]));
+		} else {
+			printf("Something went wrong at queue receive!\n");
+		}
 	}
 }