IMU Sensor Hub

Lab 1: ทำความเข้าใจโค้ด Sensor Hub

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

• IMU Sensor: เข้าใจ Accelerometer + Gyroscope

• I2C Communication: สื่อสารกับ sensor ภายนอก

• Unit Conversion: แปลง raw data เป็นหน่วยทางฟิสิกส์

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

• BMI270: 6-axis IMU sensor

• lsb_to_mps2(): แปลง raw → m/s²

• lsb_to_rps(): แปลง raw → rad/s

• Orientation Detection: ตรวจจับทิศทางการวาง

1.1 เปิดโปรเจกต์ 2-1-PSOC_Edge_IMU_Basic

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

2-1-PSOC_Edge_IMU_Basic/
├── proj_cm33_s/              # Secure Project
├── proj_cm33_ns/             
│   ├── main.c
│   ├── sensor_hub_daq_task.c 
│   └── sensor_hub_daq_task.h
└── proj_cm55/                # CM55 Project

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)
#define RESET_VAL_ZERO                      (0)

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

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

Unit Conversion: Accelerometer

/*******************************************************************************
 * Function Name: lsb_to_mps2
 *******************************************************************************
* Summary:
* This function converts the raw sensor value to meter/sec^2.
*
* Parameters:
*  val       raw value
*  g_range   The accelerometer range is expressed in multiples of g.
*  bit_width data width in bits
*
* Return:
*  float
*
*******************************************************************************/
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;
}

Unit Conversion: Gyroscope

/*******************************************************************************
 * Function Name: lsb_to_rps
 *******************************************************************************
* Summary:
* This function converts the raw gyroscope sensor value to rad/sec.
*
* Parameters:
*  val       raw value
*  dps       The gyroscope sensor range in degrees per second
*  bit_width data width in bits
*
* Return:
*  float
*
*******************************************************************************/
static float lsb_to_rps(int16_t val, float dps, uint8_t bit_width)
{
    float half_scale = (float) (1u << (bit_width - 1u));

    return ((DEG_TO_RAD) * ((dps) / (half_scale)) * (val));
}

Motion Sensor Update Orientation

/*******************************************************************************
 * Function Name: motion_sensor_update_orientation
 *******************************************************************************
 * Summary:
 *  Function that updates the orientation status to one of the 6 types,
 *  'ORIENTATION_UP, ORIENTATION_DOWN, TOP_EDGE, BOTTOM_EDGE,
 *  LEFT_EDGE, and RIGHT_EDGE'. This functions detects the axis that is most 
 *  perpendicular to the ground based on the absolute value of acceleration in 
 *  that axis. The sign of the acceleration signifies whether the axis is 
 *  facing the ground or the opposite.
 *
 * Return:
 *  CY_RSLT_SUCCESS upon successful orientation update, else a non-zero value
 *  that indicates the error.
 *
 ******************************************************************************/
static cy_rslt_t motion_sensor_update_orientation(void)
{
    /* Status variable */
    cy_rslt_t result = CY_RSLT_SUCCESS;
    int16_t abs_x;
    int16_t abs_y;
    int16_t abs_z;

    /* Read x, y, z components of acceleration */
    result = mtb_bmi270_read(&bmi270, &bmi270_data);
    if (CY_RSLT_SUCCESS != result)
    {
        printf("read data failed\r\n");
    }
    abs_x = abs(bmi270_data.sensor_data.acc.x);
    abs_y = abs(bmi270_data.sensor_data.acc.y);
    abs_z = abs(bmi270_data.sensor_data.acc.z);

    if ((abs_z > abs_x) && (abs_z > abs_y))
    {
        if (bmi270_data.sensor_data.acc.z < RESET_VAL_ZERO)
        {
            /* Kit faces down (towards the ground) */
            printf("Orientation = ORIENTATION_DOWN       \r\n");
        }
        else
        {
            /* Kit faces up (towards the sky/ceiling) */
            printf("Orientation = ORIENTATION_UP         \r\n");
        }
    }
    /* Y axis (parallel with shorter edge of board) is most aligned with
     * gravity.
     */
    else if ((abs_y > abs_x) && (abs_y > abs_z))
    {
        if (bmi270_data.sensor_data.acc.y > RESET_VAL_ZERO)
        {
            /* Kit has an inverted landscape orientation */
            printf("Orientation = ORIENTATION_BOTTOM_EDGE\r\n");
        }
        else
        {
            /* Kit has landscape orientation */
            printf("Orientation = ORIENTATION_TOP_EDGE  \r\n");
        }
    }
    /* X axis (parallel with longer edge of board) is most aligned with
     * gravity.
     */
    else
    {
        if (bmi270_data.sensor_data.acc.x < RESET_VAL_ZERO)
        {
            /* Kit has an inverted portrait orientation */
            printf("Orientation = ORIENTATION_RIGHT_EDGE \r\n");
        }
        else
        {
            /* Kit has portrait orientation */
            printf("Orientation = ORIENTATION_LEFT_EDGE  \r\n");
        }
    }
    return result;
}

1.2 เขียน Main Task Loop

/*******************************************************************************
 * Function Name: sensor_hub_daq_task
 *******************************************************************************
* Summary:
* This function initializes the I2C and BMI270 sensor, reads the sensor values,
* and prints them on the UART terminal.
*
* Parameters:
*  void *
*
* Return:
*  void
*
*******************************************************************************/
static void sensor_hub_daq_task(void *pvParameters)
{

    TickType_t xLastWakeTime = 0;
    const TickType_t xDelay = 100 / portTICK_PERIOD_MS;

    float x = 0.0f;
    float y = 0.0f;
    float z = 0.0f;

    (void)pvParameters;

    initStatus = Cy_SCB_I2C_Init(CYBSP_I2C_CONTROLLER_HW, 
                                &CYBSP_I2C_CONTROLLER_config, 
                                &CYBSP_I2C_CONTROLLER_context);

    /* I2C initialization failed. Stop program execution. */
    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);

    /* HAL I2C setup failed. Stop program execution. */
    if (CY_RSLT_SUCCESS != result)
    {
        handle_app_error();
    }

    /* Initialize can configure platform-dependent function pointers. */
    result = mtb_bmi270_init_i2c(&bmi270, 
                                &CYBSP_I2C_CONTROLLER_hal_obj, 
                                MTB_BMI270_ADDRESS_DEFAULT);
    
    /* BMI270 sensor initialization failed. Stop program execution. */
    if (CY_RSLT_SUCCESS != result)
    {
        handle_app_error();
    }

    /* Configure accelerometer and gyroscope. */
    result = mtb_bmi270_config_default(&bmi270);

    /* BMI270 sensor configuration failed. Stop program execution. */
    if (CY_RSLT_SUCCESS != result)
    {
        handle_app_error();
    }

    /* \x1b[2J\x1b[;H - ANSI ESC sequence for clear screen */
    printf("\x1b[2J\x1b[;H");

    printf("************************************************************\n");
    printf("               PSOC Edge MCU: Sensor hub IMU                \n");
    printf("************************************************************\r\n");
    printf("Code example configured in data acquisition mode\r\n\n");

    /* Initialize the xLastWakeTime with the current tick count. */
    xLastWakeTime = xTaskGetTickCount();

    for (;;)
    {
        /* Get accelerometer and gyroscope sensor data. */
        result = mtb_bmi270_read(&bmi270, &bmi270_data);

        /* BMI270 sensor read failed. Stop program execution. */
        if (CY_RSLT_SUCCESS != result)
        {
            handle_app_error();
        }

        x = lsb_to_mps2(bmi270_data.sensor_data.acc.x, 
                        ACC_RANGE_2G,
                        bmi270.sensor.resolution);
        y = lsb_to_mps2(bmi270_data.sensor_data.acc.y, 
                        ACC_RANGE_2G,
                        bmi270.sensor.resolution);
        z = lsb_to_mps2(bmi270_data.sensor_data.acc.z, 
                        ACC_RANGE_2G,
                        bmi270.sensor.resolution);
        printf("Accelerometer: \n\rx:%9.6f, y:%9.6f, z:%9.6f \n\n",
                 (double)x, (double)y, (double)z);

        x = lsb_to_rps(bmi270_data.sensor_data.gyr.x, 
                        GYR_RANGE_DPS,
                        bmi270.sensor.resolution);
        y = lsb_to_rps(bmi270_data.sensor_data.gyr.y, 
                        GYR_RANGE_DPS,
                        bmi270.sensor.resolution);
        z = lsb_to_rps(bmi270_data.sensor_data.gyr.z, 
                        GYR_RANGE_DPS,
                        bmi270.sensor.resolution);
        printf("Gyroscope: \n\rx:%9.6f, y:%9.6f, z:%9.6f \n\n",
                 (double)x, (double)y, (double)z);

        motion_sensor_update_orientation();

        /* \x1b[7A\x1b[42D - ANSI ESC sequence to get the cursor back 
         * to the start. */
        printf("\x1b[7A\x1b[42D");

        /* Toggle user led */
        Cy_GPIO_Inv(CYBSP_USER_LED1_PORT, CYBSP_USER_LED1_PIN);

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

        /* Wait for the next cycle.*/
        vTaskDelayUntil(&xLastWakeTime, xDelay);
    }
}

Create Sensor Hub DAQ Task Function

/*******************************************************************************
* Function Name: create_sensor_hub_daq_task
********************************************************************************
* Summary:
*  Function that creates Sensor Hub data acquisition task
*
* Parameters:
*  None
*
* Return:
*  CY_RSLT_SUCCESS upon successful creation of the motion sensor task, else a
*  non-zero value that indicates the error.
*
*******************************************************************************/
cy_rslt_t create_sensor_hub_daq_task(void)
{
    BaseType_t status;

    /* Create the "sensor_hub_daq" Task */
    status = xTaskCreate(sensor_hub_daq_task, "Sensor Hub DAQ",
                            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;
}

1.3 Build และ Flash

make build -j8
make program

ทดลองหมุนบอร์ด

ลองหมุนบอร์ดไปในทิศทางต่างๆ และสังเกต:

• ค่า accelerometer เปลี่ยนอย่างไร

• Orientation เปลี่ยนอย่างไร

# Build proj_cm33_s → proj_cm33_ns → proj_cm55
# Program และเปิด Terminal (115200 baud)

# จะเห็น output:
# Accelerometer:
# x: 0.012345, y: -0.023456, z: 9.806650
#
# Gyroscope:
# x: 0.001234, y: 0.002345, z: -0.003456
#
# Orientation = ORIENTATION_UP

✍️ Exercise

1. แปลงค่า : BMI270 อ่านได้ raw_x = 8192 (range ±2g, 16-bit) ให้เขียนโค้ดเพื่อคำนวณค่า acceleration ในหน่วย m/s²

2. Orientation Logic : เขียนโค้ดตรวจสอบว่าบอร์ดวางอยู่ในทิศทาง:

   • FLAT_UP

   • FLAT_DOWN

   • TILTED

---

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