Full Motion Detection

Lab 5: ระบบ Motion Detection สมบูรณ์

วัตถุประสงค์

• Integration: รวมทุกฟังก์ชันจาก Lab 1-4

• Statistics: เก็บสถิติการเคลื่อนไหว

• Real Application: ระบบ Motion Detection ที่ใช้งานได้จริง

จะได้เรียนรู้อะไร

• System Design: ออกแบบระบบที่ซับซ้อน

• Statistics Collection: เก็บ min/max/count

• LED Feedback: แสดงสถานะผ่าน LED

System Architecture

┌─────────────────────────────────────────────────────────────────┐
│               Complete Motion Detection System                  │
├─────────────────────────────────────────────────────────────────┤
│                                                                 │
│   ┌──────────┐     ┌──────────┐     ┌───────────┐               │
│   │  BMI270  │────►│ Convert  │────►│Magnitude  │               │
│   │  Sensor  │     │  to m/s² │     │√(x²+y²+z²)│               │
│   └──────────┘     └──────────┘     └────┬──────┘               │
│                                          │                      │
│                                          ▼                      │
│                                    ┌───────────┐                │
│                                    │   Filter  │                │
│                                    │ Moving Avg│                │
│                                    └─────┬─────┘                │
│                                          │                      │
│            ┌─────────────────────────────┼────────────────┐     │
│            │                             │                │     │
│            ▼                             ▼                ▼     │
│      ┌──────────┐               ┌──────────┐      ┌──────────┐  │
│      │  Motion  │               │Statistics│      │   LED    │  │
│      │ Detection│               │ Update   │      │ Feedback │  │
│      └──────────┘               └──────────┘      └──────────┘  │
│                                                                 │
└─────────────────────────────────────────────────────────────────┘

โหลด Project Code จาก AIC Github Repo เพื่อเปิดบน VSCode IDE

GitHub - Advance-Innovation-Centre-AIC/aic-psoc-edge-epc2-imu-full-systemGitHub

1. เปิดโปรเจกต์ aic-psoc-edge-epc2-imu-full-system

เปิดไฟล์ proj_cm33_ns/sensor_hub_daq_task.c และศึกษาโครงสร้าง:

aic-psoc-edge-epc2-imu-full-system/
├── proj_cm33_s/              # Secure Project
├── proj_cm33_ns/             
│   ├── main.c
│   ├── sensor_hub_daq_task.c 
│   └── sensor_hub_daq_task.h
└── proj_cm55/                # CM55 Project

เพิ่ม Include

#include <math.h>  /* sqrtf */
#include <string.h>

Important Macros and Variables

/*******************************************************************************
* Macros
*******************************************************************************/
#define GRAVITY_EARTH                       (9.80665f)
#define DEG_TO_RAD                          (0.01745f)
#define GYR_RANGE_DPS                       (2000.0f)
#define ACC_RANGE_2G                        (2.0f)

/* Filter Configuration */
#define FILTER_WINDOW_SIZE                  (10)    /* 10 samples = 1 sec at 10Hz */

/* Statistics Report Interval */
#define STATS_REPORT_INTERVAL               (100)   /* 100 samples = 10 sec at 10Hz */

/* LED Macros - Active HIGH (PSoC Edge E84 USER_LED1) */
#define LED_ON()     Cy_GPIO_Set(CYBSP_USER_LED1_PORT, CYBSP_USER_LED1_PIN)
#define LED_OFF()    Cy_GPIO_Clr(CYBSP_USER_LED1_PORT, CYBSP_USER_LED1_PIN)

/* Custom app module ID to avoid collisions */
#define APP_RSLT_MODULE_ID                  (1U)

/* App error for task creation failure */
#define APP_RSLT_ACQ_TASK_CREATE_FAILED     CY_RSLT_CREATE(CY_RSLT_TYPE_ERROR,\
                                                        APP_RSLT_MODULE_ID,\
                                                        1U)

/*******************************************************************************
* Motion State Enum
*******************************************************************************/
typedef enum {
    MOTION_STATIONARY = 0,
    MOTION_LIGHT,
    MOTION_MODERATE,
    MOTION_INTENSE
} motion_state_t;

/*******************************************************************************
* Moving Average Filter Structure
*******************************************************************************/
typedef struct {
    float buffer[FILTER_WINDOW_SIZE];
    uint32_t head;
    uint32_t count;
    float sum;
} moving_avg_filter_t;

/*******************************************************************************
* Motion Statistics Structure
*******************************************************************************/
typedef struct {
    uint32_t stationary_count;
    uint32_t light_count;
    uint32_t moderate_count;
    uint32_t intense_count;
    float max_magnitude;
    float min_magnitude;
    uint32_t total_samples;
} motion_stats_t;

/*******************************************************************************
* Global Variables
*******************************************************************************/
static mtb_hal_i2c_t CYBSP_I2C_CONTROLLER_hal_obj;
static cy_stc_scb_i2c_context_t CYBSP_I2C_CONTROLLER_context;

static mtb_bmi270_data_t bmi270_data;
static mtb_bmi270_t bmi270;

static cy_en_scb_i2c_status_t initStatus;
static cy_rslt_t result;

/* Filter and Statistics objects */
static moving_avg_filter_t mag_filter = {0};
static motion_stats_t g_stats = {0};

สร้างฟังก์ชัน Raw accelerometer Conversion ชื่อ lsb_to_mp2()

/*******************************************************************************
 * Function Name: lsb_to_mps2
 *******************************************************************************
 * Summary:
 *   Converts raw accelerometer value to m/s²
 *
 * Parameters:
 *   val       - raw value from sensor (-32768 to +32767)
 *   g_range   - accelerometer range in g (2 = ±2g)
 *   bit_width - data width in bits (16)
 *
 * Return:
 *   float - acceleration in m/s²
 ******************************************************************************/
static float lsb_to_mps2(int16_t val, int8_t g_range, uint8_t bit_width)
{
    float half_scale = (float)(1u << (bit_width - 1u));
    return ((GRAVITY_EARTH) * val * g_range) / half_scale;
}

สร้างฟังก์ชัน Magnitude Calcuation ชื่อ calculate_magnitude()

/*******************************************************************************
 * Function Name: calculate_magnitude
 *******************************************************************************
 * Summary:
 *   Calculates the magnitude of acceleration vector: |acc| = sqrt(x² + y² + z²)
 *   Used for motion detection and impact sensing.
 *
 * Parameters:
 *   acc_x, acc_y, acc_z - acceleration components in m/s²
 *
 * Return:
 *   float - magnitude in m/s² (stationary ≈ 9.81 m/s² = 1g)
 *
 * Note:
 *   - Stationary: magnitude ≈ 9.81 m/s² (1g from gravity)
 *   - Moving: magnitude deviates from 9.81
 *   - Free fall: magnitude ≈ 0 (weightlessness)
 ******************************************************************************/
static float calculate_magnitude(float acc_x, float acc_y, float acc_z)
{
    return sqrtf(acc_x * acc_x + acc_y * acc_y + acc_z * acc_z);
}

สร้างฟังก์ชัน Filter

/*******************************************************************************
 * Function Name: filter_init
 *******************************************************************************
 * Summary:
 *   Initialize the moving average filter
 ******************************************************************************/
static void filter_init(moving_avg_filter_t *filter)
{
    memset(filter, 0, sizeof(moving_avg_filter_t));
}

/*******************************************************************************
 * Function Name: filter_update
 *******************************************************************************
 * Summary:
 *   Add new value to filter and return the current moving average.
 *   Uses optimized O(1) algorithm.
 ******************************************************************************/
static float filter_update(moving_avg_filter_t *filter, float new_value)
{
    if (filter->count >= FILTER_WINDOW_SIZE)
    {
        filter->sum -= filter->buffer[filter->head];
    }
    else
    {
        filter->count++;
    }

    filter->buffer[filter->head] = new_value;
    filter->sum += new_value;
    filter->head = (filter->head + 1) % FILTER_WINDOW_SIZE;

    return filter->sum / (float)filter->count;
}

สร้างฟังก์ชัน Motion Detection ชื่อ detect_motion()

/*******************************************************************************
 * Function Name: detect_motion
 *******************************************************************************
 * Summary:
 *   Detects motion level from acceleration magnitude
 ******************************************************************************/
static motion_state_t detect_motion(float magnitude)
{
    float deviation = fabsf(magnitude - GRAVITY_EARTH);

    if (deviation < 0.1f * GRAVITY_EARTH)
    {
        return MOTION_STATIONARY;
    }
    else if (deviation < 0.3f * GRAVITY_EARTH)
    {
        return MOTION_LIGHT;
    }
    else if (deviation < 0.6f * GRAVITY_EARTH)
    {
        return MOTION_MODERATE;
    }
    else
    {
        return MOTION_INTENSE;
    }
}

สร้างฟังก์ชันที่เหลือ

/*******************************************************************************
 * Function Name: get_motion_string
 *******************************************************************************
 * Summary:
 *   Converts motion state enum to display string
 ******************************************************************************/
static const char* get_motion_string(motion_state_t state)
{
    switch (state)
    {
        case MOTION_STATIONARY: return "STATIONARY  ";
        case MOTION_LIGHT:      return "LIGHT MOTION";
        case MOTION_MODERATE:   return "MODERATE    ";
        case MOTION_INTENSE:    return "INTENSE!    ";
        default:                return "UNKNOWN     ";
    }
}

/*******************************************************************************
 * Function Name: stats_init
 *******************************************************************************
 * Summary:
 *   Initialize motion statistics
 ******************************************************************************/
static void stats_init(motion_stats_t *stats)
{
    memset(stats, 0, sizeof(motion_stats_t));
    stats->min_magnitude = 100.0f;  /* Start high */
    stats->max_magnitude = 0.0f;
}

/*******************************************************************************
 * Function Name: stats_update
 *******************************************************************************
 * Summary:
 *   Update statistics from sensor reading
 ******************************************************************************/
static void stats_update(motion_stats_t *stats,
                         motion_state_t state,
                         float magnitude)
{
    stats->total_samples++;

    /* Count by state */
    switch (state)
    {
        case MOTION_STATIONARY: stats->stationary_count++; break;
        case MOTION_LIGHT:      stats->light_count++;      break;
        case MOTION_MODERATE:   stats->moderate_count++;   break;
        case MOTION_INTENSE:    stats->intense_count++;    break;
    }

    /* Track min/max */
    if (magnitude > stats->max_magnitude)
    {
        stats->max_magnitude = magnitude;
    }
    if (magnitude < stats->min_magnitude)
    {
        stats->min_magnitude = magnitude;
    }
}

/*******************************************************************************
 * Function Name: stats_print
 *******************************************************************************
 * Summary:
 *   Print statistics summary
 ******************************************************************************/
static void stats_print(const motion_stats_t *stats)
{
    if (stats->total_samples == 0) return;

    printf("\r\n========== Motion Statistics ==========\r\n");
    printf("Total Samples: %u\r\n", (unsigned int)stats->total_samples);
    printf("Stationary: %u (%.1f%%)\r\n",
           (unsigned int)stats->stationary_count,
           (double)(100.0f * stats->stationary_count / stats->total_samples));
    printf("Light:      %u (%.1f%%)\r\n",
           (unsigned int)stats->light_count,
           (double)(100.0f * stats->light_count / stats->total_samples));
    printf("Moderate:   %u (%.1f%%)\r\n",
           (unsigned int)stats->moderate_count,
           (double)(100.0f * stats->moderate_count / stats->total_samples));
    printf("Intense:    %u (%.1f%%)\r\n",
           (unsigned int)stats->intense_count,
           (double)(100.0f * stats->intense_count / stats->total_samples));
    printf("Min Magnitude: %.3f m/s^2 (%.2f g)\r\n",
           (double)stats->min_magnitude,
           (double)(stats->min_magnitude / GRAVITY_EARTH));
    printf("Max Magnitude: %.3f m/s^2 (%.2f g)\r\n",
           (double)stats->max_magnitude,
           (double)(stats->max_magnitude / GRAVITY_EARTH));
    printf("========================================\r\n\r\n");
}

3. เขียน Main Task Loop

/*******************************************************************************
 * Function Name: sensor_hub_daq_task
 *******************************************************************************
 * Summary:
 *   Complete motion detection system with filter and statistics
 ******************************************************************************/
static void sensor_hub_daq_task(void *pvParameters)
{
    TickType_t xLastWakeTime = 0;
    const TickType_t xDelay = 100 / portTICK_PERIOD_MS;  /* 10Hz */

    float acc_x = 0.0f;
    float acc_y = 0.0f;
    float acc_z = 0.0f;
    float magnitude = 0.0f;
    float filtered_magnitude = 0.0f;
    motion_state_t motion_state = MOTION_STATIONARY;
    uint32_t print_counter = 0;

    (void)pvParameters;

    /* Initialize I2C */
    initStatus = Cy_SCB_I2C_Init(CYBSP_I2C_CONTROLLER_HW,
                                &CYBSP_I2C_CONTROLLER_config,
                                &CYBSP_I2C_CONTROLLER_context);

    if (CY_SCB_I2C_SUCCESS != initStatus)
    {
        handle_app_error();
    }

    Cy_SCB_I2C_Enable(CYBSP_I2C_CONTROLLER_HW);

    result = mtb_hal_i2c_setup(&CYBSP_I2C_CONTROLLER_hal_obj,
                                &CYBSP_I2C_CONTROLLER_hal_config,
                                &CYBSP_I2C_CONTROLLER_context,
                                NULL);

    if (CY_RSLT_SUCCESS != result)
    {
        handle_app_error();
    }

    /* Initialize BMI270 */
    result = mtb_bmi270_init_i2c(&bmi270,
                                &CYBSP_I2C_CONTROLLER_hal_obj,
                                MTB_BMI270_ADDRESS_DEFAULT);

    if (CY_RSLT_SUCCESS != result)
    {
        handle_app_error();
    }

    result = mtb_bmi270_config_default(&bmi270);

    if (CY_RSLT_SUCCESS != result)
    {
        handle_app_error();
    }

    /* Initialize filter and statistics */
    filter_init(&mag_filter);
    stats_init(&g_stats);

    /* Clear screen */
    printf("\x1b[2J\x1b[;H");

    printf("************************************************************\r\n");
    printf("     PSOC Edge MCU: Complete Motion Detection System        \r\n");
    printf("************************************************************\r\n\r\n");

    /* Initialize LED to OFF */
    LED_OFF();

    xLastWakeTime = xTaskGetTickCount();

    for (;;)
    {
        /* Read sensor */
        result = mtb_bmi270_read(&bmi270, &bmi270_data);

        if (CY_RSLT_SUCCESS != result)
        {
            handle_app_error();
        }

        /* Convert to m/s² */
        acc_x = lsb_to_mps2(bmi270_data.sensor_data.acc.x,
                            ACC_RANGE_2G,
                            bmi270.sensor.resolution);
        acc_y = lsb_to_mps2(bmi270_data.sensor_data.acc.y,
                            ACC_RANGE_2G,
                            bmi270.sensor.resolution);
        acc_z = lsb_to_mps2(bmi270_data.sensor_data.acc.z,
                            ACC_RANGE_2G,
                            bmi270.sensor.resolution);

        /* Calculate magnitude */
        magnitude = calculate_magnitude(acc_x, acc_y, acc_z);

        /* Apply filter */
        filtered_magnitude = filter_update(&mag_filter, magnitude);

        /* Detect motion (use filtered value) */
        motion_state = detect_motion(filtered_magnitude);

        /* Update statistics */
        stats_update(&g_stats, motion_state, magnitude);

        /* Display current values */
        printf("Acc: [%7.3f, %7.3f, %7.3f] m/s^2\r\n",
               (double)acc_x, (double)acc_y, (double)acc_z);
        printf("Mag: Raw=%6.3f, Flt=%6.3f m/s^2 | %s\r\n",
               (double)magnitude,
               (double)filtered_magnitude,
               get_motion_string(motion_state));

        /* LED feedback - Active LOW (PSoC Edge E84) */
        switch (motion_state)
        {
            case MOTION_STATIONARY:
                LED_OFF();  /* Set = OFF for Active LOW */
                break;
            case MOTION_LIGHT:
                Cy_GPIO_Inv(CYBSP_USER_LED1_PORT, CYBSP_USER_LED1_PIN);  /* Blink */
                break;
            case MOTION_MODERATE:
            case MOTION_INTENSE:
                LED_ON();   /* Clr = ON for Active LOW */
                break;
        }

        /* Print statistics every STATS_REPORT_INTERVAL samples */
        print_counter++;
        if (print_counter >= STATS_REPORT_INTERVAL)
        {
            stats_print(&g_stats);
            print_counter = 0;
        }

        /* Move cursor back for clean display */
        printf("\x1b[2A");

        /* Wait for UART */
        while(!(Cy_SCB_UART_IsTxComplete(CYBSP_DEBUG_UART_HW))) {};

        vTaskDelayUntil(&xLastWakeTime, xDelay);
    }
}

Create Sensor Hub DAQ Task Function

/*******************************************************************************
 * Function Name: create_sensor_hub_daq_task
 ******************************************************************************/
cy_rslt_t create_sensor_hub_daq_task(void)
{
    BaseType_t status;

    status = xTaskCreate(sensor_hub_daq_task, "Motion System",
                            TASK_SENSOR_HUB_DAQ_STACK_SIZE, NULL,
                            TASK_SENSOR_HUB_DAQ_PRIORITY, NULL);

    return (pdPASS == status) ?
        CY_RSLT_SUCCESS : APP_RSLT_ACQ_TASK_CREATE_FAILED;
}

4. Build และ Flash

make build -j8
make program

5.3 Output ที่คาดหวัง

************************************************************
     PSOC Edge MCU: Motion Detection System
************************************************************

Acc: [  0.123,  -0.045,   9.801] m/s²
Mag: Raw= 9.802, Flt= 9.805 m/s² | STATIONARY

[หลัง 10 วินาที]
========== Motion Statistics ==========
Total Samples: 100
Stationary: 85 (85.0%)
Light:      10 (10.0%)
Moderate:   4 (4.0%)
Intense:    1 (1.0%)
Min Magnitude: 9.456 m/s² (0.96 g)
Max Magnitude: 15.234 m/s² (1.55 g)
========================================

Next Steps

Week 2 (IMU)              Week 3-4 (LVGL)              Week 5 (Scope)
┌─────────────┐          ┌─────────────┐             ┌─────────────┐
│ IMU Data +  │──────────│   Display   │─────────────│   Audio &   │
│ Processing  │          │   on LCD    │             │     FFT     │
└─────────────┘          └─────────────┘             └─────────────┘
      │                         │                           │
      │    Show motion stats    │     Signal analysis       │
      └─────────────────────────┴───────────────────────────┘

---

จบ Session 2 | ต่อไป: Session 3: LVGL Display