Basic Motion Detection

Lab 3: ตรวจจับการเคลื่อนไหว

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

• Activity Detection: ตรวจจับว่าอุปกรณ์เคลื่อนไหวหรือไม่

• State Classification: แบ่งประเภทการเคลื่อนไหว

• Event Triggering: ทำ action เมื่อตรวจพบ motion

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

• Threshold Logic: ใช้ค่า threshold ในการตัดสินใจ

• Enum States: สร้าง motion states

• fabsf(): คำนวณค่า absolute

Concept: Motion Detection

หลักการ:

• เมื่อนิ่ง: Magnitude ≈ 1g (9.81 m/s²)

• เมื่อเคลื่อนไหว: Magnitude เบี่ยงเบนจาก 1g

ใช้ Threshold:

• threshold = 0.3g (ประมาณ 3 m/s²)

• ถ้าค่า absolute ของ |magnitude - 1g| > threshold แสดงว่า Motion Detected!

ระดับ:

• < 0.1g: STATIONARY

• < 0.3g: LIGHT

• < 0.6g: MODERATE

• >= 0.6g: INTENSE

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

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

3.1 เปิดโปรเจกต์ aic-psoc-edge-epc2-imu-motion

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

aic-psoc-edge-epc2-imu-motion/
├── 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 */

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)

/* 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;

/*******************************************************************************
* 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;

สร้างฟังก์ชัน 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);
}

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

/*******************************************************************************
 * Function Name: detect_motion
 *******************************************************************************
 * Summary:
 *   Detects motion level from acceleration magnitude using threshold-based
 *   classification.
 *
 * Parameters:
 *   magnitude - acceleration magnitude in m/s²
 *
 * Return:
 *   motion_state_t - detected motion level
 *
 * Thresholds:
 *   - STATIONARY: deviation < 0.1g
 *   - LIGHT:      deviation < 0.3g
 *   - MODERATE:   deviation < 0.6g
 *   - INTENSE:    deviation >= 0.6g
 ******************************************************************************/
static motion_state_t detect_motion(float magnitude)
{
    /* Calculate deviation from 1g (gravity) */
    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;
    }
}

สร้างฟังก์ชัน Motion State Conversion ชื่อ get_motion_string()

/*******************************************************************************
 * Function Name: get_motion_string
 *******************************************************************************
 * Summary:
 *   Converts motion state enum to display string
 *
 * Parameters:
 *   state - motion state enum value
 *
 * Return:
 *   const char* - human-readable motion state 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     ";
    }
}

3.2 เขียน Main Task Loop

/*******************************************************************************
 * Function Name: sensor_hub_daq_task
 *******************************************************************************
 * Summary:
 *   FreeRTOS task that reads BMI270 sensor and performs motion detection
 ******************************************************************************/
static void sensor_hub_daq_task(void *pvParameters)
{
    TickType_t xLastWakeTime = 0;
    const TickType_t xDelay = 100 / portTICK_PERIOD_MS;

    float acc_x = 0.0f;
    float acc_y = 0.0f;
    float acc_z = 0.0f;
    float magnitude = 0.0f;
    motion_state_t motion_state = MOTION_STATIONARY;

    (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();
    }

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

    printf("************************************************************\r\n");
    printf("     Lab 2-3: Motion Detection                              \r\n");
    printf("************************************************************\r\n\r\n");
    printf("Motion Thresholds:\r\n");
    printf("  STATIONARY: < 0.1g | LIGHT: < 0.3g\r\n");
    printf("  MODERATE:   < 0.6g | INTENSE: >= 0.6g\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);

        /* Detect motion state */
        motion_state = detect_motion(magnitude);

        /* Display results */
        printf("Accelerometer:\r\n");
        printf("  x: %9.6f m/s^2\r\n", (double)acc_x);
        printf("  y: %9.6f m/s^2\r\n", (double)acc_y);
        printf("  z: %9.6f m/s^2\r\n", (double)acc_z);
        printf("\r\n");
        printf("Magnitude: %9.6f m/s^2 (%.2f g)\r\n",
               (double)magnitude,
               (double)(magnitude / GRAVITY_EARTH));
        printf("Motion: %s (deviation: %.2f g)\r\n",
               get_motion_string(motion_state),
               (double)(fabsf(magnitude - GRAVITY_EARTH) / GRAVITY_EARTH));

        /* LED feedback - Active LOW (PSoC Edge E84) */
        /* Turn LED ON when motion is MODERATE or higher */
        if (motion_state >= MOTION_MODERATE)
        {
            LED_ON();   /* Clr = ON for Active LOW */
        }
        else
        {
            LED_OFF();  /* Set = OFF for Active LOW */
        }

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

        /* 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 Detection",
                            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;
}

3.3 Build และ Flash

make build -j8
make program

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

Accelerometer:
x: 0.012345, y: -0.023456, z: 9.806650 m/s²
Magnitude: 9.806789 m/s² (1.00 g)
Motion: STATIONARY   (deviation: 0.00 g)

[เมื่อเขย่าเบา]
Magnitude: 10.784467 m/s² (1.10 g)
Motion: LIGHT MOTION (deviation: 0.10 g)

[เมื่อเขย่าแรง]
Magnitude: 14.709975 m/s² (1.50 g)
Motion: MODERATE     (deviation: 0.50 g)

[เมื่อตกกระแทก]
Magnitude: 24.516625 m/s² (2.50 g)
Motion: INTENSE!     (deviation: 1.50 g)

State Diagram

---

✍️ Exercise

1. Free Fall Detection - เพิ่มฟังก์ชันตรวจจับการตกอิสระ (magnitude < 0.3g)

Hint

/* LED Macros - Active HIGH (PSoC Edge E84) */
#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)
#define LED_TOGGLE() Cy_GPIO_Inv(CYBSP_USER_LED1_PORT, CYBSP_USER_LED1_PIN)

bool detect_free_fall(float magnitude)
{
    /* Free fall: magnitude < 0.3g */
    ...
}

/* Usage */
if (detect_free_fall(magnitude))
{
    printf("*** FREE FALL DETECTED! ***\r\n");
    /* Blink LED rapidly */
    for (int i = 0; i < 10; i++)
    {
        ...
    }
}

1. Impact Detection - เพิ่มฟังก์ชันตรวจจับการกระแทก (magnitude > 3g)

Hint

bool detect_impact(float magnitude, float threshold_g)
{
    ...
}

/* Usage */
if (detect_impact(magnitude, 3.0f))
{
    printf("*** IMPACT DETECTED (%.1f g) ***\r\n",
           (double)(magnitude / GRAVITY_EARTH));

    /* Log impact */
    static uint32_t impact_count = 0;
    ...
    printf("Total impacts: %lu\r\n", impact_count);
}

---

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