diff --git a/servo_controller/scripts/test_adc_set_interval.py b/servo_controller/scripts/test_adc_set_interval.py new file mode 100644 index 0000000..82269a9 --- /dev/null +++ b/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() diff --git a/servo_controller/src/adc.c b/servo_controller/src/adc.c index bfdc939..aa9a226 100644 --- a/servo_controller/src/adc.c +++ b/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; } \ No newline at end of file diff --git a/servo_controller/src/adc.h b/servo_controller/src/adc.h index 560c574..a99ad6c 100644 --- a/servo_controller/src/adc.h +++ b/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 diff --git a/servo_controller/src/command_handler.c b/servo_controller/src/command_handler.c index 70d9845..f8f47ae 100644 --- a/servo_controller/src/command_handler.c +++ b/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;