From 9e84f39fb18e6f3e33248a025855a35a79a612b6 Mon Sep 17 00:00:00 2001 From: ggw Date: Wed, 29 Apr 2026 11:18:49 -0500 Subject: [PATCH] imu and adc work, but not at the same time x.x --- code/l452_code/Core/Inc/packet.hpp | 18 +++++++--- code/l452_code/Core/Src/ads131.cpp | 17 +++------ code/l452_code/Core/Src/datadescriptor.cpp | 8 ++--- code/l452_code/Core/Src/lsm6dsv.cpp | 35 +++++++++++++----- code/l452_code/Core/Src/main.cpp | 2 +- code/l452_code/Core/Src/spi.c | 2 +- code/l452_code/packet_parser.py | 42 ++++++++++++++++------ 7 files changed, 83 insertions(+), 41 deletions(-) diff --git a/code/l452_code/Core/Inc/packet.hpp b/code/l452_code/Core/Inc/packet.hpp index 67455f6..81c2568 100644 --- a/code/l452_code/Core/Inc/packet.hpp +++ b/code/l452_code/Core/Inc/packet.hpp @@ -2,6 +2,16 @@ #include "rtc.h" +__inline__ int32_t to_signed_24(uint8_t* p){ + uint32_t value = (((uint32_t)p[0] << 16) | + ((uint32_t)p[1] << 8) | + (uint32_t)p[2]); + if (value & 0x800000) { + value |= 0xFF000000; + } + return (int32_t)value; +} + struct packet_rtc { uint32_t t; RTC_TimeTypeDef sTime; @@ -21,25 +31,25 @@ struct packet_imu { struct packet_ekg { uint32_t t; uint8_t index; - uint32_t readings_cnts[50]; + int32_t readings_cnts[50]; }; struct packet_strain { uint32_t t; uint8_t index; - uint32_t readings_cnts[5]; + int32_t readings_cnts[5]; }; struct packet_outsideT { uint32_t t; uint8_t index; - uint32_t readings_cnts[5]; + int32_t readings_cnts[5]; }; struct packet_insideT { uint32_t t; uint8_t index; - uint32_t readings_cnts[5]; + int32_t readings_cnts[5]; }; struct packet_button { diff --git a/code/l452_code/Core/Src/ads131.cpp b/code/l452_code/Core/Src/ads131.cpp index 1cb06e3..078aab3 100644 --- a/code/l452_code/Core/Src/ads131.cpp +++ b/code/l452_code/Core/Src/ads131.cpp @@ -73,9 +73,8 @@ void read_ads131(SPI_HandleTypeDef *hspi1) { p_ekg.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; } - p_ekg.readings_cnts[p_ekg.index] = (((uint32_t)rx_buff[3]<<16) + - ((uint32_t)rx_buff[4]<<8) + - ((uint32_t)rx_buff[5])); + p_ekg.readings_cnts[p_ekg.index] = to_signed_24(&rx_buff[3]); + if (p_ekg.index == 49) { write(p_ekg); p_ekg.index = 0; @@ -86,9 +85,7 @@ void read_ads131(SPI_HandleTypeDef *hspi1) { if (p_strain.index == 0) { p_strain.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; } - p_strain.readings_cnts[p_strain.index / 10] += (((uint32_t)rx_buff[6]<<16) + - ((uint32_t)rx_buff[7]<<8) + - ((uint32_t)rx_buff[8])); + p_strain.readings_cnts[p_strain.index / 10] += to_signed_24(&rx_buff[6]); if (p_strain.index == 49) { write(p_strain); @@ -101,9 +98,7 @@ void read_ads131(SPI_HandleTypeDef *hspi1) { if (p_outsideT.index == 0) { p_outsideT.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; } - p_outsideT.readings_cnts[p_outsideT.index / 10] += (((uint32_t)rx_buff[9]<<16) + - ((uint32_t)rx_buff[10]<<8) + - ((uint32_t)rx_buff[11])); + p_outsideT.readings_cnts[p_outsideT.index / 10] += to_signed_24(&rx_buff[9]); if (p_outsideT.index == 49) { write(p_outsideT); @@ -116,9 +111,7 @@ void read_ads131(SPI_HandleTypeDef *hspi1) { if (p_insideT.index == 0) { p_insideT.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; } - p_insideT.readings_cnts[p_insideT.index / 10] += (((uint32_t)rx_buff[12]<<16) + - ((uint32_t)rx_buff[13]<<8) + - ((uint32_t)rx_buff[14])); + p_insideT.readings_cnts[p_insideT.index / 10] += to_signed_24(&rx_buff[12]); if (p_insideT.index == 49) { write(p_insideT); diff --git a/code/l452_code/Core/Src/datadescriptor.cpp b/code/l452_code/Core/Src/datadescriptor.cpp index fa0353f..30fd139 100644 --- a/code/l452_code/Core/Src/datadescriptor.cpp +++ b/code/l452_code/Core/Src/datadescriptor.cpp @@ -20,19 +20,19 @@ void data_description() { CDC_Transmit_FS((uint8_t*)buff, cx); HAL_Delay(10); - cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_ekg", typecode(), sizeof(packet_ekg), "uint32_t t", offsetof(packet_ekg, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_ekg, index), sizeof(uint8_t), "uint32_t readings_cnts[50]", offsetof(packet_ekg, readings_cnts), sizeof(uint32_t)); + cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_ekg", typecode(), sizeof(packet_ekg), "uint32_t t", offsetof(packet_ekg, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_ekg, index), sizeof(uint8_t), "int32_t readings_cnts[50]", offsetof(packet_ekg, readings_cnts), sizeof(int32_t)); CDC_Transmit_FS((uint8_t*)buff, cx); HAL_Delay(10); - cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_strain", typecode(), sizeof(packet_strain), "uint32_t t", offsetof(packet_strain, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_strain, index), sizeof(uint8_t), "uint32_t readings_cnts[5]", offsetof(packet_strain, readings_cnts), sizeof(uint32_t)); + cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_strain", typecode(), sizeof(packet_strain), "uint32_t t", offsetof(packet_strain, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_strain, index), sizeof(uint8_t), "int32_t readings_cnts[5]", offsetof(packet_strain, readings_cnts), sizeof(int32_t)); CDC_Transmit_FS((uint8_t*)buff, cx); HAL_Delay(10); - cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_outsideT", typecode(), sizeof(packet_outsideT), "uint32_t t", offsetof(packet_outsideT, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_outsideT, index), sizeof(uint8_t), "uint32_t readings_cnts[5]", offsetof(packet_outsideT, readings_cnts), sizeof(uint32_t)); + cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_outsideT", typecode(), sizeof(packet_outsideT), "uint32_t t", offsetof(packet_outsideT, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_outsideT, index), sizeof(uint8_t), "int32_t readings_cnts[5]", offsetof(packet_outsideT, readings_cnts), sizeof(int32_t)); CDC_Transmit_FS((uint8_t*)buff, cx); HAL_Delay(10); - cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_insideT", typecode(), sizeof(packet_insideT), "uint32_t t", offsetof(packet_insideT, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_insideT, index), sizeof(uint8_t), "uint32_t readings_cnts[5]", offsetof(packet_insideT, readings_cnts), sizeof(uint32_t)); + cx = snprintf(buff, sizeof(buff), "%s %d %d %s %d %d %s %d %d %s %d %d\n\r", "packet_insideT", typecode(), sizeof(packet_insideT), "uint32_t t", offsetof(packet_insideT, t), sizeof(uint32_t), "uint8_t index", offsetof(packet_insideT, index), sizeof(uint8_t), "int32_t readings_cnts[5]", offsetof(packet_insideT, readings_cnts), sizeof(int32_t)); CDC_Transmit_FS((uint8_t*)buff, cx); HAL_Delay(10); diff --git a/code/l452_code/Core/Src/lsm6dsv.cpp b/code/l452_code/Core/Src/lsm6dsv.cpp index 69c0724..65a6222 100644 --- a/code/l452_code/Core/Src/lsm6dsv.cpp +++ b/code/l452_code/Core/Src/lsm6dsv.cpp @@ -71,6 +71,8 @@ void read_lsm6dsv(SPI_HandleTypeDef *hspi1) { return; } + //CDC_Transmit_FS((uint8_t*) "Detected\n\r", 10); + uint8_t tx_buff[8] = {0}; uint8_t rx_buff[8] = {0}; @@ -81,23 +83,40 @@ void read_lsm6dsv(SPI_HandleTypeDef *hspi1) { tx_buff[0] = 0b10000000 + 0x1B; tx_buff[1] = 0; HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_RESET); - HAL_SPI_TransmitReceive(hspi1, tx_buff, rx_buff, 8, HAL_MAX_DELAY); + HAL_SPI_TransmitReceive(hspi1, tx_buff, rx_buff, 2, HAL_MAX_DELAY); HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_SET); + char buffer[64]; + int cx; + // todo: read in batches if (rx_buff[1] != 0) { + // cx = snprintf(buffer, 64, "%02x %02x\n\r", + // rx_buff[0], + // rx_buff[1]); + // CDC_Transmit_FS((uint8_t*)buffer, cx); tx_buff[0] = 0b10000000 + 0x78; // FIFO addr tx_buff[1] = 0; HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_RESET); HAL_SPI_TransmitReceive(hspi1, tx_buff, rx_buff, 8, HAL_MAX_DELAY); HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_SET); - } - p_imu.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; - p_imu.readings_cnts[0] = rx_buff[1]; - p_imu.readings_cnts[1] = ((uint16_t)rx_buff[2] << 8) + rx_buff[3]; - p_imu.readings_cnts[2] = ((uint16_t)rx_buff[4] << 8) + rx_buff[5]; - p_imu.readings_cnts[3] = ((uint16_t)rx_buff[6] << 8) + rx_buff[7]; - write(p_imu); + // cx = snprintf(buffer, 64, "%02x %02x %02x %02x %02x %02x %02x %02x\n\r", + // rx_buff[0], + // rx_buff[1], + // rx_buff[2], + // rx_buff[3], + // rx_buff[4], + // rx_buff[5], + // rx_buff[6], + // rx_buff[7]); + // CDC_Transmit_FS((uint8_t*)buffer, cx); + p_imu.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; + p_imu.readings_cnts[0] = rx_buff[1]; + p_imu.readings_cnts[1] = ((uint16_t)rx_buff[2] << 8) + rx_buff[3]; + p_imu.readings_cnts[2] = ((uint16_t)rx_buff[4] << 8) + rx_buff[5]; + p_imu.readings_cnts[3] = ((uint16_t)rx_buff[6] << 8) + rx_buff[7]; + write(p_imu); + } } diff --git a/code/l452_code/Core/Src/main.cpp b/code/l452_code/Core/Src/main.cpp index 6f37e69..6ef2b07 100644 --- a/code/l452_code/Core/Src/main.cpp +++ b/code/l452_code/Core/Src/main.cpp @@ -162,7 +162,7 @@ int main(void) /* USER CODE BEGIN WHILE */ HAL_TIM_Base_Start_IT(&htim6); - initialize_ads131(&hspi1); + //initialize_ads131(&hspi1); initialize_lsm6dsv(&hspi1); while (1) diff --git a/code/l452_code/Core/Src/spi.c b/code/l452_code/Core/Src/spi.c index 5543cf4..fb65223 100644 --- a/code/l452_code/Core/Src/spi.c +++ b/code/l452_code/Core/Src/spi.c @@ -41,7 +41,7 @@ void MX_SPI1_Init(void) hspi1.Init.Mode = SPI_MODE_MASTER; hspi1.Init.Direction = SPI_DIRECTION_2LINES; hspi1.Init.DataSize = SPI_DATASIZE_8BIT; - hspi1.Init.CLKPolarity = SPI_POLARITY_HIGH; + hspi1.Init.CLKPolarity = SPI_POLARITY_LOW; hspi1.Init.CLKPhase = SPI_PHASE_2EDGE; hspi1.Init.NSS = SPI_NSS_SOFT; hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64; diff --git a/code/l452_code/packet_parser.py b/code/l452_code/packet_parser.py index 7cb48af..efe5fca 100644 --- a/code/l452_code/packet_parser.py +++ b/code/l452_code/packet_parser.py @@ -48,12 +48,15 @@ ser.timeout = 5 ys = [] +imu_readings = {} + for i in range(1000): ser.write(b'R') data = ser.read(1024) index = 0 while (index < 1024): packet_type = (data[index + 1]<<8) + data[index] + #print(packet_type) ind = [i for i,t in enumerate(types) if t['type_code'] == packet_type] if len(ind) != 1: break @@ -62,17 +65,34 @@ for i in range(1000): for e in t['elements']: block = d[e['offset']:e['offset'] + e['size']] element_size = int(len(block) / e['n_elements']) - if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]': - chunked = [int.from_bytes(block[i:i + element_size - 1], byteorder='little', signed = True) for i in range(0, len(block), element_size)] - #BB = [block[i:i + element_size] for i in range(0, len(block), element_size)] - #if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]': - ys += chunked - plt.clf() - dat = np.array(ys[-1000:]) - plt.plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat)) - plt.pause(0.01) - ##print(BB) - e['readings'].append(chunked) + # if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]': + # chunked = [(2.4 / (1<<24)) * int.from_bytes(block[i:i + element_size], byteorder='little', signed = True) for i in range(0, len(block), element_size)] + # #BB = [block[i:i + element_size] for i in range(0, len(block), element_size)] + # #if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]': + # ys += chunked + # plt.clf() + # dat = np.array(ys[-1000:]) + # plt.plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat)) + # plt.pause(0.01) + # ##print(BB) + # e['readings'].append(chunked) + if t['type_name'] == b'packet_imu' and e['name'] == b'readings_cnts[4]': + imu_reading_type = block[0] >> 3 + imu_reading_tag_cnt = (block[0] >> 1) & 3 # tag count is sensor time slow + reading_xyz = [int.from_bytes(block[2:4], byteorder = 'big', signed = True), + int.from_bytes(block[4:6], byteorder = 'big', signed = True), + int.from_bytes(block[6:8], byteorder = 'big', signed = True)] + if imu_reading_type == 1: + #print("{:02x} {:d}".format(imu_reading_type, imu_reading_tag_cnt)) + print("Gyro: {:f} {:f} {:f}".format(*[250 * e / (1<<16) for e in reading_xyz])) + elif imu_reading_type == 2: + print("Acc: {:f} {:f} {:f}".format(*[4 * e / (1<<16) for e in reading_xyz])) + else: + print("ERR") + #chunked = [int.from_bytes(block[i:i + element_size], byteorder='little', signed = True) for i in range(0, len(block), element_size)] + #print(chunked) + #print(t['type_name']) + index += 2 + t['size'] #for t in types: