电子产品世界 » 论坛首页 » DIY与开源设计 » 电子DIY » 【let'sdo2026年第3期】成果贴-基于边缘AI实现电机状态的检测

共1条 1/1 1 跳转至页

【let'sdo2026年第3期】成果贴-基于边缘AI实现电机状态的检测

工程师
2026-11-17 23:29:45     打赏

一、项目概述

本项目以 STM32C562RE 为控制核心,使用板载按键控制步进电机停止、慢速和快速运行,

并通过 LIS3DH 三轴加速度传感器采集电机振动数据。采集数据按 XYZ 三轴记录,

NanoEdge AI Studio 生成的分类模型已接入固件,可依据连续加速度数据

识别 `motorstop`、`lowspeed` 和 `highspeed` 三种状态。

二、系统框图


三、硬件设计与连接

LIS3DH 安装在电机本体或靠近电机的固定支架上,安装方向和紧固方式在采集过程中保持一致。

传感器通过 I2C1 接入 STM32:PB6 为 SCL、PB7 为 SDA,供电接 3.3 V,传感器与开发板共地;


步进电机的四路控制信号分别由 PB0、PC0、PC2、PC3 输出,并连接到适配电机的驱动器输入端。

电机线圈不能直接由 MCU GPIO 驱动,步进电机控制板载达林顿管用于电流放大。PC13 用作板载按键输入,

PA5 板载 LED 用于显示电机运行状态。

四、主要参数

- 加速度传感器:LIS3DH,高分辨率模式,量程 ±2 g。

- 采样率:1.344 kHz;启用传感器 FIFO,减少采样过程中的漏读风险。

- I2C1:400 kHz。

- 每批数据:256 组三轴样本,约对应 0.19 秒。

- USART2:921600 波特率,8 数据位、无校验、1 停止位。

- 步进电机:四相八拍控制;慢速步进间隔 10 ms,快速 5 ms。

- 状态指示:停止时 LED 常亮;慢速时每秒亮灭切换;快速时每 250 ms 切换一次,即完整亮灭周期为 500 ms。

五、软件实现与调试

系统启动后初始化 GPIO、I2C、USART 和 FreeRTOS,并创建传感器、串口和电机三个任务。

电机任务对 PC13 按键进行消抖,按键依次切换停止、慢速和快速状态,并按照八拍序列更新四路输出。

static void functionMotorTask(void *pvParameters)
{
  TickType_t now = xTaskGetTickCount();
  TickType_t button_candidate_since = now;
  TickType_t last_step_tick = now;
  TickType_t last_led_toggle = now;
  const TickType_t debounce_ticks =
      pdMS_TO_TICKS(STEPPER_BUTTON_DEBOUNCE_MS);
  hal_gpio_pin_state_t button_candidate =
      HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN);
  hal_gpio_pin_state_t button_stable = button_candidate;
  stepper_mode_t mode = STEPPER_MODE_STOPPED;
  uint8_t led_on = 1U;
  uint8_t pattern_index = 0U;

  (void)pvParameters;
  stepper_release();
  HAL_GPIO_WritePin(LED_PORT, LED_PIN, LED_ACTIVE_STATE);

  for (;;)
  {
    hal_gpio_pin_state_t button_state =
        HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN);

    now = xTaskGetTickCount();
    if (button_state != button_candidate)
    {
      button_candidate = button_state;
      button_candidate_since = now;
    }
    else if ((button_candidate != button_stable) &&
             ((now - button_candidate_since) >= debounce_ticks))
    {
      button_stable = button_candidate;
      if (button_stable == HAL_GPIO_PIN_SET)
      {
        stepper_mode_t previous_mode = mode;

        if (mode == STEPPER_MODE_STOPPED)
        {
          mode = STEPPER_MODE_SLOW;
        }
        else if (mode == STEPPER_MODE_SLOW)
        {
          mode = STEPPER_MODE_FAST;
        }
        else
        {
          mode = STEPPER_MODE_STOPPED;
        }

        if (mode == STEPPER_MODE_STOPPED)
        {
          stepper_release();
          pattern_index = 0U;
          led_on = 1U;
          HAL_GPIO_WritePin(LED_PORT, LED_PIN, LED_ACTIVE_STATE);
        }
        else
        {
          if (previous_mode == STEPPER_MODE_STOPPED)
          {
            pattern_index = 0U;
            stepper_set_pattern(pattern_index);
            pattern_index = (uint8_t)((pattern_index + 1U) % 8U);
          }
          led_on = 1U;
          HAL_GPIO_WritePin(LED_PORT, LED_PIN, LED_ACTIVE_STATE);
          last_led_toggle = now;
        }

        last_step_tick = now;
      }
    }

    if (mode != STEPPER_MODE_STOPPED)
    {
      TickType_t step_interval =
          pdMS_TO_TICKS((mode == STEPPER_MODE_SLOW) ?
                            STEPPER_SLOW_STEP_INTERVAL_MS :
                            STEPPER_FAST_STEP_INTERVAL_MS);
      TickType_t led_phase_interval =
          pdMS_TO_TICKS((mode == STEPPER_MODE_SLOW) ?
                            LED_SLOW_PHASE_INTERVAL_MS :
                            LED_FAST_PHASE_INTERVAL_MS);

      if ((now - last_step_tick) >= step_interval)
      {
        stepper_set_pattern(pattern_index);
        pattern_index = (uint8_t)((pattern_index + 1U) % 8U);
        last_step_tick = now;
      }

      if ((now - last_led_toggle) >= led_phase_interval)
      {
        led_on = (led_on == 0U) ? 1U : 0U;
        HAL_GPIO_WritePin(LED_PORT, LED_PIN,
                          (led_on != 0U) ? LED_ACTIVE_STATE :
                                           LED_INACTIVE_STATE);
        last_led_toggle = now;
      }
    }

    vTaskDelay(1U);
  }
}

传感器任务初始化 LIS3DH,读取 FIFO 中的三轴数据并组装成 256 组批次;使用ai接口函数推导当前电机运行模式

并对模式变化进行消抖。

static void functionSensorTask(void *pvParameters)
{
  hal_i2c_handle_t *i2c = mx_i2c1_i2c_gethandle();
  hal_uart_handle_t *uart = mx_usart2_uart_gethandle();
  uint8_t sample_ready;
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  uint8_t batch_index = 0U;
  uint8_t buffer_owned = 0U;
  uint32_t samples_in_batch = 0U;
#else
  uint32_t samples_in_window = 0U;
  uint8_t model_ready = 0U;
  uint32_t candidate_class_count = 0U;
  int candidate_class = -1;
  int stable_class = -1;
  neai_result_t result;
#endif

  (void)pvParameters;
  g_lis3dh_status = lis3dh_init(i2c, uart);
#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
  g_neai_status = neai_classification_init();
  model_ready = (g_neai_status == NEAI_OK) ? 1U : 0U;
#endif

  for (;;)
  {
    if (g_lis3dh_ready == 0U)
    {
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
      if (buffer_owned != 0U)
      {
        if (xQueueSend(g_lis3dh_free_buffers, &batch_index,
                       portMAX_DELAY) != pdTRUE)
        {
          g_lis3dh_status = HAL_ERROR;
        }
        buffer_owned = 0U;
        samples_in_batch = 0U;
      }
#else
      samples_in_window = 0U;
      candidate_class = -1;
      candidate_class_count = 0U;
      stable_class = -1;
#endif

      vTaskDelay(pdMS_TO_TICKS(1000U));
      g_lis3dh_status = lis3dh_init(i2c, uart);
#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
      if (g_lis3dh_ready != 0U)
      {
        g_neai_status = neai_classification_init();
        model_ready = (g_neai_status == NEAI_OK) ? 1U : 0U;
      }
#endif
      continue;
    }

#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
    if (model_ready == 0U)
    {
      vTaskDelay(pdMS_TO_TICKS(1000U));
      g_neai_status = neai_classification_init();
      model_ready = (g_neai_status == NEAI_OK) ? 1U : 0U;
      continue;
    }
#endif

    g_lis3dh_status = lis3dh_read_sample(i2c, g_lis3dh_i2c_address,
                                         &sample_ready);
    if (g_lis3dh_status != HAL_OK)
    {
      g_lis3dh_ready = 0U;
      continue;
    }

    if (sample_ready != 0U)
    {
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
      if (buffer_owned == 0U)
      {
        if (xQueueReceive(g_lis3dh_free_buffers, &batch_index,
                          portMAX_DELAY) != pdTRUE)
        {
          g_lis3dh_status = HAL_ERROR;
          g_lis3dh_ready = 0U;
          continue;
        }
        buffer_owned = 1U;
      }

      if (buffer_owned != 0U)
      {
        g_lis3dh_sample_batch[batch_index][samples_in_batch].x_mg =
            g_lis3dh_sample.x_mg;
        g_lis3dh_sample_batch[batch_index][samples_in_batch].y_mg =
            g_lis3dh_sample.y_mg;
        g_lis3dh_sample_batch[batch_index][samples_in_batch].z_mg =
            g_lis3dh_sample.z_mg;
        samples_in_batch++;

        if (samples_in_batch == LIS3DH_SAMPLES_PER_LINE)
        {
          if (xQueueSend(g_lis3dh_ready_buffers, &batch_index,
                         portMAX_DELAY) == pdTRUE)
          {
            buffer_owned = 0U;
            samples_in_batch = 0U;
          }
          else
          {
            g_lis3dh_status = HAL_ERROR;
            g_lis3dh_ready = 0U;
          }
        }
      }
#else
      g_neai_input_signal[
          samples_in_window * NEAI_INPUT_AXIS_NUMBER] =
          (float)g_lis3dh_sample.x_mg;
      g_neai_input_signal[
          samples_in_window * NEAI_INPUT_AXIS_NUMBER + 1U] =
          (float)g_lis3dh_sample.y_mg;
      g_neai_input_signal[
          samples_in_window * NEAI_INPUT_AXIS_NUMBER + 2U] =
          (float)g_lis3dh_sample.z_mg;
      samples_in_window++;

      if (samples_in_window == NEAI_INPUT_SIGNAL_LENGTH)
      {
        int id_class = -1;

        g_neai_status = neai_classification(g_neai_input_signal,
                                             result.probabilities,
                                             &id_class);
        if ((g_neai_status == NEAI_OK) &&
            (id_class >= 0) &&
            (id_class < NEAI_NUMBER_OF_CLASSES))
        {
          if (id_class == stable_class)
          {
            candidate_class = -1;
            candidate_class_count = 0U;
          }
          else
          {
            if (id_class != candidate_class)
            {
              candidate_class = id_class;
              candidate_class_count = 1U;
            }
            else if (candidate_class_count <
                     NEAI_MODE_TRANSITION_DEBOUNCE_WINDOWS)
            {
              candidate_class_count++;
            }

            if (candidate_class_count >=
                NEAI_MODE_TRANSITION_DEBOUNCE_WINDOWS)
            {
              result.status = NEAI_OK;
              result.id_class = id_class;
              stable_class = id_class;
              candidate_class = -1;
              candidate_class_count = 0U;
              if (neai_queue_result(&result) != pdTRUE)
              {
                g_neai_status = NEAI_ERROR;
              }
            }
          }
        }
        else
        {
          if (g_neai_status == NEAI_OK)
          {
            g_neai_status = NEAI_INVALID_PARAM;
          }
          candidate_class = -1;
          candidate_class_count = 0U;
        }
        samples_in_window = 0U;
      }
#endif
    }

    if (sample_ready == 0U)
    {
      vTaskDelay(1U);
    }
  }
}

串口任务接收分类事件,并输出稳定识别出的电机状态及各类别概率。

static void functionUartTask(void *pvParameters)
{
  hal_uart_handle_t *uart = mx_usart2_uart_gethandle();
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  uint8_t batch_index;
#else
  neai_result_t result;
#endif

  (void)pvParameters;
  for (;;)
  {
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
    if (xQueueReceive(g_lis3dh_ready_buffers, &batch_index,
                      portMAX_DELAY) == pdTRUE)
    {
      g_lis3dh_uart_status = lis3dh_uart_print_sample_batch(
          uart, g_lis3dh_sample_batch[batch_index]);
      if (xQueueSend(g_lis3dh_free_buffers, &batch_index,
                     portMAX_DELAY) != pdTRUE)
      {
        g_lis3dh_uart_status = HAL_ERROR;
      }
    }
#else
    if (xQueueReceive(g_neai_result_queue, &result,
                      portMAX_DELAY) == pdTRUE)
    {
      g_lis3dh_uart_status = neai_uart_print_result(uart, &result);
    }
#endif
  }
}

接下来讲解模型推理步骤;

首先使用sscom查看单片机串口可以持续输出符合要求的训练数据

sscon-log.PNG

接下来使用nano edge ai studio新建分类"classify"项目

classfy.PNG

配置开发板型号,工程名及传感器类型

ai-1.PNG

通过串口采集三种电机状态共九组数据

对采集到的数据进行训练,我这里只训练了两个小时,部分模型就可以使用了。

时间充裕的同学可以等待模型全部训练完成

ai-3.PNG

我在训练好的模型里选了一个准确度较高的

ai-4.PNG

选择好之后,就可以导出模型并下载压缩包文件了

ai-5.PNG

最后高速ai需要实现的功能,就可以实现最终的项目任务了。

code-final.PNG

六、软件代码

/**
  ******************************************************************************
  * @file           : mx_freertos_app.c
  * @brief          : FreeRTOS initialization and application tasks
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2026 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the
  * mx_freertos_license.md file in the same directory as the generated code.
  * If no mx_freertos_license.md file comes with this software, it is provided
  * AS-IS.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "mx_freertos_app.h"
#include "mx_gpio_default.h"
#include "mx_i2c1.h"
#include "mx_usart2.h"
#include "NanoEdgeAI.h"
#include "queue.h"
#include "semphr.h"

#include <stdint.h>
#include <stdio.h>

/* Private define ------------------------------------------------------------*/
#define APP_MODE_DATA_COLLECTION      0U
#define APP_MODE_AI_RUN                1U
#define APP_CURRENT_WORK_MODE APP_MODE_AI_RUN
#define NEAI_MODE_TRANSITION_DEBOUNCE_WINDOWS 3U

#define TaskSensor_stack_size       512U
#define TaskUart_stack_size          512U
#define TaskMotor_stack_size         128U
#define STEPPER_BUTTON_DEBOUNCE_MS    30U
#define STEPPER_SLOW_STEP_INTERVAL_MS 10U
#define STEPPER_FAST_STEP_INTERVAL_MS  5U
#define LED_SLOW_PHASE_INTERVAL_MS  1000U
#define LED_FAST_PHASE_INTERVAL_MS   250U

#define LIS3DH_ADDRESS_SA0_LOW       0x18U
#define LIS3DH_ADDRESS_SA0_HIGH      0x19U
#define LIS3DH_REG_WHO_AM_I          0x0FU
#define LIS3DH_REG_CTRL_REG1         0x20U
#define LIS3DH_REG_CTRL_REG4         0x23U
#define LIS3DH_REG_CTRL_REG5         0x24U
#define LIS3DH_REG_OUT_X_L           0x28U
#define LIS3DH_REG_FIFO_CTRL         0x2EU
#define LIS3DH_REG_FIFO_SRC          0x2FU
#define LIS3DH_AUTO_INCREMENT        0x80U
#define LIS3DH_WHO_AM_I_VALUE        0x33U
#define LIS3DH_CTRL_REG5_FIFO_EN     0x40U
#define LIS3DH_FIFO_STREAM_WTM31     0x9FU
#define LIS3DH_FIFO_SRC_EMPTY        0x20U
#define LIS3DH_I2C_TIMEOUT_MS        100U
#define LIS3DH_SAMPLES_PER_LINE      256U
#define LIS3DH_BATCH_BUFFER_COUNT      2U
#define LIS3DH_VALUES_PER_SAMPLE      3U
#define LIS3DH_VALUE_TEXT_MAX        16U
#define LIS3DH_LINE_BUFFER_SIZE      \
  (LIS3DH_SAMPLES_PER_LINE * LIS3DH_VALUES_PER_SAMPLE * \
   LIS3DH_VALUE_TEXT_MAX + 2U)

#if (APP_CURRENT_WORK_MODE != APP_MODE_DATA_COLLECTION) && \
    (APP_CURRENT_WORK_MODE != APP_MODE_AI_RUN)
#error "Invalid APP_CURRENT_WORK_MODE"
#endif

/* Private typedef -----------------------------------------------------------*/
typedef struct
{
  int16_t x_counts;
  int16_t y_counts;
  int16_t z_counts;
  int32_t x_mg;
  int32_t y_mg;
  int32_t z_mg;
} lis3dh_sample_t;

#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
typedef struct
{
  int32_t x_mg;
  int32_t y_mg;
  int32_t z_mg;
} lis3dh_batch_sample_t;
#else
typedef struct
{
  enum neai_state status;
  int id_class;
  float probabilities[NEAI_NUMBER_OF_CLASSES];
} neai_result_t;
#endif

typedef enum
{
  STEPPER_MODE_STOPPED,
  STEPPER_MODE_SLOW,
  STEPPER_MODE_FAST
} stepper_mode_t;

/* Private variables ---------------------------------------------------------*/
static TaskHandle_t TaskSensor_Handle;
static TaskHandle_t TaskUart_Handle;
static TaskHandle_t TaskMotor_Handle;
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
static QueueHandle_t g_lis3dh_free_buffers;
static QueueHandle_t g_lis3dh_ready_buffers;
static lis3dh_batch_sample_t
    g_lis3dh_sample_batch[LIS3DH_BATCH_BUFFER_COUNT][LIS3DH_SAMPLES_PER_LINE];
static char g_lis3dh_uart_line[LIS3DH_LINE_BUFFER_SIZE];
#else
static QueueHandle_t g_neai_result_queue;
static float g_neai_input_signal[
    NEAI_INPUT_SIGNAL_LENGTH * NEAI_INPUT_AXIS_NUMBER];
#endif
static SemaphoreHandle_t g_lis3dh_uart_mutex;

volatile lis3dh_sample_t g_lis3dh_sample;
volatile uint32_t g_lis3dh_sample_count;
volatile uint8_t g_lis3dh_ready;
volatile uint8_t g_lis3dh_i2c_address;
volatile uint32_t g_lis3dh_status;
volatile uint32_t g_lis3dh_uart_status;
volatile uint8_t g_lis3dh_fifo_status;
volatile uint8_t g_lis3dh_who_am_i;
volatile uint32_t g_lis3dh_i2c_error;
volatile enum neai_state g_neai_status;

/* Private functions prototype ----------------------------------------------*/
static hal_status_t lis3dh_write_register(hal_i2c_handle_t *i2c,
                                          uint8_t address,
                                          uint8_t reg,
                                          uint8_t value);
static hal_status_t lis3dh_read_registers(hal_i2c_handle_t *i2c,
                                          uint8_t address,
                                          uint8_t reg,
                                          uint8_t *data,
                                          uint32_t size);
static hal_status_t lis3dh_init(hal_i2c_handle_t *i2c,
                                hal_uart_handle_t *uart);
static hal_status_t lis3dh_read_sample(hal_i2c_handle_t *i2c,
                                       uint8_t address,
                                       uint8_t *sample_ready);
static hal_status_t lis3dh_uart_transmit(hal_uart_handle_t *uart,
                                         const void *data,
                                         uint32_t size,
                                         uint32_t timeout_ms);
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
static hal_status_t lis3dh_uart_print_sample_batch(
    hal_uart_handle_t *uart,
    const lis3dh_batch_sample_t samples[LIS3DH_SAMPLES_PER_LINE]);
#else
static hal_status_t neai_uart_print_result(hal_uart_handle_t *uart,
                                           const neai_result_t *result);
static BaseType_t neai_queue_result(const neai_result_t *result);
#endif
static hal_status_t lis3dh_uart_print_init_result(hal_uart_handle_t *uart,
                                                  uint8_t address,
                                                  hal_status_t status,
                                                  uint32_t error,
                                                  uint8_t who_am_i);
static void functionSensorTask(void *pvParameters);
static void functionUartTask(void *pvParameters);
static void functionMotorTask(void *pvParameters);
static void stepper_set_pattern(uint8_t pattern_index);
static void stepper_release(void);

static const uint8_t g_stepper_half_step_sequence[8][4] =
{
  {1U, 0U, 0U, 0U},
  {1U, 1U, 0U, 0U},
  {0U, 1U, 0U, 0U},
  {0U, 1U, 1U, 0U},
  {0U, 0U, 1U, 0U},
  {0U, 0U, 1U, 1U},
  {0U, 0U, 0U, 1U},
  {1U, 0U, 0U, 1U}
};

/* Private functions ---------------------------------------------------------*/
static hal_status_t lis3dh_write_register(hal_i2c_handle_t *i2c,
                                          uint8_t address,
                                          uint8_t reg,
                                          uint8_t value)
{
  return HAL_I2C_MASTER_MemWrite(i2c, (uint32_t)address << 1U, reg,
                                 HAL_I2C_MEM_ADDR_8BIT, &value, 1U,
                                 LIS3DH_I2C_TIMEOUT_MS);
}

static hal_status_t lis3dh_read_registers(hal_i2c_handle_t *i2c,
                                          uint8_t address,
                                          uint8_t reg,
                                          uint8_t *data,
                                          uint32_t size)
{
  return HAL_I2C_MASTER_MemRead(i2c, (uint32_t)address << 1U, reg,
                                HAL_I2C_MEM_ADDR_8BIT, data, size,
                                LIS3DH_I2C_TIMEOUT_MS);
}

static hal_status_t lis3dh_uart_transmit(hal_uart_handle_t *uart,
                                         const void *data,
                                         uint32_t size,
                                         uint32_t timeout_ms)
{
  hal_status_t status;

  if (xSemaphoreTake(g_lis3dh_uart_mutex, portMAX_DELAY) != pdTRUE)
  {
    return HAL_ERROR;
  }

  status = HAL_UART_Transmit(uart, data, size, timeout_ms);
  (void)xSemaphoreGive(g_lis3dh_uart_mutex);
  return status;
}

#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
static BaseType_t neai_queue_result(const neai_result_t *result)
{
  return xQueueSend(g_neai_result_queue, result, portMAX_DELAY);
}

static hal_status_t neai_uart_print_result(hal_uart_handle_t *uart,
                                           const neai_result_t *result)
{
  char message[192];
  size_t offset;
  uint32_t i;
  int length;
  const char *predicted_name;

  if ((result->status != NEAI_OK) ||
      (result->id_class < 0) ||
      (result->id_class >= NEAI_NUMBER_OF_CLASSES))
  {
    length = snprintf(message, sizeof(message),
                      "# NanoEdge classification error: %d\r\n",
                      (int)result->status);
    if ((length < 0) || ((size_t)length >= sizeof(message)))
    {
      return HAL_ERROR;
    }

    return lis3dh_uart_transmit(uart, message, (uint32_t)length, 100U);
  }

  predicted_name = neai_get_class_name(result->id_class);
  if (predicted_name == NULL)
  {
    return HAL_ERROR;
  }

  length = snprintf(message, sizeof(message), "motor_state=%s",
                    predicted_name);
  if ((length < 0) || ((size_t)length >= sizeof(message)))
  {
    return HAL_ERROR;
  }
  offset = (size_t)length;

  for (i = 0U; i < NEAI_NUMBER_OF_CLASSES; i++)
  {
    const char *class_name = neai_get_class_name((int)i);
    float probability = result->probabilities[i];
    uint32_t probability_basis_points;

    if ((class_name == NULL) ||
        !(probability >= 0.0F && probability <= 1.0F))
    {
      return HAL_ERROR;
    }

    probability_basis_points =
        (uint32_t)(probability * 10000.0F + 0.5F);
    length = snprintf(&message[offset], sizeof(message) - offset,
                      ",%s=%lu.%02lu%%", class_name,
                      (unsigned long)(probability_basis_points / 100U),
                      (unsigned long)(probability_basis_points % 100U));
    if ((length < 0) ||
        ((size_t)length >= (sizeof(message) - offset)))
    {
      return HAL_ERROR;
    }
    offset += (size_t)length;
  }

  length = snprintf(&message[offset], sizeof(message) - offset, "\r\n");
  if ((length < 0) ||
      ((size_t)length >= (sizeof(message) - offset)))
  {
    return HAL_ERROR;
  }
  offset += (size_t)length;

  return lis3dh_uart_transmit(uart, message, (uint32_t)offset, 100U);
}
#endif

static hal_status_t lis3dh_init(hal_i2c_handle_t *i2c,
                                hal_uart_handle_t *uart)
{
  static const uint8_t addresses[] =
  {
    LIS3DH_ADDRESS_SA0_LOW,
    LIS3DH_ADDRESS_SA0_HIGH
  };
  uint8_t who_am_i;
  uint32_t i;
  hal_status_t status = HAL_ERROR;

  g_lis3dh_ready = 0U;
  for (i = 0U; i < (sizeof(addresses) / sizeof(addresses[0])); i++)
  {
    who_am_i = 0xFFU;
    status = lis3dh_read_registers(i2c, addresses[i], LIS3DH_REG_WHO_AM_I,
                                   &who_am_i, 1U);
    g_lis3dh_who_am_i = who_am_i;
    g_lis3dh_i2c_error = i2c->last_error_codes;
    g_lis3dh_uart_status = lis3dh_uart_print_init_result(
        uart, addresses[i], status, g_lis3dh_i2c_error, who_am_i);
    if ((status == HAL_OK) && (who_am_i == LIS3DH_WHO_AM_I_VALUE))
    {
      g_lis3dh_i2c_address = addresses[i];
      break;
    }
  }

  if (i == (sizeof(addresses) / sizeof(addresses[0])))
  {
    return (status == HAL_OK) ? HAL_ERROR : status;
  }

  status = lis3dh_write_register(i2c, g_lis3dh_i2c_address,
                                 LIS3DH_REG_CTRL_REG1, 0x00U);
  if (status != HAL_OK)
  {
    g_lis3dh_i2c_error = i2c->last_error_codes;
    return status;
  }

  status = lis3dh_write_register(i2c, g_lis3dh_i2c_address,
                                 LIS3DH_REG_CTRL_REG5,
                                 LIS3DH_CTRL_REG5_FIFO_EN);
  if (status != HAL_OK)
  {
    g_lis3dh_i2c_error = i2c->last_error_codes;
    return status;
  }

  status = lis3dh_write_register(i2c, g_lis3dh_i2c_address,
                                 LIS3DH_REG_FIFO_CTRL, 0x00U);
  if (status != HAL_OK)
  {
    g_lis3dh_i2c_error = i2c->last_error_codes;
    return status;
  }

  status = lis3dh_write_register(i2c, g_lis3dh_i2c_address,
                                 LIS3DH_REG_CTRL_REG4, 0x88U);
  if (status != HAL_OK)
  {
    g_lis3dh_i2c_error = i2c->last_error_codes;
    return status;
  }

  status = lis3dh_write_register(i2c, g_lis3dh_i2c_address,
                                 LIS3DH_REG_FIFO_CTRL,
                                 LIS3DH_FIFO_STREAM_WTM31);
  if (status != HAL_OK)
  {
    g_lis3dh_i2c_error = i2c->last_error_codes;
    return status;
  }

  /* 1.344 kHz ODR, all axes enabled, normal mode with high-resolution data. */
  status = lis3dh_write_register(i2c, g_lis3dh_i2c_address,
                                 LIS3DH_REG_CTRL_REG1, 0x97U);
  if (status != HAL_OK)
  {
    g_lis3dh_i2c_error = i2c->last_error_codes;
    return status;
  }

  g_lis3dh_ready = 1U;
  return HAL_OK;
}

static hal_status_t lis3dh_read_sample(hal_i2c_handle_t *i2c,
                                       uint8_t address,
                                       uint8_t *sample_ready)
{
  uint8_t fifo_status;
  uint8_t data[6];
  int16_t x;
  int16_t y;
  int16_t z;
  hal_status_t status;

  *sample_ready = 0U;
  status = lis3dh_read_registers(i2c, address, LIS3DH_REG_FIFO_SRC,
                                 &fifo_status, 1U);
  if (status != HAL_OK)
  {
    return status;
  }

  g_lis3dh_fifo_status = fifo_status;

  if ((fifo_status & LIS3DH_FIFO_SRC_EMPTY) != 0U)
  {
    return HAL_OK;
  }

  status = lis3dh_read_registers(i2c, address,
                                 LIS3DH_REG_OUT_X_L | LIS3DH_AUTO_INCREMENT,
                                 data, sizeof(data));
  if (status != HAL_OK)
  {
    return status;
  }

  x = (int16_t)(((uint16_t)data[1] << 8U) | data[0]);
  y = (int16_t)(((uint16_t)data[3] << 8U) | data[2]);
  z = (int16_t)(((uint16_t)data[5] << 8U) | data[4]);

  g_lis3dh_sample.x_counts = x / 16;
  g_lis3dh_sample.y_counts = y / 16;
  g_lis3dh_sample.z_counts = z / 16;
  g_lis3dh_sample.x_mg = g_lis3dh_sample.x_counts;
  g_lis3dh_sample.y_mg = g_lis3dh_sample.y_counts;
  g_lis3dh_sample.z_mg = g_lis3dh_sample.z_counts;
  g_lis3dh_sample_count++;
  *sample_ready = 1U;
  return HAL_OK;
}

#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
static hal_status_t lis3dh_uart_print_sample_batch(
    hal_uart_handle_t *uart,
    const lis3dh_batch_sample_t samples[LIS3DH_SAMPLES_PER_LINE])
{
  size_t offset = 0U;
  uint32_t i;

  for (i = 0U; i < LIS3DH_SAMPLES_PER_LINE; i++)
  {
    const char *separator =
        (i + 1U == LIS3DH_SAMPLES_PER_LINE) ? "\r\n" : " ";
    int length = snprintf(&g_lis3dh_uart_line[offset],
                          sizeof(g_lis3dh_uart_line) - offset,
                          "%ld.000 %ld.000 %ld.000%s",
                          (long)samples[i].x_mg,
                          (long)samples[i].y_mg,
                          (long)samples[i].z_mg,
                          separator);
    if ((length < 0) ||
        ((size_t)length >= (sizeof(g_lis3dh_uart_line) - offset)))
    {
      return HAL_ERROR;
    }

    offset += (size_t)length;
  }

  return lis3dh_uart_transmit(uart, g_lis3dh_uart_line, (uint32_t)offset,
                              1000U);
}
#endif

static hal_status_t lis3dh_uart_print_init_result(hal_uart_handle_t *uart,
                                                  uint8_t address,
                                                  hal_status_t status,
                                                  uint32_t error,
                                                  uint8_t who_am_i)
{
  char message[80];
  int length;

  if ((status == HAL_OK) && (who_am_i == LIS3DH_WHO_AM_I_VALUE))
  {
    return HAL_OK;
  }

  length = snprintf(message, sizeof(message),
                    "# LIS3DH probe 0x%02X: HAL=%d, I2C_ERR=0x%08lX, WHO=0x%02X\r\n",
                    address, (int)status, (unsigned long)error, who_am_i);

  if ((length < 0) || ((size_t)length >= sizeof(message)))
  {
    return HAL_ERROR;
  }

  return lis3dh_uart_transmit(uart, message, (uint32_t)length, 100U);
}

/* Tasks definition ----------------------------------------------------------*/
int32_t app_synctasks_init(void)
{
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  static const uint8_t initial_buffer_indices[LIS3DH_BATCH_BUFFER_COUNT] =
  {
    0U, 1U
  };
#endif
  BaseType_t ret;
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  uint32_t i;

  g_lis3dh_free_buffers =
      xQueueCreate(LIS3DH_BATCH_BUFFER_COUNT, sizeof(uint8_t));
  g_lis3dh_ready_buffers =
      xQueueCreate(LIS3DH_BATCH_BUFFER_COUNT, sizeof(uint8_t));
#else
  g_neai_result_queue = xQueueCreate(2U, sizeof(neai_result_t));
#endif
  g_lis3dh_uart_mutex = xSemaphoreCreateMutex();
  if (g_lis3dh_uart_mutex == NULL)
  {
    return -1;
  }

#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  if ((g_lis3dh_free_buffers == NULL) ||
      (g_lis3dh_ready_buffers == NULL))
  {
    return -1;
  }

  for (i = 0U; i < LIS3DH_BATCH_BUFFER_COUNT; i++)
  {
    if (xQueueSend(g_lis3dh_free_buffers, &initial_buffer_indices[i], 0U) !=
        pdTRUE)
    {
      return -1;
    }
  }
#else
  if (g_neai_result_queue == NULL)
  {
    return -1;
  }
#endif

  ret = xTaskCreate(functionSensorTask, "TaskSensor", TaskSensor_stack_size,
                    NULL, tskIDLE_PRIORITY + 2U, &TaskSensor_Handle);
  if (ret != pdPASS)
  {
    return -1;
  }

  ret = xTaskCreate(functionUartTask, "TaskUart", TaskUart_stack_size,
                    NULL, tskIDLE_PRIORITY + 1U, &TaskUart_Handle);
  if (ret != pdPASS)
  {
    return -1;
  }

  ret = xTaskCreate(functionMotorTask, "TaskMotor", TaskMotor_stack_size,
                    NULL, tskIDLE_PRIORITY + 1U, &TaskMotor_Handle);
  if (ret != pdPASS)
  {
    return -1;
  }

  return 0;
}

static void functionSensorTask(void *pvParameters)
{
  hal_i2c_handle_t *i2c = mx_i2c1_i2c_gethandle();
  hal_uart_handle_t *uart = mx_usart2_uart_gethandle();
  uint8_t sample_ready;
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  uint8_t batch_index = 0U;
  uint8_t buffer_owned = 0U;
  uint32_t samples_in_batch = 0U;
#else
  uint32_t samples_in_window = 0U;
  uint8_t model_ready = 0U;
  uint32_t candidate_class_count = 0U;
  int candidate_class = -1;
  int stable_class = -1;
  neai_result_t result;
#endif

  (void)pvParameters;
  g_lis3dh_status = lis3dh_init(i2c, uart);
#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
  g_neai_status = neai_classification_init();
  model_ready = (g_neai_status == NEAI_OK) ? 1U : 0U;
#endif

  for (;;)
  {
    if (g_lis3dh_ready == 0U)
    {
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
      if (buffer_owned != 0U)
      {
        if (xQueueSend(g_lis3dh_free_buffers, &batch_index,
                       portMAX_DELAY) != pdTRUE)
        {
          g_lis3dh_status = HAL_ERROR;
        }
        buffer_owned = 0U;
        samples_in_batch = 0U;
      }
#else
      samples_in_window = 0U;
      candidate_class = -1;
      candidate_class_count = 0U;
      stable_class = -1;
#endif

      vTaskDelay(pdMS_TO_TICKS(1000U));
      g_lis3dh_status = lis3dh_init(i2c, uart);
#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
      if (g_lis3dh_ready != 0U)
      {
        g_neai_status = neai_classification_init();
        model_ready = (g_neai_status == NEAI_OK) ? 1U : 0U;
      }
#endif
      continue;
    }

#if APP_CURRENT_WORK_MODE == APP_MODE_AI_RUN
    if (model_ready == 0U)
    {
      vTaskDelay(pdMS_TO_TICKS(1000U));
      g_neai_status = neai_classification_init();
      model_ready = (g_neai_status == NEAI_OK) ? 1U : 0U;
      continue;
    }
#endif

    g_lis3dh_status = lis3dh_read_sample(i2c, g_lis3dh_i2c_address,
                                         &sample_ready);
    if (g_lis3dh_status != HAL_OK)
    {
      g_lis3dh_ready = 0U;
      continue;
    }

    if (sample_ready != 0U)
    {
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
      if (buffer_owned == 0U)
      {
        if (xQueueReceive(g_lis3dh_free_buffers, &batch_index,
                          portMAX_DELAY) != pdTRUE)
        {
          g_lis3dh_status = HAL_ERROR;
          g_lis3dh_ready = 0U;
          continue;
        }
        buffer_owned = 1U;
      }

      if (buffer_owned != 0U)
      {
        g_lis3dh_sample_batch[batch_index][samples_in_batch].x_mg =
            g_lis3dh_sample.x_mg;
        g_lis3dh_sample_batch[batch_index][samples_in_batch].y_mg =
            g_lis3dh_sample.y_mg;
        g_lis3dh_sample_batch[batch_index][samples_in_batch].z_mg =
            g_lis3dh_sample.z_mg;
        samples_in_batch++;

        if (samples_in_batch == LIS3DH_SAMPLES_PER_LINE)
        {
          if (xQueueSend(g_lis3dh_ready_buffers, &batch_index,
                         portMAX_DELAY) == pdTRUE)
          {
            buffer_owned = 0U;
            samples_in_batch = 0U;
          }
          else
          {
            g_lis3dh_status = HAL_ERROR;
            g_lis3dh_ready = 0U;
          }
        }
      }
#else
      g_neai_input_signal[
          samples_in_window * NEAI_INPUT_AXIS_NUMBER] =
          (float)g_lis3dh_sample.x_mg;
      g_neai_input_signal[
          samples_in_window * NEAI_INPUT_AXIS_NUMBER + 1U] =
          (float)g_lis3dh_sample.y_mg;
      g_neai_input_signal[
          samples_in_window * NEAI_INPUT_AXIS_NUMBER + 2U] =
          (float)g_lis3dh_sample.z_mg;
      samples_in_window++;

      if (samples_in_window == NEAI_INPUT_SIGNAL_LENGTH)
      {
        int id_class = -1;

        g_neai_status = neai_classification(g_neai_input_signal,
                                             result.probabilities,
                                             &id_class);
        if ((g_neai_status == NEAI_OK) &&
            (id_class >= 0) &&
            (id_class < NEAI_NUMBER_OF_CLASSES))
        {
          if (id_class == stable_class)
          {
            candidate_class = -1;
            candidate_class_count = 0U;
          }
          else
          {
            if (id_class != candidate_class)
            {
              candidate_class = id_class;
              candidate_class_count = 1U;
            }
            else if (candidate_class_count <
                     NEAI_MODE_TRANSITION_DEBOUNCE_WINDOWS)
            {
              candidate_class_count++;
            }

            if (candidate_class_count >=
                NEAI_MODE_TRANSITION_DEBOUNCE_WINDOWS)
            {
              result.status = NEAI_OK;
              result.id_class = id_class;
              stable_class = id_class;
              candidate_class = -1;
              candidate_class_count = 0U;
              if (neai_queue_result(&result) != pdTRUE)
              {
                g_neai_status = NEAI_ERROR;
              }
            }
          }
        }
        else
        {
          if (g_neai_status == NEAI_OK)
          {
            g_neai_status = NEAI_INVALID_PARAM;
          }
          candidate_class = -1;
          candidate_class_count = 0U;
        }
        samples_in_window = 0U;
      }
#endif
    }

    if (sample_ready == 0U)
    {
      vTaskDelay(1U);
    }
  }
}

static void functionUartTask(void *pvParameters)
{
  hal_uart_handle_t *uart = mx_usart2_uart_gethandle();
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
  uint8_t batch_index;
#else
  neai_result_t result;
#endif

  (void)pvParameters;
  for (;;)
  {
#if APP_CURRENT_WORK_MODE == APP_MODE_DATA_COLLECTION
    if (xQueueReceive(g_lis3dh_ready_buffers, &batch_index,
                      portMAX_DELAY) == pdTRUE)
    {
      g_lis3dh_uart_status = lis3dh_uart_print_sample_batch(
          uart, g_lis3dh_sample_batch[batch_index]);
      if (xQueueSend(g_lis3dh_free_buffers, &batch_index,
                     portMAX_DELAY) != pdTRUE)
      {
        g_lis3dh_uart_status = HAL_ERROR;
      }
    }
#else
    if (xQueueReceive(g_neai_result_queue, &result,
                      portMAX_DELAY) == pdTRUE)
    {
      g_lis3dh_uart_status = neai_uart_print_result(uart, &result);
    }
#endif
  }
}

static void functionMotorTask(void *pvParameters)
{
  TickType_t now = xTaskGetTickCount();
  TickType_t button_candidate_since = now;
  TickType_t last_step_tick = now;
  TickType_t last_led_toggle = now;
  const TickType_t debounce_ticks =
      pdMS_TO_TICKS(STEPPER_BUTTON_DEBOUNCE_MS);
  hal_gpio_pin_state_t button_candidate =
      HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN);
  hal_gpio_pin_state_t button_stable = button_candidate;
  stepper_mode_t mode = STEPPER_MODE_STOPPED;
  uint8_t led_on = 1U;
  uint8_t pattern_index = 0U;

  (void)pvParameters;
  stepper_release();
  HAL_GPIO_WritePin(LED_PORT, LED_PIN, LED_ACTIVE_STATE);

  for (;;)
  {
    hal_gpio_pin_state_t button_state =
        HAL_GPIO_ReadPin(BUTTON_PORT, BUTTON_PIN);

    now = xTaskGetTickCount();
    if (button_state != button_candidate)
    {
      button_candidate = button_state;
      button_candidate_since = now;
    }
    else if ((button_candidate != button_stable) &&
             ((now - button_candidate_since) >= debounce_ticks))
    {
      button_stable = button_candidate;
      if (button_stable == HAL_GPIO_PIN_SET)
      {
        stepper_mode_t previous_mode = mode;

        if (mode == STEPPER_MODE_STOPPED)
        {
          mode = STEPPER_MODE_SLOW;
        }
        else if (mode == STEPPER_MODE_SLOW)
        {
          mode = STEPPER_MODE_FAST;
        }
        else
        {
          mode = STEPPER_MODE_STOPPED;
        }

        if (mode == STEPPER_MODE_STOPPED)
        {
          stepper_release();
          pattern_index = 0U;
          led_on = 1U;
          HAL_GPIO_WritePin(LED_PORT, LED_PIN, LED_ACTIVE_STATE);
        }
        else
        {
          if (previous_mode == STEPPER_MODE_STOPPED)
          {
            pattern_index = 0U;
            stepper_set_pattern(pattern_index);
            pattern_index = (uint8_t)((pattern_index + 1U) % 8U);
          }
          led_on = 1U;
          HAL_GPIO_WritePin(LED_PORT, LED_PIN, LED_ACTIVE_STATE);
          last_led_toggle = now;
        }

        last_step_tick = now;
      }
    }

    if (mode != STEPPER_MODE_STOPPED)
    {
      TickType_t step_interval =
          pdMS_TO_TICKS((mode == STEPPER_MODE_SLOW) ?
                            STEPPER_SLOW_STEP_INTERVAL_MS :
                            STEPPER_FAST_STEP_INTERVAL_MS);
      TickType_t led_phase_interval =
          pdMS_TO_TICKS((mode == STEPPER_MODE_SLOW) ?
                            LED_SLOW_PHASE_INTERVAL_MS :
                            LED_FAST_PHASE_INTERVAL_MS);

      if ((now - last_step_tick) >= step_interval)
      {
        stepper_set_pattern(pattern_index);
        pattern_index = (uint8_t)((pattern_index + 1U) % 8U);
        last_step_tick = now;
      }

      if ((now - last_led_toggle) >= led_phase_interval)
      {
        led_on = (led_on == 0U) ? 1U : 0U;
        HAL_GPIO_WritePin(LED_PORT, LED_PIN,
                          (led_on != 0U) ? LED_ACTIVE_STATE :
                                           LED_INACTIVE_STATE);
        last_led_toggle = now;
      }
    }

    vTaskDelay(1U);
  }
}

static void stepper_set_pattern(uint8_t pattern_index)
{
  HAL_GPIO_WritePin(STEP_1_PORT, STEP_1_PIN,
                    (g_stepper_half_step_sequence[pattern_index][0] != 0U) ?
                        STEP_1_ACTIVE_STATE : STEP_1_INACTIVE_STATE);
  HAL_GPIO_WritePin(STEP_2_PORT, STEP_2_PIN,
                    (g_stepper_half_step_sequence[pattern_index][1] != 0U) ?
                        STEP_2_ACTIVE_STATE : STEP_2_INACTIVE_STATE);
  HAL_GPIO_WritePin(STEP_3_PORT, STEP_3_PIN,
                    (g_stepper_half_step_sequence[pattern_index][2] != 0U) ?
                        STEP_3_ACTIVE_STATE : STEP_3_INACTIVE_STATE);
  HAL_GPIO_WritePin(STEP_4_PORT, STEP_4_PIN,
                    (g_stepper_half_step_sequence[pattern_index][3] != 0U) ?
                        STEP_4_ACTIVE_STATE : STEP_4_INACTIVE_STATE);
}

static void stepper_release(void)
{
  HAL_GPIO_WritePin(STEP_1_PORT, STEP_1_PIN, STEP_1_INACTIVE_STATE);
  HAL_GPIO_WritePin(STEP_2_PORT, STEP_2_PIN, STEP_2_INACTIVE_STATE);
  HAL_GPIO_WritePin(STEP_3_PORT, STEP_3_PIN, STEP_3_INACTIVE_STATE);
  HAL_GPIO_WritePin(STEP_4_PORT, STEP_4_PIN, STEP_4_INACTIVE_STATE);
}

七、效果演示

motor-state.PNG

电机实物图

motor-wire.jpg

心得体会

今日下午参加了其他芯片厂商的嵌入式ai工具培训,感觉ai工具使用上大同小异

在学习这些工具的使用上应保持"不畏初难"的态度,入门之后使用起来还是挺简单的。



共1条 1/1 1 跳转至页

回复

匿名不能发帖!请先 [ 登陆 注册 ]