Added ADC thread that waits for msgq channel bitmask

servo_controller/scripts/read_adc.py

@@ -16,8 +16,10 @@ LED_TOGGLE = 2
 LED_SET = 3
 SERVO_SET = 4
 SERVO_SET_ALL = 5
-ADC_READ = 6
+ADC_READ_RAW = 6
-ADC_READ_ALL = 7
+ADC_READ_RAW_ALL = 7
+ADC_READ = 8
+ADC_READ_ALL = 9
 
 DEVICE_ID = 0
 
@@ -114,7 +116,8 @@ def main():
 	threading.Thread(target=reader, args=(ser,), daemon=True).start()
 
 	data = bytearray()
-	data.append(17)
+	# Data needs to be a bitmask of channels
+	data.append(0b11011)
 
 	ser.write(make_packet(ADC_READ, data))
 

servo_controller/scripts/read_adc_all.py

@@ -16,8 +16,10 @@ LED_TOGGLE = 2
 LED_SET = 3
 SERVO_SET = 4
 SERVO_SET_ALL = 5
-ADC_READ = 6
+ADC_READ_RAW = 6
-ADC_READ_ALL = 7
+ADC_READ_RAW_ALL = 7
+ADC_READ = 8
+ADC_READ_ALL = 9
 
 DEVICE_ID = 0
 

servo_controller/src/adc.c

@@ -1,7 +1,10 @@
 #include "adc.h"
+#include "command_message.h"
+#include "usb.h"
 
 #include <zephyr/logging/log.h>
 #include <zephyr/drivers/adc.h>
+#include <zephyr/kernel.h>
 
 LOG_MODULE_REGISTER(adc, LOG_LEVEL_INF);
 
@@ -17,9 +20,53 @@ struct adc_sequence sequence = {
     .buffer_size = sizeof(adc_buffer),
 };
 
+// ADC THREAD
+static struct k_thread adc_thread_data;
+static k_tid_t adc_thread_id = NULL;
+#define ADC_THREAD_STACK_SIZE 2048
+#define ADC_THREAD_PRIORITY 5
+K_THREAD_STACK_DEFINE(adc_thread_stack, ADC_THREAD_STACK_SIZE);
+
+// ADC MASK BUFFER
+#define ADC_MASK_BUFFER_SIZE 10
+char adc_mask_msgq_buffer[ADC_MASK_BUFFER_SIZE * sizeof(uint32_t)];
+struct k_msgq adc_mask_msgq;
+
+
+static void adc_thread(void *p1, void *p2, void *p3) {
+	ARG_UNUSED(p1);
+	ARG_UNUSED(p2);
+	ARG_UNUSED(p3);
+
+    while (1) {
+		uint32_t mask = 0;
+        k_msgq_get(&adc_mask_msgq, &mask, K_FOREVER);
+
+		// Read ADC
+		int values[NUM_ADC_CHANNELS];
+		for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
+			int bit = (mask >> i) & 0x1;
+			
+			if (bit) {
+				values[i] = adc_read_id(i);
+			}
+			else {
+				values[i] = 0;
+			}
+		}
+		
+		// Send to USB
+		struct command_message_t *msg = usb_get_next_tx_buf();
+		command_create_message(msg, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+		usb_send_command(msg);
+	}
+}
+
 int adc_init_all(void) {
 	int ret;
 
+	k_msgq_init(&adc_mask_msgq, adc_mask_msgq_buffer, sizeof(uint32_t), ADC_MASK_BUFFER_SIZE);
+
 	// ret = mux_init();
 	// if (ret != 0) {
 	// 	LOG_ERR("Failed to setup multiplexer: %d", ret);
@@ -59,6 +106,18 @@ int adc_init_all(void) {
 	adc_sequence_init_dt(&adc_ch16_spec, &sequence);
 	adc_sequence_init_dt(&adc_ch17_spec, &sequence);
 
+	// THREAD
+	adc_thread_id = k_thread_create(
+		&adc_thread_data,
+		adc_thread_stack,
+		K_THREAD_STACK_SIZEOF(adc_thread_stack),
+		adc_thread,
+		NULL, NULL, NULL,
+		ADC_THREAD_PRIORITY,
+		0,
+		K_NO_WAIT
+	);
+
     return 0;
 }
 
@@ -118,3 +177,9 @@ int adc_read_id(int id) {
     
 	return val_mv;
 }
+
+int adc_add_mask(uint32_t mask) {
+	k_msgq_put(&adc_mask_msgq, &mask, K_NO_WAIT);
+
+	return 0;
+}

servo_controller/src/adc.h

@@ -2,12 +2,16 @@
 #define ADC_H
 
 
+#include <stdint.h>
+
+
 #define MUX_CHANNELS 16
 #define NUM_ADC_CHANNELS 18
 
 
 int adc_init_all();
 int adc_read_id(int id);
+int adc_add_mask(uint32_t mask);
 
 
 #endif // ADC_H

servo_controller/src/command_handler.c

@@ -65,29 +65,45 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
-		case ADC_READ: {
+		case ADC_READ_RAW: {
 			int channel = msg->data[0];
 			int value = adc_read_id(channel);
 
 			struct command_message_t *reply = usb_get_next_tx_buf();
-			command_create_message(reply, sizeof(value), ADC_READ, (uint8_t *)&value);
+			command_create_message(reply, sizeof(value), msg->command, (uint8_t *)&value);
 			usb_send_command(reply);
 
 			break;
 		}
-		case ADC_READ_ALL: {
+		case ADC_READ_RAW_ALL: {
 			int values[NUM_ADC_CHANNELS];
 			for (int i = 0; i < NUM_ADC_CHANNELS; i++) {
 				values[i] = adc_read_id(i);
 			}
 
 			struct command_message_t *reply = usb_get_next_tx_buf();
-			command_create_message(reply, sizeof(values), ADC_READ_ALL, (uint8_t *)values);
+			command_create_message(reply, sizeof(values), msg->command, (uint8_t *)values);
 			usb_send_command(reply);
 			
 			break;
 		}
 
+		case ADC_READ: {
+			uint32_t mask;
+			memcpy(&mask, &msg->data[0], sizeof(uint32_t));
+
+			adc_add_mask(mask);
+
+			break;
+		}
+		case ADC_READ_ALL: {
+			uint32_t mask = 0b1111111111111111;
+
+			adc_add_mask(mask);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;

servo_controller/src/command_message.h

@@ -17,8 +17,11 @@ typedef enum {
 	LED_SET,
 	SERVO_SET,
 	SERVO_SET_ALL,
+	ADC_READ_RAW,
+	ADC_READ_RAW_ALL,
 	ADC_READ,
 	ADC_READ_ALL,
+	ADC_SET_READ,
 
 	// Keep last
 	NUM_COMMANDS,