Added timer trigger for continuous ADC, added command to set the ADC read timer interval

servo_controller/scripts/test_adc_set_interval.py

@@ -0,0 +1,129 @@
+#!/usr/bin/env python3
+
+import serial
+import struct
+import threading
+import time
+
+PORT = "/dev/ttyACM0"
+BAUDRATE = 115200
+
+COMMAND_PREFIX = 0x69
+
+COMMAND_ACK = 0
+COMMAND_NACK = 1
+LED_TOGGLE = 2
+LED_SET = 3
+SERVO_SET = 4
+SERVO_SET_ALL = 5
+ADC_READ_RAW = 6
+ADC_READ_RAW_ALL = 7
+ADC_READ = 8
+ADC_READ_ALL = 9
+ADC_SET_READ = 10
+
+DEVICE_ID = 0
+
+
+def calculate_crc(msg: bytes) -> int:
+	s = sum(msg) & 0xFF
+	return (-s) & 0xFF
+
+
+def make_packet(command: int, data: bytes = b"") -> bytes:
+	length = len(data)
+
+	pkt = bytearray()
+	pkt.append(COMMAND_PREFIX)
+	pkt.append(length)
+	pkt.append(DEVICE_ID)
+	pkt.append(command)
+	pkt.append(0)			  # CRC placeholder
+	pkt.extend(data)
+
+	pkt[4] = calculate_crc(pkt[:4] + pkt[5:])
+
+	return bytes(pkt)
+
+
+def verify_crc(packet: bytes) -> bool:
+	crc = packet[4]
+	calc = calculate_crc(packet[:4] + packet[5:])
+	return crc == calc
+
+
+def packet_size(buf: bytes):
+	if len(buf) < 2:
+		return None
+	return 5 + buf[1]
+
+
+def reader(ser):
+	rx = bytearray()
+
+	while True:
+		data = ser.read(64)
+		if not data:
+			continue
+
+		rx.extend(data)
+
+		while rx:
+			# Binary packet?
+			if rx[0] == COMMAND_PREFIX:
+				size = packet_size(rx)
+				if size is None or len(rx) < size:
+					break
+
+				pkt = bytes(rx[:size])
+				del rx[:size]
+
+				if not verify_crc(pkt):
+					print("RX: Bad CRC:", pkt.hex())
+					continue
+
+				length = pkt[1]
+				dev_id = pkt[2]
+				cmd = pkt[3]
+
+				if cmd == COMMAND_ACK:
+					print(f"<-- ACK (device={dev_id})")
+
+				elif cmd == COMMAND_NACK:
+					print(f"<-- NACK (device={dev_id})")
+
+				else:
+					print(f"<-- Command {cmd} len={length}")
+					print(f"<-- RX: {pkt.hex(' ')} ")
+
+			else:
+				# ASCII log output
+				idx = rx.find(b'\n')
+				if idx == -1:
+					break
+
+				line = rx[:idx + 1]
+				del rx[:idx + 1]
+
+				try:
+					print("[LOG]", line.decode().rstrip())
+				except UnicodeDecodeError:
+					print("[RAW]", line.hex())
+
+
+def main():
+	ser = serial.Serial(PORT, BAUDRATE, timeout=0.05)
+
+
+	data = bytearray()
+	data.extend((1).to_bytes(4, byteorder="little"))
+
+	ser.write(make_packet(ADC_SET_READ, data))
+	threading.Thread(target=reader, args=(ser,), daemon=True).start()
+
+	print("Done.")
+	time.sleep(10)
+
+
+if __name__ == "__main__":
+	main()

servo_controller/src/adc.c

@@ -32,6 +32,14 @@ K_THREAD_STACK_DEFINE(adc_thread_stack, ADC_THREAD_STACK_SIZE);
 char adc_mask_msgq_buffer[ADC_MASK_BUFFER_SIZE * sizeof(uint32_t)];
 struct k_msgq adc_mask_msgq;
 
+// ADC TIMER
+struct k_timer adc_timer;
+
+
+static void adc_timer_handler(struct k_timer *timer) {
+    // Timer trigger
+	adc_add_mask(0b1111111111111111);
+}
 
 static void adc_thread(void *p1, void *p2, void *p3) {
 	ARG_UNUSED(p1);
@@ -65,8 +73,12 @@ static void adc_thread(void *p1, void *p2, void *p3) {
 int adc_init_all(void) {
 	int ret;
 
+	// MSGQ
 	k_msgq_init(&adc_mask_msgq, adc_mask_msgq_buffer, sizeof(uint32_t), ADC_MASK_BUFFER_SIZE);
 
+	// TIMER
+	k_timer_init(&adc_timer, adc_timer_handler, NULL);
+
 	// ret = mux_init();
 	// if (ret != 0) {
 	// 	LOG_ERR("Failed to setup multiplexer: %d", ret);
@@ -181,5 +193,17 @@ int adc_read_id(int id) {
 int adc_add_mask(uint32_t mask) {
 	k_msgq_put(&adc_mask_msgq, &mask, K_NO_WAIT);
 
+	return 0;
+}
+
+int adc_timer_set(int interval_ms) {
+
+	if (interval_ms > 0) {
+		k_timer_start(&adc_timer, K_MSEC(interval_ms), K_MSEC(interval_ms));
+	}
+	else {
+		k_timer_stop(&adc_timer);
+	}
+	
 	return 0;
 }

servo_controller/src/adc.h

@@ -12,6 +12,7 @@
 int adc_init_all();
 int adc_read_id(int id);
 int adc_add_mask(uint32_t mask);
+int adc_timer_set(int interval_ms);
 
 
 #endif // ADC_H

servo_controller/src/command_handler.c

@@ -96,6 +96,7 @@ int command_handler(struct command_message_t *msg) {
 
 			break;
 		}
+
 		case ADC_READ_ALL: {
 			uint32_t mask = 0b1111111111111111;
 
@@ -104,6 +105,15 @@ int command_handler(struct command_message_t *msg) {
 			break;
 		}
 
+		case ADC_SET_READ: {
+			int interval_ms;
+			memcpy(&interval_ms, &msg->data[0], sizeof(int));
+
+			adc_timer_set(interval_ms);
+			
+			break;
+		}
+
 		default: {
 			LOG_WRN("Unknown command received: %d", msg->command);
 			return -EINVAL;