Added thread safety to USB TX, servo control thread, primitive ADC thread, python test codes
This commit is contained in:
Your Name
@@ -1,5 +1,6 @@
|
||||
CONFIG_GPIO=y
|
||||
CONFIG_PWM=y
|
||||
CONFIG_ADC=y
|
||||
|
||||
# Serial
|
||||
CONFIG_SERIAL=y
|
||||
@@ -19,7 +20,8 @@ CONFIG_USBD_SELF_POWERED=y
|
||||
CONFIG_USBD_MAX_POWER=125
|
||||
|
||||
# LOG
|
||||
CONFIG_LOG=n
|
||||
CONFIG_LOG=y
|
||||
CONFIG_CBPRINTF_FP_SUPPORT=y # For logging float values
|
||||
CONFIG_USBD_CDC_ACM_LOG_LEVEL_OFF=y # This removes a pointless warning
|
||||
CONFIG_LOG_DEFAULT_LEVEL=3
|
||||
CONFIG_LOG_MODE_IMMEDIATE=y
|
||||
|
||||
@@ -12,7 +12,10 @@ COMMAND_PREFIX = 0x69
|
||||
|
||||
COMMAND_ACK = 0
|
||||
COMMAND_NACK = 1
|
||||
LED = 2
|
||||
LED_TOGGLE = 2
|
||||
LED_SET = 3
|
||||
SERVO_SET = 4
|
||||
SERVO_SET_ALL = 5
|
||||
|
||||
DEVICE_ID = 0
|
||||
|
||||
@@ -112,7 +115,7 @@ def main():
|
||||
|
||||
for i in range(10):
|
||||
print(f"--> Sending LED command {i + 1}")
|
||||
ser.write(make_packet(LED))
|
||||
ser.write(make_packet(LED_TOGGLE))
|
||||
time.sleep(0.5)
|
||||
|
||||
print("Done.")
|
||||
|
||||
@@ -0,0 +1,126 @@
|
||||
#!/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 = 6
|
||||
ADC_READ_ALL = 7
|
||||
|
||||
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)
|
||||
|
||||
threading.Thread(target=reader, args=(ser,), daemon=True).start()
|
||||
|
||||
data = bytearray()
|
||||
data.append(17)
|
||||
|
||||
ser.write(make_packet(ADC_READ, data))
|
||||
|
||||
print("Done.")
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,125 @@
|
||||
#!/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 = 6
|
||||
ADC_READ_ALL = 7
|
||||
|
||||
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)
|
||||
|
||||
threading.Thread(target=reader, args=(ser,), daemon=True).start()
|
||||
|
||||
data = bytearray()
|
||||
|
||||
ser.write(make_packet(ADC_READ_ALL, data))
|
||||
|
||||
print("Done.")
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,128 @@
|
||||
#!/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
|
||||
|
||||
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})")
|
||||
|
||||
elif cmd == COMMAND_ERROR:
|
||||
print(f"<-- ERROR (device={dev_id})")
|
||||
|
||||
else:
|
||||
print(f"<-- Command {cmd} len={length}")
|
||||
|
||||
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)
|
||||
|
||||
threading.Thread(target=reader, args=(ser,), daemon=True).start()
|
||||
|
||||
angle = 0.123
|
||||
data = bytearray()
|
||||
for i in range(18):
|
||||
data.extend(struct.pack('<f', angle))
|
||||
|
||||
ser.write(make_packet(SERVO_SET_ALL, data))
|
||||
|
||||
print("Done.")
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,128 @@
|
||||
#!/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
|
||||
|
||||
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})")
|
||||
|
||||
elif cmd == COMMAND_ERROR:
|
||||
print(f"<-- ERROR (device={dev_id})")
|
||||
|
||||
else:
|
||||
print(f"<-- Command {cmd} len={length}")
|
||||
|
||||
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)
|
||||
|
||||
threading.Thread(target=reader, args=(ser,), daemon=True).start()
|
||||
|
||||
angle = 0.123
|
||||
data = bytearray()
|
||||
data.append(0)
|
||||
data.extend(struct.pack('<f', angle))
|
||||
|
||||
ser.write(make_packet(SERVO_SET, data))
|
||||
|
||||
print("Done.")
|
||||
time.sleep(2)
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@@ -0,0 +1,120 @@
|
||||
#include "adc.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/drivers/adc.h>
|
||||
|
||||
LOG_MODULE_REGISTER(adc, LOG_LEVEL_INF);
|
||||
|
||||
|
||||
#define ZEPHYR_USER_NODE DT_PATH(zephyr_user)
|
||||
static const struct adc_dt_spec adc_mux_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 0);
|
||||
static const struct adc_dt_spec adc_ch16_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 1);
|
||||
static const struct adc_dt_spec adc_ch17_spec = ADC_DT_SPEC_GET_BY_IDX(ZEPHYR_USER_NODE, 2);
|
||||
|
||||
int16_t adc_buffer;
|
||||
struct adc_sequence sequence = {
|
||||
.buffer = &adc_buffer,
|
||||
.buffer_size = sizeof(adc_buffer),
|
||||
};
|
||||
|
||||
int adc_init_all(void) {
|
||||
int 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");
|
||||
return -1;
|
||||
}
|
||||
if (!adc_is_ready_dt(&adc_ch16_spec)) {
|
||||
LOG_ERR("ADC channel 17 not ready");
|
||||
return -1;
|
||||
}
|
||||
if (!adc_is_ready_dt(&adc_ch17_spec)) {
|
||||
LOG_ERR("ADC channel 18 not ready");
|
||||
return -1;
|
||||
}
|
||||
|
||||
ret = adc_channel_setup_dt(&adc_mux_spec);
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to setup ADC mux: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
ret = adc_channel_setup_dt(&adc_ch16_spec);
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to setup ADC channel 17: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
ret = adc_channel_setup_dt(&adc_ch17_spec);
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to setup ADC channel 18: %d", ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
adc_sequence_init_dt(&adc_mux_spec, &sequence);
|
||||
adc_sequence_init_dt(&adc_ch16_spec, &sequence);
|
||||
adc_sequence_init_dt(&adc_ch17_spec, &sequence);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
int adc_read_id(int id) {
|
||||
int ret;
|
||||
int val_mv = 0;
|
||||
adc_buffer = 0;
|
||||
|
||||
if (id < MUX_CHANNELS) {
|
||||
// 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 = adc_read_dt(&adc_mux_spec, &sequence);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Could not read (%d)\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
val_mv = (int)adc_buffer;
|
||||
// ret = adc_raw_to_millivolts_dt(&adc_mux_spec, &val_mv);
|
||||
}
|
||||
else if (id == MUX_CHANNELS) {
|
||||
// Read unmuxed channel 17
|
||||
(void)adc_sequence_init_dt(&adc_ch16_spec, &sequence);
|
||||
|
||||
ret = adc_read_dt(&adc_ch16_spec, &sequence);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Could not read (%d)\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
val_mv = (int)adc_buffer;
|
||||
// ret = adc_raw_to_millivolts_dt(&adc_ch16_spec, &val_mv);
|
||||
}
|
||||
else if (id == (MUX_CHANNELS + 1)) {
|
||||
// Read unmuxed channel 18
|
||||
(void)adc_sequence_init_dt(&adc_ch17_spec, &sequence);
|
||||
|
||||
ret = adc_read_dt(&adc_ch17_spec, &sequence);
|
||||
if (ret < 0) {
|
||||
LOG_ERR("Could not read (%d)\n", ret);
|
||||
return 0;
|
||||
}
|
||||
|
||||
val_mv = (int)adc_buffer;
|
||||
// ret = adc_raw_to_millivolts_dt(&adc_ch17_spec, &val_mv);
|
||||
}
|
||||
|
||||
|
||||
return val_mv;
|
||||
}
|
||||
@@ -0,0 +1,13 @@
|
||||
#ifndef ADC_H
|
||||
#define ADC_H
|
||||
|
||||
|
||||
#define MUX_CHANNELS 16
|
||||
#define NUM_ADC_CHANNELS 18
|
||||
|
||||
|
||||
int adc_init_all();
|
||||
int adc_read_id(int id);
|
||||
|
||||
|
||||
#endif // ADC_H
|
||||
@@ -1,5 +1,9 @@
|
||||
#include "command_handler.h"
|
||||
#include "command_message.h"
|
||||
#include "led.h"
|
||||
#include "servo.h"
|
||||
#include "adc.h"
|
||||
#include "usb.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
|
||||
@@ -37,6 +41,53 @@ int command_handler(struct command_message_t *msg) {
|
||||
break;
|
||||
}
|
||||
|
||||
case SERVO_SET: {
|
||||
// Set SERVO
|
||||
int servo = msg->data[0];
|
||||
float angle;
|
||||
memcpy(&angle, &msg->data[1], sizeof(float));
|
||||
|
||||
servo_set_angle(servo, angle);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case SERVO_SET_ALL: {
|
||||
// Set ALL SERVO
|
||||
float angles[NUM_SERVO_CHANNELS];
|
||||
for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
||||
int start = i*4;
|
||||
memcpy(&angles[i], &msg->data[start], sizeof(float));
|
||||
}
|
||||
|
||||
servo_set_all_angles(angles);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
case ADC_READ: {
|
||||
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);
|
||||
usb_send_command(reply);
|
||||
|
||||
break;
|
||||
}
|
||||
case ADC_READ_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);
|
||||
usb_send_command(reply);
|
||||
|
||||
break;
|
||||
}
|
||||
|
||||
default: {
|
||||
LOG_WRN("Unknown command received: %d", msg->command);
|
||||
return -EINVAL;
|
||||
|
||||
@@ -11,7 +11,7 @@ void command_message_init(struct command_message_t *msg) {
|
||||
msg->id = COMMAND_ID;
|
||||
}
|
||||
|
||||
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]) {
|
||||
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]) {
|
||||
// Ensure length doesn't exceed available space
|
||||
if (length > sizeof(msg->data) - 1) {
|
||||
return;
|
||||
|
||||
@@ -8,12 +8,20 @@
|
||||
#define COMMAND_PREFIX 0x69
|
||||
#define COMMAND_ID 0x00
|
||||
#define COMMAND_DATA_SIZE 160
|
||||
#define COMMAND_HEADER_SIZE 5
|
||||
|
||||
typedef enum {
|
||||
COMMAND_ACK,
|
||||
COMMAND_NACK,
|
||||
LED_TOGGLE,
|
||||
LED_SET,
|
||||
SERVO_SET,
|
||||
SERVO_SET_ALL,
|
||||
ADC_READ,
|
||||
ADC_READ_ALL,
|
||||
|
||||
// Keep last
|
||||
NUM_COMMANDS,
|
||||
} commands_e;
|
||||
|
||||
struct command_message_t {
|
||||
@@ -40,7 +48,7 @@ void command_message_init(struct command_message_t *msg);
|
||||
* @param command Command type
|
||||
* @param data Data payload
|
||||
*/
|
||||
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[160]);
|
||||
void command_create_message(struct command_message_t *msg, uint8_t length, commands_e command, uint8_t data[]);
|
||||
|
||||
/**
|
||||
* @brief Calculate CRC for command message
|
||||
|
||||
@@ -1,5 +1,7 @@
|
||||
#include "led.h"
|
||||
#include "usb.h"
|
||||
#include "servo.h"
|
||||
#include "adc.h"
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
|
||||
@@ -8,17 +10,33 @@ LOG_MODULE_REGISTER(main, LOG_LEVEL_INF);
|
||||
int main(void) {
|
||||
int ret;
|
||||
|
||||
// USB init
|
||||
ret = usb_init();
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to enable USB");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// LED init
|
||||
ret = led_init();
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to enable LED");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// ADC init before SERVO
|
||||
ret = adc_init_all();
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to enable ADC");
|
||||
return 0;
|
||||
}
|
||||
|
||||
// SERVO init
|
||||
ret = servo_init();
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to enable SERVO");
|
||||
return 0;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -2,10 +2,14 @@
|
||||
|
||||
#include <zephyr/logging/log.h>
|
||||
#include <zephyr/drivers/pwm.h>
|
||||
#include <zephyr/kernel.h>
|
||||
|
||||
#include <math.h>
|
||||
|
||||
LOG_MODULE_REGISTER(servo, LOG_LEVEL_INF);
|
||||
|
||||
// ----------- SERVOS -----------
|
||||
|
||||
// SERVOS
|
||||
static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
|
||||
PWM_DT_SPEC_GET(DT_ALIAS(servo1)),
|
||||
PWM_DT_SPEC_GET(DT_ALIAS(servo2)),
|
||||
@@ -26,12 +30,119 @@ static const struct pwm_dt_spec servo_pwm_specs[NUM_SERVO_CHANNELS] = {
|
||||
PWM_DT_SPEC_GET(DT_ALIAS(servo17)),
|
||||
PWM_DT_SPEC_GET(DT_ALIAS(servo18)),
|
||||
};
|
||||
static struct servo_t servos[NUM_SERVO_CHANNELS];
|
||||
|
||||
// THREAD
|
||||
static struct k_thread servo_thread_data;
|
||||
static k_tid_t servo_thread_id = NULL;
|
||||
#define SERVO_THREAD_STACK_SIZE 2048
|
||||
K_THREAD_STACK_DEFINE(servo_thread_stack, SERVO_THREAD_STACK_SIZE);
|
||||
struct k_sem servo_semaphore;
|
||||
|
||||
|
||||
static int pin_init(const ) {
|
||||
static void servo_thread(void *p1, void *p2, void *p3) {
|
||||
ARG_UNUSED(p1);
|
||||
ARG_UNUSED(p2);
|
||||
ARG_UNUSED(p3);
|
||||
|
||||
LOG_INF("Servo control thread started");
|
||||
|
||||
while (1) {
|
||||
k_sem_take(&servo_semaphore, K_FOREVER);
|
||||
|
||||
// TODO: acceleration?
|
||||
|
||||
for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
||||
servo_set(i, servos[i].set_position);
|
||||
servos[i].current_position = servos[i].set_position;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int servo_init() {
|
||||
int ret;
|
||||
|
||||
k_sem_init(&servo_semaphore, 0, 1);
|
||||
memset(servos, 0, sizeof(servos));
|
||||
|
||||
for (int ch = 0; ch < NUM_SERVO_CHANNELS; ch++) {
|
||||
if (!pwm_is_ready_dt(&servo_pwm_specs[ch])) {
|
||||
LOG_ERR("PWM device for servo %d is not ready", ch + 1);
|
||||
return -ENODEV;
|
||||
}
|
||||
}
|
||||
|
||||
for (int ch = 0; ch < NUM_SERVO_CHANNELS; ch++) {
|
||||
ret = pwm_set_pulse_dt(&servo_pwm_specs[ch], PWM_USEC(0));
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to initialize PWM for servo %d: %d", ch + 1, ret);
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
|
||||
servo_thread_id = k_thread_create(
|
||||
&servo_thread_data,
|
||||
servo_thread_stack,
|
||||
K_THREAD_STACK_SIZEOF(servo_thread_stack),
|
||||
servo_thread,
|
||||
NULL, NULL, NULL,
|
||||
5,
|
||||
0,
|
||||
K_NO_WAIT
|
||||
);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void servo_set_angle(int channel, float angle) {
|
||||
servos[channel].set_position = angle;
|
||||
LOG_DBG("Settings servo %d to %f", channel, (double)angle);
|
||||
k_sem_give(&servo_semaphore);
|
||||
}
|
||||
|
||||
void servo_set_all_angles(float angles[NUM_SERVO_CHANNELS]) {
|
||||
for (int i = 0; i < NUM_SERVO_CHANNELS; i++) {
|
||||
servos[i].set_position = angles[i];
|
||||
LOG_DBG("Settings servo %d to %f", i, (double)angles[i]);
|
||||
}
|
||||
k_sem_give(&servo_semaphore);
|
||||
}
|
||||
|
||||
static uint32_t servo_angle_to_pulse_us(float angle) {
|
||||
// Check limits
|
||||
if (angle > SERVO_MAX_ANGLE) {
|
||||
return SERVO_MAX_PULSE;
|
||||
} else if (angle < SERVO_MIN_ANGLE) {
|
||||
return SERVO_MIN_PULSE;
|
||||
}
|
||||
|
||||
// Angle to pulse
|
||||
float pulse_offset_us = angle * SERVO_ANGLE_PULSE_MULTIPLIER;
|
||||
uint32_t pulse_us = (uint32_t)(SERVO_CENTER_PULSE + pulse_offset_us);
|
||||
|
||||
if (pulse_us < SERVO_MIN_PULSE) {
|
||||
return SERVO_MIN_PULSE;
|
||||
} else if (pulse_us > SERVO_MAX_PULSE) {
|
||||
return SERVO_MAX_PULSE;
|
||||
}
|
||||
|
||||
return pulse_us;
|
||||
}
|
||||
|
||||
int servo_set(int channel, float angle) {
|
||||
if (channel >= NUM_SERVO_CHANNELS || channel < 0) {
|
||||
LOG_ERR("Servo channel exceeds NUM_SERVO_CHANNELS");
|
||||
return -EINVAL;
|
||||
}
|
||||
|
||||
float effective_angle = angle;
|
||||
|
||||
uint32_t pulse_us = servo_angle_to_pulse_us(effective_angle);
|
||||
int ret = pwm_set_pulse_dt(&servo_pwm_specs[channel], PWM_USEC(pulse_us));
|
||||
if (ret != 0) {
|
||||
LOG_ERR("Failed to set PWM pulse for servo %d: %d", channel + 1, ret);
|
||||
return ret;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -2,12 +2,26 @@
|
||||
#define SERVO_H
|
||||
|
||||
|
||||
|
||||
#define NUM_SERVO_CHANNELS 18
|
||||
#define SERVO_CENTER_PULSE 1500
|
||||
#define SERVO_MIN_PULSE 750
|
||||
#define SERVO_MAX_PULSE 2500
|
||||
#define SERVO_HALF_PULSE_RANGE 875
|
||||
#define SERVO_MAX_ANGLE 2.3561944902f
|
||||
#define SERVO_MIN_ANGLE -2.3561944902f
|
||||
#define SERVO_ANGLE_PULSE_MULTIPLIER 371.3615338798826f
|
||||
|
||||
|
||||
struct servo_t {
|
||||
float current_position;
|
||||
float set_position;
|
||||
};
|
||||
|
||||
|
||||
int servo_init();
|
||||
void servo_set_angle(int channel, float angle);
|
||||
void servo_set_all_angles(float angles[NUM_SERVO_CHANNELS]);
|
||||
int servo_set(int channel, float angle);
|
||||
|
||||
|
||||
#endif // SERVO_H
|
||||
+88
-32
@@ -17,11 +17,11 @@ LOG_MODULE_REGISTER(usb, LOG_LEVEL_INF);
|
||||
const struct device *const uart_dev = DEVICE_DT_GET_ONE(zephyr_cdc_acm_uart);
|
||||
static struct usbd_context *usb_context;
|
||||
|
||||
// THREAD
|
||||
static struct k_thread usb_thread_data;
|
||||
static k_tid_t usb_thread_id = NULL;
|
||||
#define USB_THREAD_STACK_SIZE 2048
|
||||
K_THREAD_STACK_DEFINE(usb_thread_stack, USB_THREAD_STACK_SIZE);
|
||||
// RX THREAD
|
||||
static struct k_thread usb_rx_thread_data;
|
||||
static k_tid_t usb_rx_thread_id = NULL;
|
||||
#define USB_RX_THREAD_STACK_SIZE 2048
|
||||
K_THREAD_STACK_DEFINE(usb_rx_thread_stack, USB_RX_THREAD_STACK_SIZE);
|
||||
|
||||
// RX BUFFER
|
||||
#define RING_BUF_SIZE 255
|
||||
@@ -29,10 +29,21 @@ static uint8_t ring_buffer[RING_BUF_SIZE];
|
||||
static struct ring_buf ringbuf;
|
||||
struct k_sem rx_semaphore;
|
||||
|
||||
// TX THREAD
|
||||
static struct k_thread usb_tx_thread_data;
|
||||
static k_tid_t usb_tx_thread_id = NULL;
|
||||
#define USB_TX_THREAD_STACK_SIZE 512
|
||||
K_THREAD_STACK_DEFINE(usb_tx_thread_stack, USB_TX_THREAD_STACK_SIZE);
|
||||
|
||||
// TX BUFFER (add ack and nack at the and as static)
|
||||
#define TX_BUFFER_SIZE 10
|
||||
struct command_message_t usb_tx_buffer[TX_BUFFER_SIZE + 2];
|
||||
struct command_message_t *usb_tx_buf_ptr;
|
||||
char usb_tx_ptr_msgq_buffer[(TX_BUFFER_SIZE + 2) * sizeof(struct command_message_t *)];
|
||||
struct k_msgq usb_tx_ptr_msgq;
|
||||
|
||||
// ACK / NACK messages
|
||||
#define RETURN_ACK true
|
||||
struct command_message_t ack_msg;
|
||||
struct command_message_t nack_msg;
|
||||
|
||||
|
||||
static void interrupt_handler(const struct device *dev, void *user_data) {
|
||||
@@ -73,7 +84,7 @@ static void interrupt_handler(const struct device *dev, void *user_data) {
|
||||
}
|
||||
}
|
||||
|
||||
static void usb_thread(void *p1, void *p2, void *p3) {
|
||||
static void usb_rx_thread(void *p1, void *p2, void *p3) {
|
||||
ARG_UNUSED(p1);
|
||||
ARG_UNUSED(p2);
|
||||
ARG_UNUSED(p3);
|
||||
@@ -109,7 +120,7 @@ static void usb_thread(void *p1, void *p2, void *p3) {
|
||||
if (calculated_crc != msg.crc) {
|
||||
if (RETURN_ACK) {
|
||||
// Send NACK
|
||||
usb_send_command(&nack_msg);
|
||||
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
|
||||
}
|
||||
continue;
|
||||
}
|
||||
@@ -118,13 +129,13 @@ static void usb_thread(void *p1, void *p2, void *p3) {
|
||||
if (ret == 0) {
|
||||
if (RETURN_ACK) {
|
||||
// Send ACK
|
||||
usb_send_command(&ack_msg);
|
||||
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE]);
|
||||
}
|
||||
}
|
||||
else {
|
||||
if (RETURN_ACK) {
|
||||
// Send NACK
|
||||
usb_send_command(&nack_msg);
|
||||
usb_send_command(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -162,12 +173,36 @@ static void usb_msg_cb(struct usbd_context *const ctx, const struct usbd_msg *ms
|
||||
}
|
||||
}
|
||||
|
||||
static void usb_tx_thread(void *p1, void *p2, void *p3) {
|
||||
struct command_message_t *data;
|
||||
|
||||
while (1) {
|
||||
k_msgq_get(&usb_tx_ptr_msgq, &data, K_FOREVER);
|
||||
|
||||
if (!device_is_ready(uart_dev)) {
|
||||
// FIXME: Don't drop packages
|
||||
continue;
|
||||
}
|
||||
|
||||
// Message size: prefix + length + id + command + crc + data
|
||||
size_t msg_size = COMMAND_HEADER_SIZE + data->length;
|
||||
uint8_t *msg_bytes = (uint8_t *)data;
|
||||
|
||||
/* uart_poll_out blocks until sent, ensuring data integrity */
|
||||
for (size_t i = 0; i < msg_size; i++) {
|
||||
uart_poll_out(uart_dev, msg_bytes[i]);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int usb_init() {
|
||||
ring_buf_init(&ringbuf, sizeof(ring_buffer), ring_buffer);
|
||||
k_sem_init(&rx_semaphore, 0, 1);
|
||||
usb_tx_buf_ptr = usb_tx_buffer;
|
||||
k_msgq_init(&usb_tx_ptr_msgq, usb_tx_ptr_msgq_buffer, sizeof(struct command_message_t *), TX_BUFFER_SIZE);
|
||||
|
||||
command_create_ack(&ack_msg);
|
||||
command_create_nack(&nack_msg);
|
||||
command_create_ack(&usb_tx_buffer[TX_BUFFER_SIZE]);
|
||||
command_create_nack(&usb_tx_buffer[TX_BUFFER_SIZE + 1]);
|
||||
|
||||
int ret;
|
||||
|
||||
@@ -194,38 +229,59 @@ int usb_init() {
|
||||
uart_irq_callback_set(uart_dev, interrupt_handler);
|
||||
uart_irq_rx_enable(uart_dev);
|
||||
|
||||
usb_thread_id = k_thread_create(
|
||||
&usb_thread_data,
|
||||
usb_thread_stack,
|
||||
K_THREAD_STACK_SIZEOF(usb_thread_stack),
|
||||
usb_thread,
|
||||
// RX THREAD
|
||||
usb_rx_thread_id = k_thread_create(
|
||||
&usb_rx_thread_data,
|
||||
usb_rx_thread_stack,
|
||||
K_THREAD_STACK_SIZEOF(usb_rx_thread_stack),
|
||||
usb_rx_thread,
|
||||
NULL, NULL, NULL,
|
||||
5,
|
||||
0,
|
||||
K_NO_WAIT
|
||||
);
|
||||
|
||||
if (usb_thread_id == NULL) {
|
||||
LOG_ERR("Failed to create USB thread");
|
||||
if (usb_rx_thread_id == NULL) {
|
||||
LOG_ERR("Failed to create USB RX thread");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
// TX THREAD
|
||||
usb_tx_thread_id = k_thread_create(
|
||||
&usb_tx_thread_data,
|
||||
usb_tx_thread_stack,
|
||||
K_THREAD_STACK_SIZEOF(usb_tx_thread_stack),
|
||||
usb_tx_thread,
|
||||
NULL, NULL, NULL,
|
||||
5,
|
||||
0,
|
||||
K_NO_WAIT
|
||||
);
|
||||
|
||||
if (usb_tx_thread_id == NULL) {
|
||||
LOG_ERR("Failed to create USB TX thread");
|
||||
return -ENOMEM;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
struct command_message_t* usb_get_next_tx_buf() {
|
||||
struct command_message_t *buf = usb_tx_buf_ptr;
|
||||
|
||||
// Increment the buffer pointer
|
||||
usb_tx_buf_ptr++;
|
||||
|
||||
if (usb_tx_buf_ptr > &usb_tx_buffer[TX_BUFFER_SIZE-1]) {
|
||||
usb_tx_buf_ptr = usb_tx_buffer;
|
||||
}
|
||||
|
||||
return buf;
|
||||
}
|
||||
|
||||
int usb_send_command(struct command_message_t *msg) {
|
||||
if (!device_is_ready(uart_dev)) {
|
||||
return -ENODEV;
|
||||
}
|
||||
|
||||
// Message size: prefix + length + id + command + crc + data
|
||||
size_t msg_size = 5 + msg->length;
|
||||
uint8_t *msg_bytes = (uint8_t *)msg;
|
||||
|
||||
/* uart_poll_out blocks until sent, ensuring data integrity */
|
||||
for (size_t i = 0; i < msg_size; i++) {
|
||||
uart_poll_out(uart_dev, msg_bytes[i]);
|
||||
}
|
||||
|
||||
k_msgq_put(&usb_tx_ptr_msgq, &msg, K_NO_WAIT);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@@ -6,6 +6,7 @@
|
||||
|
||||
|
||||
int usb_init();
|
||||
struct command_message_t* usb_get_next_tx_buf();
|
||||
int usb_send_command(struct command_message_t *msg);
|
||||
|
||||
|
||||
|
||||
Reference in New Issue
Block a user