pachas
Dependencies: mbed QEI FastPWM
Diff: pressure_sensors.cpp
- Revision:
- 11:5cb7ae8bd831
- Parent:
- 9:95fdcdc0977e
- Child:
- 12:3bc2465b034a
diff -r b2d87404309a -r 5cb7ae8bd831 pressure_sensors.cpp --- a/pressure_sensors.cpp Mon Apr 27 19:52:02 2020 +0000 +++ b/pressure_sensors.cpp Thu May 07 03:52:31 2020 +0000 @@ -1,140 +1,120 @@ +/* File inclusion */ #include "mbed.h" #include "stdint.h" #include "math.h" #include "project_defines.h" #include "pressure_sensors.h" - +#include "filters.h" /* Object definition */ I2C pressure_sensor_01(PRESSURE_SENSOR_01_SDA_PIN, PRESSURE_SENSOR_01_SCL_PIN); I2C pressure_sensor_02(PRESSURE_SENSOR_02_SDA_PIN, PRESSURE_SENSOR_02_SCL_PIN); /* Global variable definition */ -int16_t pressure_01_bin, pressure_02_bin; float pressure_01_psi, pressure_02_psi; float flow_lpm, volume_ml; -float lpf_01_prev_out, lpf_02_prev_out; - -//float diff_pressure_psi; char sensor_read_data_buffer[2]; + /* Function definition */ void Pressure_Sensors_Initialize(void){ - int16_t sensor_01_aux, sensor_02_aux; - - pressure_sensor_01.frequency(PRESSURE_SENSOR_I2C_CLK_HZ ); - pressure_sensor_02.frequency(PRESSURE_SENSOR_I2C_CLK_HZ ); - - /* Perform an initial measurement */ - - pressure_sensor_01.read(PRESSURE_SENSOR_01_SRA, sensor_read_data_buffer, 2); - sensor_01_aux = ((sensor_read_data_buffer[0] << 8) | sensor_read_data_buffer[1]) & 0x3FFF; + pressure_sensor_01.frequency(PRESSURE_SENSOR_I2C_CLK_HZ); + pressure_sensor_02.frequency(PRESSURE_SENSOR_I2C_CLK_HZ); - if(sensor_01_aux < PRESSURE_SENSOR_OUTPUT_MIN){ - sensor_01_aux = PRESSURE_SENSOR_OUTPUT_MIN; - } - - pressure_sensor_02.read(PRESSURE_SENSOR_02_SRA, sensor_read_data_buffer, 2); - sensor_02_aux = ((sensor_read_data_buffer[0] << 8) | sensor_read_data_buffer[1]) & 0x3FFF; - - if(sensor_02_aux < PRESSURE_SENSOR_OUTPUT_MIN){ - sensor_02_aux = PRESSURE_SENSOR_OUTPUT_MIN; - } - - lpf_01_prev_out = (float)sensor_01_aux; - lpf_02_prev_out = (float)sensor_02_aux; - + /* Initialize filters */ + Pressure_01_Cheby2_LPF(0, 1); + Pressure_02_Cheby2_LPF(0, 1); + Flow_Cheby2_HPF(0, 1); + Flow_FIR_LPF(0,1); } void Pressure_Sensors_Read(void){ - int16_t raw_pressure_01_bin, raw_pressure_02_bin; + int16_t pressure_01_bin; + int16_t pressure_02_bin; - /* Read binary values */ - pressure_sensor_01.read(PRESSURE_SENSOR_01_SRA, sensor_read_data_buffer, 2); - raw_pressure_01_bin = ((sensor_read_data_buffer[0] << 8) | sensor_read_data_buffer[1]) & 0x3FFF; + /* Read pressure 1 (differential) binary value */ + + sensor_read_data_buffer[0] = 0; + sensor_read_data_buffer[1] = 0; - if(raw_pressure_01_bin < PRESSURE_SENSOR_OUTPUT_MIN){ - raw_pressure_01_bin = PRESSURE_SENSOR_OUTPUT_MIN; - } - - pressure_sensor_02.read(PRESSURE_SENSOR_02_SRA, sensor_read_data_buffer, 2); - raw_pressure_02_bin = ((sensor_read_data_buffer[0] << 8) | sensor_read_data_buffer[1]) & 0x3FFF; + pressure_sensor_01.read(PRESSURE_SENSOR_01_SRA, sensor_read_data_buffer, 2); + pressure_01_bin = ((sensor_read_data_buffer[0] << 8) | sensor_read_data_buffer[1]) & 0x3FFF; + if(pressure_01_bin < PRESSURE_SENSOR_OUTPUT_MIN){ + pressure_01_bin = PRESSURE_SENSOR_OUTPUT_MIN; + } + + /* Read pressure 2 binary value */ - if(raw_pressure_02_bin < PRESSURE_SENSOR_OUTPUT_MIN){ - raw_pressure_02_bin = PRESSURE_SENSOR_OUTPUT_MIN; - } + sensor_read_data_buffer[0] = 0; + sensor_read_data_buffer[1] = 0; - /* Filter binary values */ - pressure_01_bin = Low_Pass_Filter_01(raw_pressure_01_bin); - pressure_02_bin = Low_Pass_Filter_02(raw_pressure_02_bin); + pressure_sensor_02.read(PRESSURE_SENSOR_02_SRA, sensor_read_data_buffer, 2); + pressure_02_bin = ((sensor_read_data_buffer[0] << 8) | sensor_read_data_buffer[1]) & 0x3FFF; + if(pressure_02_bin < PRESSURE_SENSOR_OUTPUT_MIN){ + pressure_02_bin = PRESSURE_SENSOR_OUTPUT_MIN; + } /* Calculate pressure values in PSI */ pressure_01_psi = (pressure_01_bin - PRESSURE_SENSOR_OUTPUT_MIN) * (PRESSURE_SENSOR_PRESSURE_MAX - PRESSURE_SENSOR_PRESSURE_MIN); pressure_01_psi = (pressure_01_psi / (PRESSURE_SENSOR_OUTPUT_MAX - PRESSURE_SENSOR_OUTPUT_MIN)) + PRESSURE_SENSOR_PRESSURE_MIN; - //pressure_01_psi += PRESSURE_SENSOR_OFFSET_PSI; + pressure_01_psi += PRESSURE_SENSOR_01_OFFSET_PSI; + pressure_02_psi = (pressure_02_bin - PRESSURE_SENSOR_OUTPUT_MIN) * (PRESSURE_SENSOR_PRESSURE_MAX - PRESSURE_SENSOR_PRESSURE_MIN); pressure_02_psi = (pressure_02_psi / (PRESSURE_SENSOR_OUTPUT_MAX - PRESSURE_SENSOR_OUTPUT_MIN)) + PRESSURE_SENSOR_PRESSURE_MIN; - //pressure_02_psi += PRESSURE_SENSOR_OFFSET_PSI; + pressure_02_psi += PRESSURE_SENSOR_02_OFFSET_PSI; + + /* Filter pressure values */ + pressure_01_psi = Pressure_01_Cheby2_LPF(pressure_01_psi, 0); + pressure_02_psi = Pressure_02_Cheby2_LPF(pressure_02_psi, 0); + } - void Calculate_Flow_From_Pressure(void){ float diff_pressure_psi; - diff_pressure_psi = pressure_02_psi - pressure_01_psi; + diff_pressure_psi = pressure_02_psi - pressure_01_psi; if(diff_pressure_psi < 0){ - flow_lpm = -SPIROMETER_SENSOR_CONSTANT * sqrt(-diff_pressure_psi); + flow_lpm = -SPIROMETER_SENSOR_CONSTANT_OXYGEN * sqrt(-diff_pressure_psi); }else{ - flow_lpm = SPIROMETER_SENSOR_CONSTANT * sqrt(diff_pressure_psi); + flow_lpm = SPIROMETER_SENSOR_CONSTANT_OXYGEN * sqrt(diff_pressure_psi); } -} - -void Calculate_Volume_From_Flow(void){ - static float previous_flow_lpm = 0.0; - float delta_volume_ml; - - delta_volume_ml = (PRESSURE_SENSOR_READ_PERIOD_MS * (flow_lpm + previous_flow_lpm)) / 60.0; - - volume_ml = volume_ml + delta_volume_ml; - - previous_flow_lpm = flow_lpm; + /* Filter flow value */ + flow_lpm = Flow_FIR_LPF(flow_lpm, 0); + flow_lpm = Flow_Cheby2_HPF(flow_lpm, 0); } -int16_t Low_Pass_Filter_01(int16_t raw_value){ - - float lpf_out, sample_period_sec, div; + +/* Simpson's integration */ + +void Calculate_Volume_From_Flow(void){ + + static uint8_t integral_count = 0; + static float f_x[3] = {0.0, 0.0, 0.0}; + float delta_volume_ml; - sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS); - div = sample_period_sec + LPF_RC_CONSTANT_SEC; + f_x[2] = f_x[1]; + f_x[1] = f_x[0]; + f_x[0] = flow_lpm; + + integral_count++; - lpf_out = (((float)raw_value) * (sample_period_sec / div)) + (lpf_01_prev_out * (LPF_RC_CONSTANT_SEC / div)); - lpf_01_prev_out = lpf_out; + if(integral_count >= 2){ + delta_volume_ml = ((float)PRESSURE_SENSOR_READ_PERIOD_MS / 180.0) * (f_x[2] + (4 * f_x[1]) + f_x[0]); + volume_ml += delta_volume_ml; + integral_count = 0; + } - return ((int16_t)lpf_out); - } -int16_t Low_Pass_Filter_02(int16_t raw_value){ - - float lpf_out, sample_period_sec, div; - - sample_period_sec = 0.001 * ((float)PRESSURE_SENSOR_READ_PERIOD_MS); - div = sample_period_sec + LPF_RC_CONSTANT_SEC; - - lpf_out = (((float)raw_value) * (sample_period_sec / div)) + (lpf_02_prev_out * (LPF_RC_CONSTANT_SEC / div)); - lpf_02_prev_out = lpf_out; - - return ((int16_t)lpf_out); -}