Added old multiplexer driver

servo_controller/src/adc.c

@@ -1,6 +1,7 @@
 #include "adc.h"
 #include "command_message.h"
 #include "usb.h"
+#include "mux.h"
 
 #include <zephyr/logging/log.h>
 #include <zephyr/drivers/adc.h>
@@ -79,11 +80,11 @@ int adc_init_all(void) {
 	// TIMER
 	k_timer_init(&adc_timer, adc_timer_handler, NULL);
 
-	// ret = mux_init();
-	// if (ret != 0) {
-	// 	LOG_ERR("Failed to setup multiplexer: %d", ret);
-	// 	return ret;
-	// }
+	ret = mux_init();
+	if (ret != 0) {
+		LOG_ERR("Failed to setup multiplexer: %d", ret);
+		return ret;
+	}
 
 	if (!adc_is_ready_dt(&adc_mux_spec)) {
 		LOG_ERR("ADC mux channel not ready");
@@ -143,12 +144,11 @@ int adc_read_id(int id) {
 		// Read muxed channels
 		(void)adc_sequence_init_dt(&adc_mux_spec, &sequence);
 
-		// Set mux TODO:
-		// ret = mux_select_channel(id);
-		// if (ret < 0) {
-		// 	LOG_ERR("Could not set multiplexer (%d)\n", ret);
-		// 	return 0;
-		// }
+		ret = mux_select_channel(id);
+		if (ret < 0) {
+			LOG_ERR("Could not set multiplexer (%d)\n", ret);
+			return 0;
+		}
 		
 		ret = adc_read_dt(&adc_mux_spec, &sequence);
 		if (ret < 0) {

servo_controller/src/mux.c

@@ -0,0 +1,44 @@
+#include "mux.h"
+
+#include <zephyr/logging/log.h>
+#include <stdlib.h>
+
+LOG_MODULE_REGISTER(mux);
+
+#define MUX_NODE DT_ALIAS(mux)
+
+static const struct device *mux_dev = DEVICE_DT_GET(MUX_NODE);
+
+int mux_init(void) {
+	int ret;
+
+	if (!DT_NODE_HAS_STATUS(MUX_NODE, okay)) {
+		LOG_ERR("mux alias missing or disabled in devicetree");
+		return -ENODEV;
+	}
+
+	if (!device_is_ready(mux_dev)) {
+		LOG_ERR("Multiplexer device not ready");
+		return -ENODEV;
+	}
+
+	ret = cd74hc4067_enable(mux_dev);
+	if ((ret != 0) && (ret != -ENOTSUP)) {
+		LOG_ERR("Failed to enable multiplexer: %d", ret);
+		return ret;
+	}
+
+	ret = cd74hc4067_select_channel(mux_dev, 0U);
+	if (ret != 0) {
+		LOG_ERR("Failed to select initial channel: %d", ret);
+		return ret;
+	}
+
+	LOG_INF("Multiplexer HAL initialized");
+
+	return 0;
+}
+
+int mux_select_channel(int channel) {
+    return cd74hc4067_select_channel(mux_dev, channel);
+}

servo_controller/src/mux.h

@@ -0,0 +1,13 @@
+#ifndef MUX_H
+#define MUX_H
+
+
+#include "cd74hc4067.h"
+
+#define MUX_CHANNELS CD74HC4067_SELECT_CHANNEL_COUNT
+
+int mux_init(void);
+int mux_select_channel(int channel);
+
+
+#endif // MUX_H