Solved
What is wrong with my pedometer logic?
I work on an STM32F407G-DISC1 pedometer project, in which the program is supposed to use built-in lis3dsh accelerometer via SPI to detect and count steps and display the number of steps on the screen (ILI9341 SPI).
I have tested my screen code, and it works properly, so problem lies in how I use accelerometer or detect steps.
To detect steps, I read lis3dsh registers with 100HZ frequency by making a call to SPI from a 100HZ timer interrupt. These snippets are interrupt and timer configuration respectively.
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
{
if (htim->Instance == TIM2) {
if ((draw_ready == 1) && (accelerometer_ready == 1)) {
accelerometer_read_all_axes_dma(xyz_data, &hspi1);
}
}
//else if (htim->Instance == TIM3) {
// if (draw_ready == 1) {
//steps_count += 1;
//update_lcd_flag = 1;
// }
//}
}static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_ClockConfigTypeDef sClockSourceConfig = {0};
TIM_MasterConfigTypeDef sMasterConfig = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2;
htim2.Init.Prescaler = 999;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 1679;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
if (HAL_TIM_ConfigClockSource(&htim2, &sClockSourceConfig) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
}The accelerometer is initialized at the start of the program using a function defined in accelerometer.c file. The following snippets are main and accelerometer functions respectively.
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_I2S3_Init();
MX_SPI1_Init();
MX_USB_HOST_Init();
MX_SPI2_Init();
MX_TIM2_Init();
MX_TIM3_Init();
/* USER CODE BEGIN 2 */
//timers
HAL_TIM_Base_Start_IT(&htim2);
HAL_TIM_Base_Start_IT(&htim3);
//Reset the screen
ILI9341_Init(&hspi2);
//screen experiment, delete later (black rectangle)
HAL_Delay(1000);
//Write_Rectangle(0, 50, 0, 50, 0, &hspi2); //good
//draw_digit(10, 10, 12, 8, 0, &hspi2); //good
//draw_number(UINT32_MAX, 12, &hspi2); //good
//Set everything for the accelometer
//fill buffer with 0s
for (int i = 0; i < ACC_BUF_SIZE; i++) {
acc_values_buf[i] = 0;
}
//receive and set accelerator sensitivity
accelerometer_init(&hspi1);
accelerometer_ready = 1;
//initial dma call (deprecated)
//HAL_SPI_TransmitReceive_DMA(&hspi1, xyz_addr, xyz_data, 6);
//default value
//clean_screen(0xFF, &hspi2);
//draw_number(steps_count, 12, &hspi2);
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
uint32_t steps_count_prev = -1;
clean_screen(0xFFFF, &hspi2);
draw_ready = 1;
uint32_t prev_mag = 77299264;
uint32_t prev_prev_mag = 77299264;
uint32_t below_cutoff_detected = 0;
while (1)
{
/* USER CODE END WHILE */
//MX_USB_HOST_Process();
/* USER CODE BEGIN 3 */
if (update_pedometer == 1) {
//declare variables
int16_t x, y, z;
uint32_t mag;
uint32_t prev_index, prev_prev_index, prev_sample, prev_prev_sample;
prev_index = 0; prev_prev_index = 0; prev_sample = 0; prev_prev_sample = 0;
//calculate magnitude
x = (int16_t)((uint16_t)xyz_data[1] | ((uint16_t)xyz_data[2] << 8));
y = (int16_t)((uint16_t)xyz_data[3] | ((uint16_t)xyz_data[4] << 8));
z = (int16_t)((uint16_t)xyz_data[5] | ((uint16_t)xyz_data[6] << 8));
int32_t x32, y32, z32;
x32 = x; y32 = y; z32 = z;
mag = (uint32_t)(x32*x32 + y32*y32 + z32*z32); //auto-convert float to int //was sqrt
last_peak += 1; //when the last time there was a peak
value_period = 0;
uint32_t avg_mag = (mag + prev_mag + prev_prev_mag) / 3;
//make all checks
if (/*(last_peak < MAX_PEAK_DIFF) &&*/ (last_peak > MIN_PEAK_DIFF)
&& (avg_mag > PEAK_CUTOFF)
/*&& (below_cutoff_detected == 1)*/) {
steps_count += 1; //uncomment later
update_lcd_flag = 1;
last_peak = 0;
below_cutoff_detected = 0;
}
else if (avg_mag < LOW_CUTOFF) {
below_cutoff_detected = 1;
}
update_pedometer = 0;
prev_prev_mag = prev_mag;
prev_mag = mag;
}
//During each SPI clock cycle, full-duplex transmission of a single bit occurs.
//1 clock cycle is 168/32MHz = 5.25MHz
//meaning there are 5.25 * 10^6 bit, or 5.25 * 10^6/8 bytes = 656250 bytes
//or, all coordinates are received 109375 times.
if (update_lcd_flag == 1) {
Write_Rectangle(0, 30, 0, ILI9341_WIDTH - 1, 0xFFFF, &hspi2);
draw_number(steps_count, 12, &hspi2);
update_lcd_flag = 0;
}
steps_count_prev = steps_count;
}
/* USER CODE END 3 */
}
#include "accelerometer.h"
void accelerometer_read_all_axes_dma(uint8_t src[7], SPI_HandleTypeDef *hspi1) {
uint8_t tx[7] = {OUT_X_L | 0x80, 0, 0, 0, 0, 0, 0};
uint8_t rx[7];
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_RESET);
HAL_SPI_TransmitReceive_DMA(hspi1, tx, src, 7);
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_SET);
//memcpy(src, &rx[1], 6);
}
uint8_t accelerometer_read(uint8_t reg, SPI_HandleTypeDef *hspi1) {
//https://stackoverflow.com/questions/67922914/stm32-spi-communication-with-hal
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_RESET);
uint8_t tx[2];
uint8_t rx[2];
tx[0] = reg | 0x80; // READ bit
tx[1] = 0x00; //the master transmits a dummy byte for the purpose of generating more clocks on which the slave can respond
HAL_SPI_TransmitReceive(hspi1, tx, rx, 2, 100); //send and receive 1 byte
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_SET);
return rx[1];
}
void accelerometer_write(uint8_t reg, uint8_t val, SPI_HandleTypeDef *hspi1) {
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_RESET);
uint8_t tx[2];
tx[0] = reg & 0x7F;
tx[1] = val;
HAL_SPI_Transmit(hspi1, tx, 2, 100);
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_SET);
}
void accelerometer_init(SPI_HandleTypeDef *hspi1) {
//we don't need a queue
accelerometer_write(FIFO_CTRL, 0x00, hspi1);
//set sensitivity to 4
uint8_t ctrl_val;
ctrl_val = accelerometer_read(CTRL_REG5, hspi1);
ctrl_val = ctrl_val | 0x08; //set FSCALE0 so that sensitivity is +-4gg
uint8_t tx_buf[2];
tx_buf[0] = CTRL_REG5 & 0x7F; //register address
tx_buf[1] = ctrl_val; //data to write into it
accelerometer_write(CTRL_REG5, ctrl_val, hspi1);
//enable the device
uint8_t ctrl4_val = 0x7F;
accelerometer_write(CTRL_REG4, ctrl4_val, hspi1);
}The steps are detected in SPI1 callback. The following functions are SPI1 initialization and SPI1 RtRx callback respectively.
static void MX_SPI1_Init(void)
{
/* USER CODE BEGIN SPI1_Init 0 */
/* USER CODE END SPI1_Init 0 */
/* USER CODE BEGIN SPI1_Init 1 */
/* USER CODE END SPI1_Init 1 */
/* SPI1 parameter configuration*/
hspi1.Instance = SPI1;
hspi1.Init.Mode = SPI_MODE_MASTER;
hspi1.Init.Direction = SPI_DIRECTION_2LINES;
hspi1.Init.DataSize = SPI_DATASIZE_8BIT;
hspi1.Init.CLKPolarity = SPI_POLARITY_LOW;
hspi1.Init.CLKPhase = SPI_PHASE_1EDGE;
hspi1.Init.NSS = SPI_NSS_SOFT;
hspi1.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
hspi1.Init.FirstBit = SPI_FIRSTBIT_MSB;
hspi1.Init.TIMode = SPI_TIMODE_DISABLE;
hspi1.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
hspi1.Init.CRCPolynomial = 10;
if (HAL_SPI_Init(&hspi1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN SPI1_Init 2 */
/* USER CODE END SPI1_Init 2 */
}
//spi callback function
void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef * hspi)
{
HAL_GPIO_WritePin(ACC_GPIO_PORT, ACC_GPIO_PIN, GPIO_PIN_SET);
if(hspi == &hspi1) {
//declare variables
int16_t x, y, z;
uint32_t mag;
uint32_t prev_index, prev_prev_index, prev_sample, prev_prev_sample;
prev_index = 0; prev_prev_index = 0; prev_sample = 0; prev_prev_sample = 0;
//calculate magnitude
x = (int16_t)((uint16_t)xyz_data[1] | ((uint16_t)xyz_data[2] << 8));
y = (int16_t)((uint16_t)xyz_data[3] | ((uint16_t)xyz_data[4] << 8));
z = (int16_t)((uint16_t)xyz_data[5] | ((uint16_t)xyz_data[6] << 8));
int32_t x32, y32, z32;
x32 = x; y32 = y; z32 = z;
mag = (uint32_t)(x32*x32 + y32*y32 + z32*z32); //auto-convert float to int //was sqrt
last_peak += 1; //when the last time there was a peak
value_period = 0;
prev_index = buf_index; //previous sample index
prev_prev_index = (prev_index == 0) ? (ACC_BUF_SIZE - 1) : (prev_index - 1); //(prev_index - 1) % ACC_BUF_SIZE; //pre-previous sample index
prev_sample = acc_values_buf[prev_index];
prev_prev_sample = acc_values_buf[prev_prev_index];
buf_index = (buf_index + 1) % ACC_BUF_SIZE; //update buf_index
acc_values_buf[buf_index] = mag; //add the sample
//make all checks
if ((last_peak < MAX_PEAK_DIFF && last_peak > MIN_PEAK_DIFF)
&& (prev_sample > prev_prev_sample)
&& (prev_sample > mag)
&& (prev_sample > PEAK_CUTOFF)) {
steps_count += 1; //uncomment later
update_lcd_flag = 1;
last_peak = 0;
}
else {
//update_lcd_flag = 0;
}
}
}In the TxRx callback function, I compare the previous sample (each sample is sum of squared x/y/z values) with the one before it and the current sample. If the previous sample is greater than either of its to neighbors, greater than a cutoff (which is supposed to be 0.35g), and it has been between 20 and 90 samples since the last sample corresponding all these conditions (this condition performs as a filter), it gets counted as a step.
The following snippets are constants and global variables used in the code above
#define MAX_PEAK_DIFF 90 //100 per second, so 90 to include slow walk
#define MIN_PEAK_DIFF 20 //100 per second, so 20 to include run
#define PEAK_CUTOFF 8220334 //0.35 g (32,767^2/(4^2)*(0.35^2))
#define ACC_BUF_SIZE 100 //1 second = 109375
uint32_t acc_values_buf[ACC_BUF_SIZE];
volatile uint8_t xyz_addr[6];
volatile uint8_t xyz_data[7];
uint32_t value_period = 0;
uint32_t buf_index = 0;
uint32_t last_peak = 0;
volatile uint32_t steps_count = 0;
volatile uint32_t update_lcd_flag = 1;
volatile uint32_t screen_dma_filled = 0;
volatile uint32_t bytes_remaining = 0;
extern volatile uint8_t dma_buf[PIXEL_BUFFER_SIZE];
uint32_t draw_ready = 0;
volatile int accelerometer_ready = 0;
#define FIFO_CTRL 0x2E
#define CTRL_REG4 0x20
#define ACC_GPIO_PORT GPIOE
#define ACC_GPIO_PIN GPIO_PIN_3
#define OUT_X_L 0x28
#define OUT_X_H 0x29
#define OUT_Y_L 0x2A
#define OUT_Y_H 0x2B
#define OUT_Z_L 0x2C
#define OUT_Z_H 0x2D
#define CTRL_REG5 0x24I think that the problem is either in step detection logic, or lis3dsh initialization, but I am not sure what the root of the problem is exactly.
UPD: I have accidentally changed the code for the main function in this post to the newer one (I wanted to edit a comment instead)