First implementation of digital out for leg enable

servo_controller/src/command_handler.c

@@ -4,6 +4,7 @@
 #include "servo.h"
 #include "adc.h"
 #include "usb.h"
+#include "digital_out.h"
 
 #include <zephyr/logging/log.h>
 
@@ -114,6 +115,14 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
+		case DIGITAL_LEG_OUT: {
+			uint8_t mask = msg->data[0];
+
+			digital_out_set_leg(mask);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;