ESP32 Forum

Code: Select all

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <inttypes.h>
#include "hal/gpio_types.h"
#include "sdkconfig.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_chip_info.h"
#include "esp_flash.h"
#include "esp_log.h"
#include "driver/gpio.h"

#include "lis3dh.h"
#include "vibration.h"

/* Vibration Analysis Parameters */
#define samples             1024     
#define samplingFrequency   1344
#define ampFactor           3.7067873
#define GPIO_INPUT_PIN      15

SemaphoreHandle_t readSemaphore; // Semaphore handle
static const char *TAG = "esp-basic-vibration";

void VibrationTask(void *pvParameter)
{
    printf("============== Starting Vibration Analysis Task ==============\n");

    // Variables definition
    AxesAccel_t ac;
    float aRMS = 0;
    float vRMS = 0;
    float ax[samples];
    float ay[samples];
    float az[samples];
    float ve[samples];

    float Img[samples];

    // Temp debug
    uint8_t value = 0x02;
    bool sts = 0;
    unsigned int counter = 0;

    // Peripherals Init
    ESP_ERROR_CHECK(I2C_MASTER_INIT());
    ESP_LOGI(TAG, "I2C initialized successfully");

    // Accelerometer Config
    LIS3DH_SetInt1Pin(LIS3DH_I1_DRDY1_ON_INT1_ENABLE);
    LIS3DH_SetODR(LIS3DH_ODR_1344Hz_NP_5367HZ_LP);
    LIS3DH_SetMode(LIS3DH_NORMAL);
    LIS3DH_SetFullScale(LIS3DH_FULLSCALE_8);
    LIS3DH_SetAxis(LIS3DH_X_ENABLE | LIS3DH_Y_ENABLE | LIS3DH_Z_ENABLE);

    // Config Internal High Pass Filter
    // LIS3DH_SetMode(LIS3DH_HPM_NORMAL_MODE);
    // LIS3DH_SetHPFCutOFF(LIS3DH_HPFCF_3);
    // LIS3DH_SetFilterDataSel(0x00);

    sts = LIS3DH_GetWHO_AM_I(&value);
    printf("Read Status: %d\n", sts);
    printf("Who I am : %d\n", value);

    vTaskDelay(2000 / portTICK_PERIOD_MS);
    LIS3DH_GetAccAxes(&ac); 

    while (true) 
    {   

        // Get acceleration data in mg and save each axis
        LIS3DH_GetAccAxes(&ac); 
        az[counter] = ac.axis_z;
        ay[counter] = ac.axis_y;
        ax[counter] = ac.axis_x;

        // Espera a que la interrupción indique que se puede leer el sensor
        xSemaphoreTake(readSemaphore, 100 / portTICK_PERIOD_MS);

        if (counter >= samples)
        {   
            printf("============== Running Vibration Analysis Task ==============\n");
            printf("= Counter value : %d\n", counter);

            // Counter reset
            counter = 0;

            // Remove DC Component
            RemoveOffset(ax, samples);
            RemoveOffset(ay, samples);
            RemoveOffset(az, samples);

            // Get Acceleration resultant 3 axis
            // Resultant(ax, ay, az, samples);

            // Get velocity applying numerical integration
            Velocity(ax, ve, samples, samplingFrequency);

            // Get Vibration RMS Values
            RMS_Value(ax, samples, &aRMS);
            RMS_Value(ve, samples, &vRMS);

            // Save vector data on vReal and vImg for FFT
            memset(Img, 0, samples * sizeof(float));

            // FFT
            FFT_Init(ve, Img, samples, samplingFrequency);
            FFT_Windowing(FFT_WIN_TYP_HAMMING, FFT_FORWARD); // Recommended use for high frequencies. Relative to Sampling Frequency. In this case: 70Hz
            FFT_Compute(FFT_FORWARD);
            FFT_ComplexToMagnitude();

            // Find Peaks
            float amp[5], freq[5];
            FFT_MajorPeaks(&amp[0], &freq[0], 1, 3);

            // printf("============== Ended Vibration Analysis ==============\n");

            // =========== DEBUG LOG ================
            // printf("[$] Ac RMS : %.2f mg.\n", aRMS);
            // printf("[$] Ve RMS : %.2f mm/s.\n", vRMS);
            // printf("1st Peak -> Freq: %.2f Hz - Amp: %.2f mm/s \n", freq[0], amp[0]);
            // printf("2nd Peak -> Freq: %.2f Hz - Amp: %.2f mm/s \n", freq[0], amp[0]);
            // printf("3rd Peak -> Freq: %.2f Hz - Amp: %.2f mm/s \n", freq[0], amp[0]);
            
            // for (uint i = 128; i < 256; i++) {
            //     printf("[#] Accel [%d] --> X: %.2f  -  Y: %.2f  -  Z: %.2f\n", i, ax[i], ay[i], az[i]);
            // }

            vTaskDelay(2000 / portTICK_PERIOD_MS);
        }
        else 
            counter++;

    }
}

void IRAM_ATTR isr_handler(void *arg)
{   
    BaseType_t xHigherPriorityTaskWoken = pdFALSE;
    xSemaphoreGiveFromISR(readSemaphore, &xHigherPriorityTaskWoken); // Se despierta la tarea que espera por el semáforo
    if (xHigherPriorityTaskWoken) {
        portYIELD_FROM_ISR(); // Se cambia a una tarea de mayor prioridad
    }
}

void configure_interrupt(void)
{

    gpio_config_t gpio_cfg = {
        .intr_type = GPIO_INTR_POSEDGE, // Configurar el modo de interrupción como flanco de subida
        .mode = GPIO_MODE_INPUT, // Configurar el modo de pin como entrada
        .pin_bit_mask = (1ULL << GPIO_INPUT_PIN) // Configurar el pin que se utilizará como entrada
    };
    gpio_config(&gpio_cfg);

    // Configurar la rutina de interrupción
    gpio_install_isr_service(ESP_INTR_FLAG_LEVEL3); // Nivel de prioridad de interrupción
    gpio_isr_handler_add(GPIO_INPUT_PIN, isr_handler, NULL); // Configurar el manejador de interrupción

}

void app_main(void)
{

    configure_interrupt();
    readSemaphore = xSemaphoreCreateBinary(); // Se crea el semáforo
    xTaskCreate(&VibrationTask, "xVibrationTask", 24576, NULL, 5, NULL);

}

Code: Select all

xSemaphoreTake(readSemaphore, 100 / portTICK_PERIOD_MS);