Untitled
unknown
plain_text
10 months ago
22 kB
4
Indexable
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file : main.c
* @brief : Main program body
******************************************************************************
* @attention
*
* Copyright (c) 2023 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include <string.h>
#include <stdio.h>
#include <PID.h>
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define MS_TO_M 60000 // for converting to flow rate from miliseconds to minutes [slm]
// pressure sensor
#define ADC_BUF_LEN 24
#define SAMPLES 100
#define SAMPLES1000 1000
#define m 0.0634 //calibration slope for pressure sensor -> m = (bar_max - bar_min)/(V_max-V_min)
#define DEFAULT_MIN_PRESSURE 5
#define DEFAULT_MAX_PRESSURE 40
// turbine control
#define PID_KP 6 //22
#define PID_KI 0.001 //0.005699
#define PID_KD 0.002 //0.0046
// flow sensor
#define ADDR 64<<1
#define OFFSET_FLOW 32768
#define SCALE_FLOW 120
// PEEP valve control
#define PID_PEEP_KP 10
#define PID_PEEP_KI 0.005699
#define PID_PEEP_KD 0.0046
#define PWM_PEEP_MAX 1800
#define PWM_PEEP_MIN 1300
#define PWM_TURBINE_MIN 10
// flow sensor
#define ADDR 64<<1
#define OFFSET_FLOW 32768
#define SCALE_FLOW 120
//Communication
#define BUFFER_LENGTH_RX 128
#define BUFFER_LENGTH_TX 128
//safety check
#define PWM_SAVE_MIN_FLOW 10
#define PWM_SAVE_MAX_FLOW 50
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;
DMA_HandleTypeDef hdma_adc1;
I2C_HandleTypeDef hi2c3;
TIM_HandleTypeDef htim2;
TIM_HandleTypeDef htim10;
TIM_HandleTypeDef htim11;
UART_HandleTypeDef huart1;
/* USER CODE BEGIN PV */
// pressure and oxygen sensors
unsigned short int adc_buf[ADC_BUF_LEN] = {0}; // TODO: check if DMA is still necessary
float adcSumPressure;
float adcSumPressureIn;
float adcSumOxygen;
float adcVoltagePressure;
float adcVoltagePressureIn;
float adcVoltageOxygen;
float adcAveragePressure;
float adcAveragePressureIn;
float adcAverageOxygen;
float pressureBar;
float pressureBarIn;
float pressureCmH2O;
float pressureCmH2OIn;
float OxygenLevel;
int adcCounter = 0;
int adcCounter1000 = 0;
// breath control
int breathCounter = 1;
int inhaleTime = 2500;
int exhaleTime = 3000;
// turbine control
pid_data pidData;
float pwmSpeed;
//PEEP valve control
pid_data pidPEEP;
// flow sensor
uint8_t i2cTransmitBuf[2] = {0x10, 0x00};
uint8_t i2cReceiveBuf[2];
float flowSLM;
float flowRate = 1;
float volume = 0;
uint8_t counter;
// UART communication with Raspberry Pi
//Receive
uint8_t receivedBuffer[BUFFER_LENGTH_RX]; //Data buffer
/*volatile uint8_t intRxCplt; //Flag signaling receive of new byte
uint16_t positionInRxTab = 0; //Position of the frame in buffer
uint16_t nonReceivedExpectedBytesCounter; //Counter of the non-received but expected bytes
//Transmit
static uint8_t dataToTx[BUFFER_LENGTH_TX]; //Data buffer
uint16_t positionInTxTab = 0; //Position of the frame in buffer
*/
int maxPressure;
int maxVolume;
uint8_t receive[10];
uint8_t receive_buffer;
uint8_t transmit[4];
uint8_t mode = 0;
uint8_t PEEP = DEFAULT_MIN_PRESSURE;
HAL_StatusTypeDef status;
int maxPressure_t, maxVolume_t, inhaleTime_t, exhaleTime_t;
uint8_t startByte = 0xFF;
uint8_t stopByte = 0xEE;
uint8_t ackByte = 0xFE;
/* USER CODE END PV */
/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_DMA_Init(void);
static void MX_ADC1_Init(void);
static void MX_TIM10_Init(void);
static void MX_TIM11_Init(void);
static void MX_I2C3_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_TIM2_Init(void);
/* USER CODE BEGIN PFP */
int pwmPEEP;
float pidOutput;
void controlPEEP(void){
pwmPEEP = PWM_PEEP_MIN;
pidOutput = pid_calculate(&pidPEEP, PEEP, pressureCmH2O);
pwmPEEP -= (int16_t)pidOutput;
if(pwmPEEP < PWM_PEEP_MIN) pwmPEEP = PWM_PEEP_MIN;
if(pwmPEEP > PWM_PEEP_MAX) pwmPEEP = PWM_PEEP_MAX;
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, pwmPEEP);
}
void safetyCheck(double pressure, double minPressure, double maxPressure) {
if (pressure < minPressure) {
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, PWM_SAVE_MAX_FLOW);
} else if (pressure > maxPressure) {
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, PWM_SAVE_MIN_FLOW);
}
}
void pressureMode(void){
//breath control logic
if(breathCounter == inhaleTime) //start of exhale phase
{
pwmSpeed = PWM_TURBINE_MIN;
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, (uint16_t)pwmSpeed);
controlPEEP();
HAL_GPIO_WritePin(GPIOB, VALVE_IN_Pin, 1);
breathCounter++;
pid_reset(&pidData);
}
else if (breathCounter == (inhaleTime + exhaleTime)) //start of inhale phase
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 1800);
HAL_GPIO_WritePin(GPIOB, VALVE_IN_Pin, 0);
breathCounter = 0;
}
else
{
if(breathCounter < inhaleTime) // in inhale phase
{
pwmSpeed = pid_calculate(&pidData, 35, pressureCmH2O);
if(pwmSpeed < PWM_TURBINE_MIN){
pwmSpeed = PWM_TURBINE_MIN;
}
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, (uint16_t)pwmSpeed);
}
breathCounter++;
}
}
void flowMode(void){
flowRate = 30; //(1/inhaleTime)*MS_TO_M; // flow rate = (Volume[L] / Inhale Time[ms]) * 60000
if(breathCounter == inhaleTime) //start of exhale phase
{
pwmSpeed = PWM_TURBINE_MIN;
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, (uint16_t)pwmSpeed);
controlPEEP();
pid_reset(&pidPEEP);
HAL_GPIO_WritePin(GPIOB, VALVE_IN_Pin, 1);
breathCounter++;
pid_reset(&pidData);
volume = 0;
}
else if (breathCounter == (inhaleTime + exhaleTime)) //start of inhale phase
{
__HAL_TIM_SET_COMPARE(&htim2, TIM_CHANNEL_1, 1800);
HAL_GPIO_WritePin(GPIOB, VALVE_IN_Pin, 0);
breathCounter = 0;
}
else
{
if(breathCounter < inhaleTime) // in inhale phase
{
pwmSpeed = pid_calculate(&pidData, 35, pressureCmH2O);
if(pwmSpeed < PWM_TURBINE_MIN){
pwmSpeed = PWM_TURBINE_MIN;
}
__HAL_TIM_SET_COMPARE(&htim10, TIM_CHANNEL_1, (uint16_t)pwmSpeed);
}
if(breathCounter == 1000) {
volume += flowSLM;
}
breathCounter++;
}
}
//Communication
/*void receiveData(void)
{
if (intRxCplt)
{
intRxCplt = 0;
nonReceivedExpectedBytesCounter = 0;
}
else
{
nonReceivedExpectedBytesCounter++;
if (nonReceivedExpectedBytesCounter > 50)
{
uint8_t temp = USART1->DR;
HAL_UART_Receive_IT(&huart1, dataFromRx, 1);
nonReceivedExpectedBytesCounter = 0;
}
return;
}
__disable_irq();
positionInRxTab = 0;
processReceivedData();
__enable_irq();
}
void processReceivedData(void)
{
uint8_t i = 1;
char x = dataFromRx[++i];
char D = dataFromRx[++i];
}*/
/* USER CODE END PFP */
/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
/**
* @brief The application entry point.
* @retval int
*/
int main(void)
{
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */
/* MCU Configuration--------------------------------------------------------*/
/* Reset of all peripherals, Initializes the Flash interface and the Systick. */
HAL_Init();
/* USER CODE BEGIN Init */
pid_init(&pidData, PID_KP, PID_KI, PID_KD, 0);
pid_init(&pidPEEP, PID_PEEP_KP, PID_PEEP_KI, PID_PEEP_KD, 0);
/* USER CODE END Init */
/* Configure the system clock */
SystemClock_Config();
/* USER CODE BEGIN SysInit */
/* USER CODE END SysInit */
/* Initialize all configured peripherals */
MX_GPIO_Init();
MX_DMA_Init();
MX_ADC1_Init();
MX_TIM10_Init();
MX_TIM11_Init();
MX_I2C3_Init();
MX_USART1_UART_Init();
MX_TIM2_Init();
/* USER CODE BEGIN 2 */
HAL_ADC_Start_DMA(&hadc1, (uint32_t*)adc_buf, ADC_BUF_LEN);
HAL_TIM_PWM_Start(&htim10, TIM_CHANNEL_1);
HAL_TIM_PWM_Start(&htim2, TIM_CHANNEL_1);
HAL_TIM_Base_Start_IT(&htim11);
HAL_GPIO_WritePin(GPIOB, VALVE_Pin, 1);
HAL_GPIO_WritePin(GPIOB, VALVE_IN_Pin, 0);
HAL_I2C_Master_Transmit(&hi2c3, ADDR, i2cTransmitBuf, sizeof(i2cTransmitBuf), HAL_MAX_DELAY);
//transmit[0] = indicator;
//receive_elements_counter = 0;
counter = 0;
//HAL_UART_Transmit_IT(&huart1, transmit, sizeof(transmit));
HAL_UART_Receive_IT(&huart1, receive, sizeof(receive));
/* USER CODE END 2 */
/* Infinite loop */
/* USER CODE BEGIN WHILE */
while (1)
{
/* USER CODE END WHILE */
/* USER CODE BEGIN 3 */
}
/* USER CODE END 3 */
}
/**
* @brief System Clock Configuration
* @retval None
*/
void SystemClock_Config(void)
{
RCC_OscInitTypeDef RCC_OscInitStruct = {0};
RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};
/** Configure the main internal regulator output voltage
*/
__HAL_RCC_PWR_CLK_ENABLE();
__HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);
/** Initializes the RCC Oscillators according to the specified parameters
* in the RCC_OscInitTypeDef structure.
*/
RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
RCC_OscInitStruct.HSIState = RCC_HSI_ON;
RCC_OscInitStruct.HSICalibrationValue = RCC_HSICALIBRATION_DEFAULT;
RCC_OscInitStruct.PLL.PLLState = RCC_PLL_NONE;
if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
{
Error_Handler();
}
/** Initializes the CPU, AHB and APB buses clocks
*/
RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
|RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_HSI;
RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;
if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
{
Error_Handler();
}
}
/**
* @brief ADC1 Initialization Function
* @param None
* @retval None
*/
static void MX_ADC1_Init(void)
{
/* USER CODE BEGIN ADC1_Init 0 */
/* USER CODE END ADC1_Init 0 */
ADC_ChannelConfTypeDef sConfig = {0};
/* USER CODE BEGIN ADC1_Init 1 */
/* USER CODE END ADC1_Init 1 */
/** Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion)
*/
hadc1.Instance = ADC1;
hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV2;
hadc1.Init.Resolution = ADC_RESOLUTION_12B;
hadc1.Init.ScanConvMode = ENABLE;
hadc1.Init.ContinuousConvMode = ENABLE;
hadc1.Init.DiscontinuousConvMode = DISABLE;
hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
hadc1.Init.NbrOfConversion = 3;
hadc1.Init.DMAContinuousRequests = ENABLE;
hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
if (HAL_ADC_Init(&hadc1) != HAL_OK)
{
Error_Handler();
}
/** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_0;
sConfig.Rank = 1;
sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_1;
sConfig.Rank = 2;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/** Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time.
*/
sConfig.Channel = ADC_CHANNEL_2;
sConfig.Rank = 3;
if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN ADC1_Init 2 */
/* USER CODE END ADC1_Init 2 */
}
/**
* @brief I2C3 Initialization Function
* @param None
* @retval None
*/
static void MX_I2C3_Init(void)
{
/* USER CODE BEGIN I2C3_Init 0 */
/* USER CODE END I2C3_Init 0 */
/* USER CODE BEGIN I2C3_Init 1 */
/* USER CODE END I2C3_Init 1 */
hi2c3.Instance = I2C3;
hi2c3.Init.ClockSpeed = 100000;
hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2;
hi2c3.Init.OwnAddress1 = 0;
hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
hi2c3.Init.OwnAddress2 = 0;
hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
if (HAL_I2C_Init(&hi2c3) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN I2C3_Init 2 */
/* USER CODE END I2C3_Init 2 */
}
/**
* @brief TIM2 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM2_Init(void)
{
/* USER CODE BEGIN TIM2_Init 0 */
/* USER CODE END TIM2_Init 0 */
TIM_MasterConfigTypeDef sMasterConfig = {0};
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM2_Init 1 */
/* USER CODE END TIM2_Init 1 */
htim2.Instance = TIM2;
htim2.Init.Prescaler = 15;
htim2.Init.CounterMode = TIM_COUNTERMODE_UP;
htim2.Init.Period = 19999;
htim2.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim2.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_PWM_Init(&htim2) != HAL_OK)
{
Error_Handler();
}
sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
if (HAL_TIMEx_MasterConfigSynchronization(&htim2, &sMasterConfig) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim2, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM2_Init 2 */
/* USER CODE END TIM2_Init 2 */
HAL_TIM_MspPostInit(&htim2);
}
/**
* @brief TIM10 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM10_Init(void)
{
/* USER CODE BEGIN TIM10_Init 0 */
/* USER CODE END TIM10_Init 0 */
TIM_OC_InitTypeDef sConfigOC = {0};
/* USER CODE BEGIN TIM10_Init 1 */
/* USER CODE END TIM10_Init 1 */
htim10.Instance = TIM10;
htim10.Init.Prescaler = 0;
htim10.Init.CounterMode = TIM_COUNTERMODE_UP;
htim10.Init.Period = 100-1;
htim10.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim10.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
if (HAL_TIM_Base_Init(&htim10) != HAL_OK)
{
Error_Handler();
}
if (HAL_TIM_PWM_Init(&htim10) != HAL_OK)
{
Error_Handler();
}
sConfigOC.OCMode = TIM_OCMODE_PWM1;
sConfigOC.Pulse = 0;
sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
if (HAL_TIM_PWM_ConfigChannel(&htim10, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM10_Init 2 */
/* USER CODE END TIM10_Init 2 */
HAL_TIM_MspPostInit(&htim10);
}
/**
* @brief TIM11 Initialization Function
* @param None
* @retval None
*/
static void MX_TIM11_Init(void)
{
/* USER CODE BEGIN TIM11_Init 0 */
/* USER CODE END TIM11_Init 0 */
/* USER CODE BEGIN TIM11_Init 1 */
/* USER CODE END TIM11_Init 1 */
htim11.Instance = TIM11;
htim11.Init.Prescaler = 100-1;
htim11.Init.CounterMode = TIM_COUNTERMODE_UP;
htim11.Init.Period = 100-1;
htim11.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
htim11.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_ENABLE;
if (HAL_TIM_Base_Init(&htim11) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN TIM11_Init 2 */
/* USER CODE END TIM11_Init 2 */
}
/**
* @brief USART1 Initialization Function
* @param None
* @retval None
*/
static void MX_USART1_UART_Init(void)
{
/* USER CODE BEGIN USART1_Init 0 */
/* USER CODE END USART1_Init 0 */
/* USER CODE BEGIN USART1_Init 1 */
/* USER CODE END USART1_Init 1 */
huart1.Instance = USART1;
huart1.Init.BaudRate = 9600;
huart1.Init.WordLength = UART_WORDLENGTH_8B;
huart1.Init.StopBits = UART_STOPBITS_1;
huart1.Init.Parity = UART_PARITY_NONE;
huart1.Init.Mode = UART_MODE_TX_RX;
huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
huart1.Init.OverSampling = UART_OVERSAMPLING_16;
if (HAL_UART_Init(&huart1) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN USART1_Init 2 */
/* USER CODE END USART1_Init 2 */
}
/**
* Enable DMA controller clock
*/
static void MX_DMA_Init(void)
{
/* DMA controller clock enable */
__HAL_RCC_DMA2_CLK_ENABLE();
/* DMA interrupt init */
/* DMA2_Stream0_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA2_Stream0_IRQn, 0, 0);
HAL_NVIC_EnableIRQ(DMA2_Stream0_IRQn);
}
/**
* @brief GPIO Initialization Function
* @param None
* @retval None
*/
static void MX_GPIO_Init(void)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
/* USER CODE BEGIN MX_GPIO_Init_1 */
/* USER CODE END MX_GPIO_Init_1 */
/* GPIO Ports Clock Enable */
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, VALVE_BYPASS_Pin|VALVE_IN_Pin|VALVE_Pin, GPIO_PIN_RESET);
/*Configure GPIO pins : VALVE_BYPASS_Pin VALVE_IN_Pin VALVE_Pin */
GPIO_InitStruct.Pin = VALVE_BYPASS_Pin|VALVE_IN_Pin|VALVE_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
/* USER CODE BEGIN MX_GPIO_Init_2 */
/* USER CODE END MX_GPIO_Init_2 */
}
/* USER CODE BEGIN 4 */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef* htim)
{
if (htim == &htim11) //timer 11 measures 1ms intervals
{
safetyCheck(pressureCmH2O, PEEP, maxPressure);
HAL_I2C_Master_Receive(&hi2c3, ADDR, i2cReceiveBuf, sizeof(i2cReceiveBuf), HAL_MAX_DELAY);
signed int res = (i2cReceiveBuf[0] << 8) | i2cReceiveBuf[1];
flowSLM = (res - OFFSET_FLOW) / SCALE_FLOW;
if(mode == 0)
{
flowMode();
}
else if (mode == 1)
{
pressureMode();
}
transmit[0] = startByte;
transmit[1] = counter;
transmit[2] = flowSLM + 100;
transmit[3] = pressureCmH2O;
HAL_UART_Transmit_IT(&huart1, transmit, sizeof(transmit));
counter = (counter < 99) ? counter + 1 : 0;
}
}
// Pressure sensor callback
void HAL_ADC_ConvCpltCallback(ADC_HandleTypeDef* hadc)
{
adcSumPressure += adc_buf[0];
adcSumPressureIn +=adc_buf[2];
adcSumOxygen += adc_buf[1];
adcCounter++;
adcCounter1000++;
if(adcCounter >= SAMPLES){
adcAveragePressure = adcSumPressure / SAMPLES;
adcVoltagePressure = (adcAveragePressure / 4095) * 3.3;
pressureBar = m * adcVoltagePressure; // bar = m * measured + b m = (bar_max - bar_min)/(V_max-V_min) b = bar_min=m*V_min
pressureCmH2O = pressureBar * 1019.71621 * 0.4;
adcSumPressure = 0;
adcAveragePressureIn = adcSumPressureIn / SAMPLES;
adcVoltagePressureIn = (adcAveragePressureIn / 4095) * 3.3;
pressureBarIn = m * adcVoltagePressureIn; // bar = m * measured + b m = (bar_max - bar_min)/(V_max-V_min) b = bar_min=m*V_min
pressureCmH2OIn = pressureBarIn * 1019.71621 * 0.4;
adcSumPressureIn = 0;
adcCounter = 0;
}
if(adcCounter1000>= SAMPLES1000){
adcAverageOxygen = adcSumOxygen / SAMPLES1000;
adcVoltageOxygen = (adcAverageOxygen / 4095) * 3.3;
OxygenLevel = (float) (adcVoltageOxygen / 3.3) * 100 + 1.5;
adcSumOxygen = 0;
adcCounter1000 = 0;
}
}
void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) {
HAL_UART_Receive_IT(&huart1, receivedBuffer, BUFFER_LENGTH_RX);
uint16_t bufferShift = 0;
uint8_t holder = receivedBuffer[0];
while(holder != 0xFF){
holder = receivedBuffer[++bufferShift];
}
receive[0] = holder;
for(int i = 1; i < 10; i++){
receive[i] = receivedBuffer[++bufferShift];
}
if(receive[0] == startByte && receive[9] == stopByte){
mode = receive[1];
if(mode) maxPressure = (receive[3] << 8) | receive[2];
else maxVolume = (receive[3] << 8) | receive[2];
inhaleTime = (receive[5] << 8) | receive[4];
exhaleTime = (receive[7] << 8) | receive[6];
PEEP = receive[8];
//HAL_UART_Transmit_IT(&huart1, &ackByte, 1);
}
bufferShift = 0;
//Do dopracowania
}
/* USER CODE END 4 */
/**
* @brief This function is executed in case of error occurrence.
* @retval None
*/
void Error_Handler(void)
{
/* USER CODE BEGIN Error_Handler_Debug */
/* User can add his own implementation to report the HAL error return state */
__disable_irq();
while (1)
{
}
/* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
/**
* @brief Reports the name of the source file and the source line number
* where the assert_param error has occurred.
* @param file: pointer to the source file name
* @param line: assert_param error line source number
* @retval None
*/
void assert_failed(uint8_t *file, uint32_t line)
{
/* USER CODE BEGIN 6 */
/* User can add his own implementation to report the file name and line number,
ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
/* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
Editor is loading...
Leave a Comment