Moving Average Filter

Lab 4: Moving Average Filter

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

• Noise Reduction: ข้อมูล sensor มี noise ต้องกรอง

• Smoother Data: ข้อมูลราบเรียบขึ้นสำหรับ motion detection

• Circular Buffer: โครงสร้างข้อมูลสำคัญใน Embedded

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

• Moving Average: ค่าเฉลี่ยเคลื่อนที่

• Circular Buffer: เก็บข้อมูลแบบวนรอบ

• Optimized Sum: ไม่ต้องบวกใหม่ทุกครั้ง

1. หลักการ Moving Average Filter

Moving Average = ค่าเฉลี่ยของ N samples ล่าสุด

1.1 สูตรหลักของ Moving Average (ค่าเฉลี่ยของ N ตัวล่าสุด)

ให้ x[k] คือ sample ล่าสุด ณ เวลา k และใช้หน้าต่างยาว N

$$y[k]  =   \frac{1}{N}\sum_{i=0}^{N-1} x[k-i]$$

ตัวอย่าง (N=5):

samples = [9.8, 9.9, 10.1, 9.7, 9.8]

average = (9.8 + 9.9 + 10.1 + 9.7 + 9.8) / 5 = 9.86

ข้อดี:

• ลด noise แบบสุ่ม

• ข้อมูลราบเรียบขึ้น

ข้อเสีย:

• มี delay (latency)

• ตอบสนองช้าลง

1.1.1 สูตรด้วยตัวแปร sum

• ถ้า buffer เต็มแล้ว: ลบค่าที่จะถูกเขียนทับออกจาก sum

• ใส่ค่าใหม่ แล้วบวกเข้า sum

• ค่าเฉลี่ย = sum / count

เขียนเป็น LaTeX ได้แบบนี้ (เมื่อ buffer เต็มแล้ว count=N):

$$S[k]  =  S[k−1]  −  x[k−N]  +  x[k]$$

$$y[k]  =  \frac{S[k]}{N}$$

กรณีช่วงเริ่มต้นที่ยังไม่ครบ N ตัว (เหมือน count++ ในโค้ด):

$$y[k]  =  \frac{S[k]}{\text{count}}, \quad \text{โดยที่ } \text{count}=\min(k+1,\,N)$$

1.2 Circular Buffer Visualization

Push: 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11

Buffer size = 10:
After push(10): [1, 2, 3, 4, 5, 6, 7, 8, 9, 10] head=0
After push(11): [11,2, 3, 4, 5, 6, 7, 8, 9, 10] head=1
                 ↑ overwrites oldest (1)

1.2.1 สูตร “Circular Buffer index” (head ที่วนด้วย modulo)

ในโค้ดมีประมาณ:

$$head←(head+1) mod N$$

(ใช้สื่อว่า index วนกลับไป 0 เมื่อครบหน้าต่าง)

---

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

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

2. เปิดโปรเจกต์ aic-psoc-edge-epc2-imu-filter

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

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

/* 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;      /* Position to write next */
    uint32_t count;     /* Current number of samples */
    float sum;          /* Running sum (optimized) */
} moving_avg_filter_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 object for magnitude */
static moving_avg_filter_t mag_filter = {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 (reset to zero)
 *
 * Parameters:
 *   filter - pointer to filter structure
 ******************************************************************************/
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: subtract oldest, add newest.
 *
 * Parameters:
 *   filter    - pointer to filter structure
 *   new_value - new sample to add
 *
 * Return:
 *   float - current moving average
 *
 * Algorithm:
 *   1. Subtract oldest value from sum (if buffer full)
 *   2. Add new value to buffer and sum
 *   3. Update head pointer (circular wrap)
 *   4. Return sum / count
 ******************************************************************************/
static float filter_update(moving_avg_filter_t *filter, float new_value)
{
    /* Subtract oldest value from sum (if buffer is full) */
    if (filter->count >= FILTER_WINDOW_SIZE)
    {
        filter->sum -= filter->buffer[filter->head];
    }
    else
    {
        filter->count++;
    }

    /* Add new value to buffer and sum */
    filter->buffer[filter->head] = new_value;
    filter->sum += new_value;

    /* Move head pointer (wrap around) */
    filter->head = (filter->head + 1) % FILTER_WINDOW_SIZE;

    /* Return average */
    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;
    }
}

สร้างฟังก์ชัน 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. เขียน Main Task Loop

/*******************************************************************************
 * Function Name: sensor_hub_daq_task
 *******************************************************************************
 * Summary:
 *   FreeRTOS task that reads BMI270 sensor with moving average filter
 ******************************************************************************/
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;
    float filtered_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();
    }

    /* Initialize filter */
    filter_init(&mag_filter);

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

    printf("************************************************************\r\n");
    printf("     Lab 2-4: Moving Average Filter                         \r\n");
    printf("************************************************************\r\n\r\n");
    printf("Filter Window Size: %d samples (%.1f sec at 10Hz)\r\n\r\n",
           FILTER_WINDOW_SIZE,
           (float)FILTER_WINDOW_SIZE / 10.0f);

    /* 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 raw magnitude */
        magnitude = calculate_magnitude(acc_x, acc_y, acc_z);

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

        /* Detect motion using filtered value */
        motion_state = detect_motion(filtered_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: Raw=%6.3f, Filtered=%6.3f m/s^2\r\n",
               (double)magnitude,
               (double)filtered_magnitude);
        printf("Motion: %s (deviation: %.2f g)\r\n",
               get_motion_string(motion_state),
               (double)(fabsf(filtered_magnitude - GRAVITY_EARTH) / GRAVITY_EARTH));

        /* LED feedback - Active LOW (PSoC Edge E84) */
        if (motion_state >= MOTION_MODERATE)
        {
            LED_ON();
        }
        else
        {
            LED_OFF();
        }

        /* 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, "IMU Filter",
                            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

Output ที่คาดหวัง

Magnitude: Raw= 9.856, Filtered= 9.812 m/s²
Motion: STATIONARY   (deviation: 0.00 g)

[เมื่อเขย่า - สังเกตว่า Filtered เปลี่ยนช้ากว่า Raw]
Magnitude: Raw=12.345, Filtered=10.456 m/s²
Motion: LIGHT MOTION (deviation: 0.07 g)

[หลังหยุดเขย่า - Filtered ค่อยๆ กลับสู่ปกติ]
Magnitude: Raw= 9.810, Filtered=10.123 m/s²
Motion: LIGHT MOTION (deviation: 0.03 g)

---

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