ggw/chest_strap
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

imu and adc work, but not at the same time x.x

Browse Source
This commit is contained in:
ggw
2026-04-29 11:18:49 -05:00
parent 4e4ea8f15e
commit 9e84f39fb1
7 changed files with 83 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
code/l452_code/Core/Inc/packet.hpp
+14 -4
View File
@@ -2,6 +2,16 @@
#include "rtc.h" #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 { struct packet_rtc {
uint32_t t; uint32_t t;
RTC_TimeTypeDef sTime; RTC_TimeTypeDef sTime;
@@ -21,25 +31,25 @@ struct packet_imu {
struct packet_ekg { struct packet_ekg {
uint32_t t; uint32_t t;
uint8_t index; uint8_t index;
uint32_t readings_cnts[50]; int32_t readings_cnts[50];
}; };
struct packet_strain { struct packet_strain {
uint32_t t; uint32_t t;
uint8_t index; uint8_t index;
uint32_t readings_cnts[5]; int32_t readings_cnts[5];
}; };
struct packet_outsideT { struct packet_outsideT {
uint32_t t; uint32_t t;
uint8_t index; uint8_t index;
uint32_t readings_cnts[5]; int32_t readings_cnts[5];
}; };
struct packet_insideT { struct packet_insideT {
uint32_t t; uint32_t t;
uint8_t index; uint8_t index;
uint32_t readings_cnts[5]; int32_t readings_cnts[5];
}; };
struct packet_button { struct packet_button {
code/l452_code/Core/Src/ads131.cpp
+5 -12
View File
@@ -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.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT;
} }
p_ekg.readings_cnts[p_ekg.index] = (((uint32_t)rx_buff[3]<<16) + p_ekg.readings_cnts[p_ekg.index] = to_signed_24(&rx_buff[3]);
((uint32_t)rx_buff[4]<<8) +
((uint32_t)rx_buff[5]));
if (p_ekg.index == 49) { if (p_ekg.index == 49) {
write(p_ekg); write(p_ekg);
p_ekg.index = 0; p_ekg.index = 0;
@@ -86,9 +85,7 @@ void read_ads131(SPI_HandleTypeDef *hspi1) {
if (p_strain.index == 0) { if (p_strain.index == 0) {
p_strain.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; 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) + p_strain.readings_cnts[p_strain.index / 10] += to_signed_24(&rx_buff[6]);
((uint32_t)rx_buff[7]<<8) +
((uint32_t)rx_buff[8]));
if (p_strain.index == 49) { if (p_strain.index == 49) {
write(p_strain); write(p_strain);
@@ -101,9 +98,7 @@ void read_ads131(SPI_HandleTypeDef *hspi1) {
if (p_outsideT.index == 0) { if (p_outsideT.index == 0) {
p_outsideT.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; 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) + p_outsideT.readings_cnts[p_outsideT.index / 10] += to_signed_24(&rx_buff[9]);
((uint32_t)rx_buff[10]<<8) +
((uint32_t)rx_buff[11]));
if (p_outsideT.index == 49) { if (p_outsideT.index == 49) {
write(p_outsideT); write(p_outsideT);
@@ -116,9 +111,7 @@ void read_ads131(SPI_HandleTypeDef *hspi1) {
if (p_insideT.index == 0) { if (p_insideT.index == 0) {
p_insideT.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; 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) + p_insideT.readings_cnts[p_insideT.index / 10] += to_signed_24(&rx_buff[12]);
((uint32_t)rx_buff[13]<<8) +
((uint32_t)rx_buff[14]));
if (p_insideT.index == 49) { if (p_insideT.index == 49) {
write(p_insideT); write(p_insideT);
code/l452_code/Core/Src/datadescriptor.cpp
+4 -4
View File
@@ -20,19 +20,19 @@ void data_description() {
CDC_Transmit_FS((uint8_t*)buff, cx); CDC_Transmit_FS((uint8_t*)buff, cx);
HAL_Delay(10); 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<packet_ekg>(), 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<packet_ekg>(), 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); CDC_Transmit_FS((uint8_t*)buff, cx);
HAL_Delay(10); 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<packet_strain>(), 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<packet_strain>(), 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); CDC_Transmit_FS((uint8_t*)buff, cx);
HAL_Delay(10); 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<packet_outsideT>(), 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<packet_outsideT>(), 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); CDC_Transmit_FS((uint8_t*)buff, cx);
HAL_Delay(10); 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<packet_insideT>(), 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<packet_insideT>(), 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); CDC_Transmit_FS((uint8_t*)buff, cx);
HAL_Delay(10); HAL_Delay(10);
code/l452_code/Core/Src/lsm6dsv.cpp
+27 -8
View File
@@ -71,6 +71,8 @@ void read_lsm6dsv(SPI_HandleTypeDef *hspi1) {
return; return;
} }
//CDC_Transmit_FS((uint8_t*) "Detected\n\r", 10);
uint8_t tx_buff[8] = {0}; uint8_t tx_buff[8] = {0};
uint8_t rx_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[0] = 0b10000000 + 0x1B;
tx_buff[1] = 0; tx_buff[1] = 0;
HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_RESET); 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); HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_SET);
char buffer[64];
int cx;
// todo: read in batches // todo: read in batches
if (rx_buff[1] != 0) { 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[0] = 0b10000000 + 0x78; // FIFO addr
tx_buff[1] = 0; tx_buff[1] = 0;
HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_RESET); 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, 8, HAL_MAX_DELAY);
HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_SET); HAL_GPIO_WritePin(GPIOC, IMU_CS_Pin, GPIO_PIN_SET);
}
p_imu.t = ((uint32_t)tim6_reloads) * (htim6.Init.Period + 1) + TIM6->CNT; // cx = snprintf(buffer, 64, "%02x %02x %02x %02x %02x %02x %02x %02x\n\r",
p_imu.readings_cnts[0] = rx_buff[1]; // rx_buff[0],
p_imu.readings_cnts[1] = ((uint16_t)rx_buff[2] << 8) + rx_buff[3]; // rx_buff[1],
p_imu.readings_cnts[2] = ((uint16_t)rx_buff[4] << 8) + rx_buff[5]; // rx_buff[2],
p_imu.readings_cnts[3] = ((uint16_t)rx_buff[6] << 8) + rx_buff[7]; // rx_buff[3],
write(p_imu); // 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);
}
} }
code/l452_code/Core/Src/main.cpp
+1 -1
View File
@@ -162,7 +162,7 @@ int main(void)
/* USER CODE BEGIN WHILE */ /* USER CODE BEGIN WHILE */
HAL_TIM_Base_Start_IT(&htim6); HAL_TIM_Base_Start_IT(&htim6);
initialize_ads131(&hspi1); //initialize_ads131(&hspi1);
initialize_lsm6dsv(&hspi1); initialize_lsm6dsv(&hspi1);
while (1) while (1)
code/l452_code/Core/Src/spi.c
+1 -1
View File
@@ -41,7 +41,7 @@ void MX_SPI1_Init(void)
hspi1.Init.Mode = SPI_MODE_MASTER; hspi1.Init.Mode = SPI_MODE_MASTER;
hspi1.Init.Direction = SPI_DIRECTION_2LINES; hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT; 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.CLKPhase = SPI_PHASE_2EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT; hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64; hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64;
code/l452_code/packet_parser.py
+31 -11
View File
@@ -48,12 +48,15 @@ ser.timeout = 5
ys = [] ys = []
imu_readings = {}
for i in range(1000): for i in range(1000):
ser.write(b'R') ser.write(b'R')
data = ser.read(1024) data = ser.read(1024)
index = 0 index = 0
while (index < 1024): while (index < 1024):
packet_type = (data[index + 1]<<8) + data[index] 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] ind = [i for i,t in enumerate(types) if t['type_code'] == packet_type]
if len(ind) != 1: if len(ind) != 1:
break break
@@ -62,17 +65,34 @@ for i in range(1000):
for e in t['elements']: for e in t['elements']:
block = d[e['offset']:e['offset'] + e['size']] block = d[e['offset']:e['offset'] + e['size']]
element_size = int(len(block) / e['n_elements']) element_size = int(len(block) / e['n_elements'])
if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]': # 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)] # 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)] # #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]': # #if t['type_name'] == b'packet_ekg' and e['name'] == b'readings_cnts[50]':
ys += chunked # ys += chunked
plt.clf() # plt.clf()
dat = np.array(ys[-1000:]) # dat = np.array(ys[-1000:])
plt.plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat)) # plt.plot(dat, 'k.', linestyle = '--')#signal.filtfilt(b, a, dat))
plt.pause(0.01) # plt.pause(0.01)
##print(BB) # ##print(BB)
e['readings'].append(chunked) # 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'] index += 2 + t['size']
#for t in types: #for t in types: