/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * <h2><center>&copy; Copyright (c) 2021 STMicroelectronics.
  * All rights reserved.</center></h2>
  *
  * This software component is licensed by ST under BSD 3-Clause license,
  * the "License"; You may not use this file except in compliance with the
  * License. You may obtain a copy of the License at:
  *                        opensource.org/licenses/BSD-3-Clause
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */

/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
/*
 * BASE CLOCK 12MHz
 * Desired interrupt period 60s
 */
const uint16_t tim21_prescaler = 60000-1; // 100Hz
//const uint16_t tim21_period = 12000-1; // 60s
//const uint16_t tim21_period = 1200-1; // 6s
const uint16_t tim21_period = MEASUREMENT_PERIOD_S * (SYSTEM_CLOCK_HZ / tim21_prescaler) - 1;
//const uint16_t tim21_period = 200-1; // 1s

/* Input register memory map
 * (All registers are 16-bit)
 * -------------------------
 *
 * 30010 : CO2  [ppm]        Unsigned value in range [0,40000]
 * 30011 : T    [deg_C * 10] From SHT4x; unsigned value in range [0; 1250]; e.g. 21.5 C => 215
 * 30012 : RH   [%]          From SHT4x; unsigned value in range [0; 100]
 *
 * Backup T and RH sensor:
 * 30013 : T    [deg_C * 10] From SCD4x; unsigned value in range [0; 600]; e.g. 21.5 C => 215
 * 30014 : RH   [%]          From SCD4x; unsigned value in range [0; 100]
 *
 * Extended temperature range (signed values):
 * 30015 : T    [deg_C * 10] From SHT4x; signed value (two's complement) in range [-400;1250]
 * 30016 : T    [deg_C * 10] From SCD4x; signed value (two's complement) in range [-100;600]; e.g. -12.3 C => -123
 *
 */
/* Input registers memory map implementation */
enum
{
	REGISTER_NUM_T = 30010, /* deg C */
	REGISTER_NUM_T_F = 30011, /* deg F */
	REGISTER_NUM_RH = 30012, /* %, from SHT4x */
	REGISTER_NUM_CO2 = 30013, /* ppm */
	REGISTER_NUM_VOC_INDEX = 30014, /* VOC index as calculated by Sensirion library (1 to 500, average 100) */
	REGISTER_NUM_VOC_TICKS = 30015, /* raw VOC ticks */
	REGISTER_NUM_NOX_INDEX = 30016,
	REGISTER_NUM_NOX_TICKS = 30017,
	REGISTER_NUM_PMC_MASS_1_0 = 30018, /* ug / m^3 */
	REGISTER_NUM_PMC_MASS_2_5 = 30019, /* ug / m^3 */
	REGISTER_NUM_PMC_MASS_4_0 = 30020, /* ug / m^3 */
	REGISTER_NUM_PMC_MASS_10_0 = 30021, /* ug / m^3 */
	REGISTER_NUM_PMC_NUMBER_0_5 = 30022, /* 1 / m^3 */
	REGISTER_NUM_PMC_NUMBER_1_0 = 30023, /* 1 / m^3 */
	REGISTER_NUM_PMC_NUMBER_2_5 = 30024, /* 1 / m^3 */
	REGISTER_NUM_PMC_NUMBER_4_0 = 30025, /* 1 / m^3 */
	REGISTER_NUM_PMC_NUMBER_10_0 = 30026, /* 1 / m^3 */
	REGISTER_NUM_TYPICAL_PARTICLE_SIZE = 30027, /* nm */
	REGISTER_NUM_T_SCD4x = 30028, /* deg C */
	REGISTER_NUM_T_SCD4x_F = 30029, /* deg F */
	REGISTER_NUM_RH_SCD4x = 30030, /* % */
	REGISTER_NUM_DEVICE_IDENTIFIER = 30100
	/* VOC Index has initial blackout beriod, when the data is not ready. VOC index is 0 during this period */
} data_registers_numbers;

enum
{
	REGISTER_NUM_LED_ON = 40001,
	REGISTER_NUM_LED_BRIGHTNESS = 40002,
	REGISTER_NUM_LED_SMOOTH = 40003,
	REGISTER_NUM_CO2_ALERT_LIMIT1 = 40004,
	REGISTER_NUM_CO2_ALERT_LIMIT2 = 40005,
	REGISTER_NUM_SCD4x_T_OFFSET = 40006,
	REGISTER_NUM_MODBUS_ADDR = 40007,
	REGISTER_NUM_BAUDRATE = 40008,
	REGISTER_NUM_RESET_DEVICE = 40100
} config_registers_numbers;

enum
{
	REGISTER_NUM_VENDOR_NAME = 30050,
	REGISTER_NUM_PRODUCT_CODE = 30051,
	REGISTER_NUM_REVISION = 30052,
	REGISTER_NUM_PRODUCT_NAME = 30033,
	REGISTER_NUM_SERIAL_NUMBER = 30054,
} identification_registers_numbers;

/*
 * Device type specification: TODO improve and move to external library
 */

#define DEVICE_IDENTIFIER 0xbeef
const uint16_t device_identifier = DEVICE_IDENTIFIER;

/* Variables to store the measured data */
int16_t T_SCD4x, T_SHT4x, TF_SCD4x, TF_SHT4x;
uint16_t CO2, RH_SCD4x, RH_SHT4x;
sps30_data_t PM_SPS30;
/* VOC related varibles */
uint16_t voc_ticks_compensated;
uint16_t voc_ticks;
uint32_t voc_index;

/* Struct to store the sensor config */
config_t sensor_config;
/* Device ID struct */
modbus_device_id_t device_id;
uint8_t sensor_config_pending_write = 0;
uint8_t baudrate_changed = 0;
uint8_t modbus_address_changed = 0;
uint8_t co2_valid = 0;
/* dynamic sensor configuration */
uint8_t scd4x_is_connected = 0;
uint8_t sps30_is_connected = 0;
uint8_t sgp40_is_connected = 0;
/* Sensirion library for VOC */
GasIndexAlgorithmParams voc_params;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM21_Init(void);
static void MX_TIM2_Init(void);
static void MX_TIM22_Init(void);
static void MX_IWDG_Init(void);
/* USER CODE BEGIN PFP */
void USART2_TX_Buffer(uint8_t* buffer_tx, uint16_t buffer_tx_len);
void CO2_to_color(uint16_t co2, uint16_t co2_limit_yellow, uint16_t co2_limit_red);
/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/*
 * Functions for SHT4x lib
 */


int8_t sht4x_i2c_transmit(uint8_t address, uint8_t *buffer, int len)
{
	i2c_transmit(address, buffer, len);
}

int8_t sht4x_i2c_receive(uint8_t address, uint8_t *buffer, int len)
{
	i2c_receive(address, buffer, len);
}

uint8_t sensirion_crc8_calculate(const uint8_t *data, uint16_t count)
{
	crc8_calculate(data, count);
}

int8_t sht4x_disable_interrupts(void)
{
	uart_disable_interrupts();
}

int8_t sht4x_enable_interrupts(void)
{
	uart_enable_interrupts();
}

int8_t sgp40_i2c_transmit(uint8_t address, uint8_t *buffer, int len)
{
	i2c_transmit(address, buffer, len);
}

int8_t sgp40_i2c_receive(uint8_t address, uint8_t *buffer, int len)
{
	i2c_receive(address, buffer, len);
}

int8_t sgp40_disable_interrupts(void)
{
	uart_disable_interrupts();
}

int8_t sgp40_enable_interrupts(void)
{
	uart_enable_interrupts();
}

int8_t scd4x_i2c_transmit(uint8_t address, uint8_t *buffer, int len)
{
	i2c_transmit(address, buffer, len);
}

int8_t scd4x_i2c_receive(uint8_t address, uint8_t *buffer, int len)
{
	i2c_receive(address, buffer, len);
}

int8_t scd4x_disable_interrupts(void)
{
	uart_disable_interrupts();
}

int8_t scd4x_enable_interrupts(void)
{
	uart_enable_interrupts();
}

void delay_ms(int delay_ms)
{
	LL_mDelay(delay_ms);
}
/* 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. */

  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_SYSCFG);
  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_PWR);

  /* System interrupt init*/
  /* SysTick_IRQn interrupt configuration */
  NVIC_SetPriority(SysTick_IRQn, 3);

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */
  /* Read config from EEPROM - if unsuccessful, set the default values*/
  int8_t config_read_status = config_read(&sensor_config);
  if (config_read_status != CONFIG_OK)
  {
	  sensor_config.modbus_addr = MODBUS_DEFAULT_SLAVE_ADDRESS;
	  sensor_config.led_co2_alert_limit1 = CONFIG_DEFAULT_LED_ALERT1_LIMIT;
	  sensor_config.led_co2_alert_limit2 = CONFIG_DEFAULT_LED_ALERT2_LIMIT;
	  sensor_config.led_on = CONFIG_DEFAULT_LED_ON;
	  sensor_config.led_brightness = CONFIG_DEFAULT_LED_BRIGHTNESS;
	  sensor_config.led_smooth = CONFIG_DEFAULT_LED_SMOOTH;
	  sensor_config.scd4x_t_offset = CONFIG_DEFAULT_SCD4x_T_OFFSET;
	  sensor_config.baudrate_index = CONFIG_DEFAULT_BAUDRATE_INDEX;
  }
  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_USART2_UART_Init();
  MX_TIM21_Init();
  MX_TIM2_Init();
  MX_TIM22_Init();
//  MX_IWDG_Init();
  /* USER CODE BEGIN 2 */

  /* Turn on MAGENTA LED to signal startup state */
//  LL_GPIO_ResetOutputPin(LED_R_GPIO_Port, LED_R_Pin);
//  LL_GPIO_ResetOutputPin(LED_G_GPIO_Port, LED_G_Pin);
//  LL_GPIO_ResetOutputPin(LED_B_GPIO_Port, LED_B_Pin);

  /* Enable I2C for sensors */
  LL_I2C_Enable(I2C1);


  /* Set the modbus address */
  modbus_slave_set_address(sensor_config.modbus_addr);

  /* Enable UART for RS485 */
  LL_USART_Enable(USART2);

  /* Start the timer for measurement triggering */
  LL_TIM_EnableCounter(TIM21);
  LL_TIM_EnableIT_UPDATE(TIM21);

  /* Enable PWM */
  LL_TIM_CC_EnableChannel(TIM22, LL_TIM_CHANNEL_CH1);
  LL_TIM_CC_EnableChannel(TIM22, LL_TIM_CHANNEL_CH2);
  LL_TIM_CC_EnableChannel(TIM2, LL_TIM_CHANNEL_CH1);
  LL_TIM_EnableCounter(TIM22);
  LL_TIM_EnableCounter(TIM2);

  /* I2C context init (for SHT4x and SCD4x) */
  i2c_context_t i2c_context;
  i2c_context.i2c = I2C1;
  i2c_init(&i2c_context);

  /* RGB LED context init */
  rgbled_context_t rgbled_context;
  rgbled_context.timer.R = TIM22;
  rgbled_context.timer.G = TIM22;
  rgbled_context.timer.B = TIM2;
  rgbled_context.channel.R = 2;
  rgbled_context.channel.G = 1;
  rgbled_context.channel.B = 1;
  rgbled_init(&rgbled_context);
  rgbled_set_intensity(sensor_config.led_brightness);
  rgbled_set_color(RGBLED_WHITE); /* white color indicates init process */

  /* Fill device ID struct */
  device_id.object_name.VendorName         = "Veles Labs";
  device_id.object_name.ProductCode        = "124C41";
  device_id.object_name.MajorMinorRevision = "1.2";
  device_id.object_name.VendorUrl          = "https://veles-labs.com";
  device_id.object_name.ProductName        = "SensCO2";
  device_id.object_name.ModelName          = "Hugo";
  modbus_slave_init_device_id(&device_id);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */

  /* Wait 1000ms for sensors initialization */
  /* SHT4x Init Time: max 1 	ms 	(datasheet pg. 8) */
  /* SCD4x Init Time: max 1000 	ms 	(datasheet pg. 6) */
  /* SPS30 Init Time: max 30000 ms 	(datasheet pg. 2) */
  LL_mDelay(5000);

  if (scd4x_start_periodic_measurement() == SCD4X_OK)
  {
	  scd4x_is_connected = 1;
  }

  /* Attempt to start SPS30 measurement and check if it's connected */
  sps30_reset();
  LL_mDelay(500); // should be less than 100 ms (we have 500 just to be sure)
  if (sps30_start_measurement() == SPS30_OK)
  {
	  sps30_is_connected = 1;
  }

  if (sgp40_measure_raw_signal(&voc_ticks) == SGP40_OK)
  {
	  sgp40_is_connected = 1;
	  GasIndexAlgorithm_init(&voc_params, GasIndexAlgorithm_ALGORITHM_TYPE_VOC);
  }

  LL_mDelay(5000);

  static uint32_t new_baud;
  /* Enter the main loop */
  while (1)
  {
	  /* Watchdog is set to 28 s (37 kHz, prescaler 256, 4095 reload value) */
	  LL_IWDG_ReloadCounter(IWDG);
	  if (usart2_rx_done == 1)
	  {
	  /* Process the message */
		  if (usart2_rx_message_too_long)
		  {
			  /* Do nothing, just delete the buffer and set the flag back to zero*/
			  usart2_rx_message_too_long = 0;
		  } else
		  {
			  /* Process the message:
			   * message is stored in modbus_buffer[], no copying necessary;
			   * but we need to make sure that modbus_buffer[] will not be used while
			   * processing the message: this can be done by disabling RX interrupt */
			  LL_USART_DisableIT_RXNE(USART2);
			  modbus_slave_process_msg(modbus_buffer, usart2_rx_message_len);
			  /* Reset the RX DONE flag */
			  usart2_rx_done = 0;
			  LL_USART_EnableIT_RXNE(USART2);
		  }
		  /* Reset the RX DONE flag */
		  usart2_rx_done = 0;
	  }
	  /* if config changed (MODBUS write), reflect changes to EEPROM */
	  if (sensor_config_pending_write) {
		  config_write(&sensor_config);
		  sensor_config_pending_write = 0;
	  }
	  if (modbus_address_changed)
	  {
		  modbus_slave_set_address(sensor_config.modbus_addr);
		  modbus_address_changed = 0;
	  }
	  if (baudrate_changed)
	  {
		  while (!LL_USART_IsActiveFlag_TXE(USART2));
		  uart_disable_interrupts();

		  LL_USART_SetBaudRate(USART2, SYSTICK_FREQ_HZ, LL_USART_OVERSAMPLING_16, config_baudrates[sensor_config.baudrate_index]);
		  uart_enable_interrupts();
		  LL_USART_EnableDirectionRx(USART2);
		  LL_USART_EnableDirectionTx(USART2);
		  baudrate_changed = 0;

		  new_baud = LL_USART_GetBaudRate(USART2, SYSTICK_FREQ_HZ, LL_USART_OVERSAMPLING_16);
	  }
	  /* It is time for measurement */
	  if (tim21_elapsed_period == 1)
	  {
		  /* TODO: Check the data */
		  /* Read SHT4x data (always connected) */
		  sht4x_measure(&T_SHT4x, &RH_SHT4x);
		  TF_SHT4x = CELSIUS_TO_FAHRENHEIT(T_SHT4x);
		  /* Read SCD4x data (if connected) */
		  if (scd4x_is_connected == 1)
		  {
			  if (scd4x_read_measurement(&CO2, &T_SCD4x, &RH_SCD4x) != SCD4X_OK) {
				  /* read failed, either I2C fail or CRC error */
				  // TODO something
			  }
			  TF_SCD4x = CELSIUS_TO_FAHRENHEIT(T_SCD4x);
			  if (CO2 > 0) {
				  co2_valid = 1;
			  } else {
				  co2_valid = 0;
			  }
		  }

		  /* Read SPS30 data (if connected) */
		  if (sps30_is_connected == 1)
		  {
			  sps30_read_measured_values(&PM_SPS30);
		  }
		  /* TODO: Process data and light a desired color of LED */
		  /* TODO: Add hystheresis */

		  /* Read SGP40 (if connected) */
		  if (sgp40_is_connected == 1)
		  {
			  //sgp40_measure_raw_signal(&voc_ticks);
			  sgp40_measure_raw_signal_compensated(&voc_ticks_compensated, RH_SHT4x, T_SHT4x);
//			  sgp40_measure_raw_signal(&voc_ticks);
			  /* Sensirion VOC library */
			  GasIndexAlgorithm_process(&voc_params, voc_ticks_compensated, (int32_t*)&voc_index);

		  }

		  /* Reset the TIM21 Elapsed Period Flag */
		  tim21_elapsed_period = 0;
	  }
	  /* TEST END */
	  if (sensor_config.led_on == 1) {
		  if (co2_valid == 1) {
			  if (sensor_config.led_smooth == 0) {
				  if (CO2 <= sensor_config.led_co2_alert_limit1) {
					  /* CO2 is OK -> GREEN */
					  rgbled_set_color(RGBLED_GREEN);

				  } else if ((sensor_config.led_co2_alert_limit1 < CO2) && (CO2 <= sensor_config.led_co2_alert_limit2)) {
					  /* CO2 is NOT OK -> YELLOW */
					  rgbled_set_color(RGBLED_YELLOW);


				  } else if (sensor_config.led_co2_alert_limit2 < CO2) {
					  /* CO2 is CRITICAL -> RED */
					  rgbled_set_color(RGBLED_RED);
				  }
			  } else {
				  /* interpolate color according to CO2 level */
				  CO2_to_color(CO2, sensor_config.led_co2_alert_limit1, sensor_config.led_co2_alert_limit2);
			  }
		  } else {
			  /* CO2 invalid */
			  rgbled_set_color(RGBLED_MAGENTA);
		  }

	  } else {
		  /* LED disabled by user */
		  rgbled_set_color(RGBLED_OFF);
	  }
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  LL_FLASH_SetLatency(LL_FLASH_LATENCY_0);
  while(LL_FLASH_GetLatency()!= LL_FLASH_LATENCY_0)
  {
  }
  LL_PWR_SetRegulVoltageScaling(LL_PWR_REGU_VOLTAGE_SCALE1);
  LL_RCC_HSI_Enable();

   /* Wait till HSI is ready */
  while(LL_RCC_HSI_IsReady() != 1)
  {

  }
  LL_RCC_HSI_SetCalibTrimming(16);
  LL_RCC_LSI_Enable();

   /* Wait till LSI is ready */
  while(LL_RCC_LSI_IsReady() != 1)
  {

  }
  LL_RCC_PLL_ConfigDomain_SYS(LL_RCC_PLLSOURCE_HSI, LL_RCC_PLL_MUL_3, LL_RCC_PLL_DIV_4);
  LL_RCC_PLL_Enable();

   /* Wait till PLL is ready */
  while(LL_RCC_PLL_IsReady() != 1)
  {

  }
  LL_RCC_SetAHBPrescaler(LL_RCC_SYSCLK_DIV_1);
  LL_RCC_SetAPB1Prescaler(LL_RCC_APB1_DIV_1);
  LL_RCC_SetAPB2Prescaler(LL_RCC_APB2_DIV_1);
  LL_RCC_SetSysClkSource(LL_RCC_SYS_CLKSOURCE_PLL);

   /* Wait till System clock is ready */
  while(LL_RCC_GetSysClkSource() != LL_RCC_SYS_CLKSOURCE_STATUS_PLL)
  {

  }

  LL_Init1msTick(SYSTEM_CLOCK_HZ);
  LL_SetSystemCoreClock(SYSTEM_CLOCK_HZ);
  LL_RCC_SetUSARTClockSource(LL_RCC_USART2_CLKSOURCE_PCLK1);
  LL_RCC_SetI2CClockSource(LL_RCC_I2C1_CLKSOURCE_PCLK1);
}

/**
  * @brief I2C1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  LL_I2C_InitTypeDef I2C_InitStruct = {0};

  LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

  LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA);
  /**I2C1 GPIO Configuration
  PA9   ------> I2C1_SCL
  PA10   ------> I2C1_SDA
  */
  GPIO_InitStruct.Pin = LL_GPIO_PIN_9;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_1;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = LL_GPIO_PIN_10;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_OPENDRAIN;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_UP;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_1;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /* Peripheral clock enable */
  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_I2C1);

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  /** I2C Initialization
  */
  LL_I2C_EnableAutoEndMode(I2C1);
  LL_I2C_DisableOwnAddress2(I2C1);
  LL_I2C_DisableGeneralCall(I2C1);
  LL_I2C_EnableClockStretching(I2C1);
  I2C_InitStruct.PeripheralMode = LL_I2C_MODE_I2C;
  I2C_InitStruct.Timing = 0x40000A0B;
  I2C_InitStruct.AnalogFilter = LL_I2C_ANALOGFILTER_ENABLE;
  I2C_InitStruct.DigitalFilter = 0;
  I2C_InitStruct.OwnAddress1 = 0;
  I2C_InitStruct.TypeAcknowledge = LL_I2C_ACK;
  I2C_InitStruct.OwnAddrSize = LL_I2C_OWNADDRESS1_7BIT;
  LL_I2C_Init(I2C1, &I2C_InitStruct);
  LL_I2C_SetOwnAddress2(I2C1, 0, LL_I2C_OWNADDRESS2_NOMASK);
  /* USER CODE BEGIN I2C1_Init 2 */
  /* USER CODE END I2C1_Init 2 */

}

/**
  * @brief IWDG Initialization Function
  * @param None
  * @retval None
  */
static void MX_IWDG_Init(void)
{

  /* USER CODE BEGIN IWDG_Init 0 */

  /* USER CODE END IWDG_Init 0 */

  /* USER CODE BEGIN IWDG_Init 1 */

  /* USER CODE END IWDG_Init 1 */
  LL_IWDG_Enable(IWDG);
  LL_IWDG_EnableWriteAccess(IWDG);
  LL_IWDG_SetPrescaler(IWDG, LL_IWDG_PRESCALER_256);
  LL_IWDG_SetReloadCounter(IWDG, 4095);
  while (LL_IWDG_IsReady(IWDG) != 1)
  {
  }

  LL_IWDG_ReloadCounter(IWDG);
  /* USER CODE BEGIN IWDG_Init 2 */

  /* USER CODE END IWDG_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 */

  LL_TIM_InitTypeDef TIM_InitStruct = {0};
  LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};

  LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* Peripheral clock enable */
  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_TIM2);

  /* USER CODE BEGIN TIM2_Init 1 */

  /* USER CODE END TIM2_Init 1 */
  TIM_InitStruct.Prescaler = RGBLED_PRESCALER;
  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
  TIM_InitStruct.Autoreload = RGBLED_PERIOD;
  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
  LL_TIM_Init(TIM2, &TIM_InitStruct);
  LL_TIM_DisableARRPreload(TIM2);
  LL_TIM_OC_EnablePreload(TIM2, LL_TIM_CHANNEL_CH1);
  TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1;
  TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
  TIM_OC_InitStruct.CompareValue = 0;
  TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
  LL_TIM_OC_Init(TIM2, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);
  LL_TIM_OC_DisableFast(TIM2, LL_TIM_CHANNEL_CH1);
  LL_TIM_SetTriggerOutput(TIM2, LL_TIM_TRGO_RESET);
  LL_TIM_DisableMasterSlaveMode(TIM2);
  /* USER CODE BEGIN TIM2_Init 2 */

  /* USER CODE END TIM2_Init 2 */
  LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA);
  /**TIM2 GPIO Configuration
  PA5   ------> TIM2_CH1
  */
  GPIO_InitStruct.Pin = LL_GPIO_PIN_5;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_5;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

}

/**
  * @brief TIM21 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM21_Init(void)
{

  /* USER CODE BEGIN TIM21_Init 0 */

  /* USER CODE END TIM21_Init 0 */

  LL_TIM_InitTypeDef TIM_InitStruct = {0};

  /* Peripheral clock enable */
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM21);

  /* TIM21 interrupt Init */
  NVIC_SetPriority(TIM21_IRQn, 0);
  NVIC_EnableIRQ(TIM21_IRQn);

  /* USER CODE BEGIN TIM21_Init 1 */

  /* USER CODE END TIM21_Init 1 */
  TIM_InitStruct.Prescaler = tim21_prescaler;
  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
  TIM_InitStruct.Autoreload = tim21_period;
  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
  LL_TIM_Init(TIM21, &TIM_InitStruct);
  LL_TIM_EnableARRPreload(TIM21);
  LL_TIM_SetClockSource(TIM21, LL_TIM_CLOCKSOURCE_INTERNAL);
  LL_TIM_SetTriggerOutput(TIM21, LL_TIM_TRGO_RESET);
  LL_TIM_DisableMasterSlaveMode(TIM21);
  /* USER CODE BEGIN TIM21_Init 2 */

  /* USER CODE END TIM21_Init 2 */

}

/**
  * @brief TIM22 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM22_Init(void)
{

  /* USER CODE BEGIN TIM22_Init 0 */

  /* USER CODE END TIM22_Init 0 */

  LL_TIM_InitTypeDef TIM_InitStruct = {0};
  LL_TIM_OC_InitTypeDef TIM_OC_InitStruct = {0};

  LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* Peripheral clock enable */
  LL_APB2_GRP1_EnableClock(LL_APB2_GRP1_PERIPH_TIM22);

  /* USER CODE BEGIN TIM22_Init 1 */

  /* USER CODE END TIM22_Init 1 */
  TIM_InitStruct.Prescaler = RGBLED_PRESCALER;
  TIM_InitStruct.CounterMode = LL_TIM_COUNTERMODE_UP;
  TIM_InitStruct.Autoreload = RGBLED_PERIOD;
  TIM_InitStruct.ClockDivision = LL_TIM_CLOCKDIVISION_DIV1;
  LL_TIM_Init(TIM22, &TIM_InitStruct);
  LL_TIM_DisableARRPreload(TIM22);
  LL_TIM_OC_EnablePreload(TIM22, LL_TIM_CHANNEL_CH1);
  TIM_OC_InitStruct.OCMode = LL_TIM_OCMODE_PWM1;
  TIM_OC_InitStruct.OCState = LL_TIM_OCSTATE_DISABLE;
  TIM_OC_InitStruct.CompareValue = 0;
  TIM_OC_InitStruct.OCPolarity = LL_TIM_OCPOLARITY_HIGH;
  LL_TIM_OC_Init(TIM22, LL_TIM_CHANNEL_CH1, &TIM_OC_InitStruct);
  LL_TIM_OC_DisableFast(TIM22, LL_TIM_CHANNEL_CH1);
  LL_TIM_OC_EnablePreload(TIM22, LL_TIM_CHANNEL_CH2);
  LL_TIM_OC_Init(TIM22, LL_TIM_CHANNEL_CH2, &TIM_OC_InitStruct);
  LL_TIM_OC_DisableFast(TIM22, LL_TIM_CHANNEL_CH2);
  LL_TIM_SetTriggerOutput(TIM22, LL_TIM_TRGO_RESET);
  LL_TIM_DisableMasterSlaveMode(TIM22);
  /* USER CODE BEGIN TIM22_Init 2 */

  /* USER CODE END TIM22_Init 2 */
  LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA);
  /**TIM22 GPIO Configuration
  PA6   ------> TIM22_CH1
  PA7   ------> TIM22_CH2
  */
  GPIO_InitStruct.Pin = LL_GPIO_PIN_6;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_5;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = LL_GPIO_PIN_7;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_LOW;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_5;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  LL_USART_InitTypeDef USART_InitStruct = {0};

  LL_GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* Peripheral clock enable */
  LL_APB1_GRP1_EnableClock(LL_APB1_GRP1_PERIPH_USART2);

  LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA);
  /**USART2 GPIO Configuration
  PA1   ------> USART2_DE
  PA2   ------> USART2_TX
  PA3   ------> USART2_RX
  */
  GPIO_InitStruct.Pin = LL_GPIO_PIN_1;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_4;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = LL_GPIO_PIN_2;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_4;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  GPIO_InitStruct.Pin = LL_GPIO_PIN_3;
  GPIO_InitStruct.Mode = LL_GPIO_MODE_ALTERNATE;
  GPIO_InitStruct.Speed = LL_GPIO_SPEED_FREQ_VERY_HIGH;
  GPIO_InitStruct.OutputType = LL_GPIO_OUTPUT_PUSHPULL;
  GPIO_InitStruct.Pull = LL_GPIO_PULL_NO;
  GPIO_InitStruct.Alternate = LL_GPIO_AF_4;
  LL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /* USART2 interrupt Init */
  NVIC_SetPriority(USART2_IRQn, 0);
  NVIC_EnableIRQ(USART2_IRQn);

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  USART_InitStruct.BaudRate = config_baudrates[sensor_config.baudrate_index];
  USART_InitStruct.DataWidth = LL_USART_DATAWIDTH_9B;
  USART_InitStruct.StopBits = LL_USART_STOPBITS_1;
  USART_InitStruct.Parity = LL_USART_PARITY_EVEN;
  USART_InitStruct.TransferDirection = LL_USART_DIRECTION_TX_RX;
  USART_InitStruct.HardwareFlowControl = LL_USART_HWCONTROL_NONE;
  USART_InitStruct.OverSampling = LL_USART_OVERSAMPLING_16;
  LL_USART_Init(USART2, &USART_InitStruct);
  LL_USART_EnableDEMode(USART2);
  LL_USART_SetDESignalPolarity(USART2, LL_USART_DE_POLARITY_HIGH);
  LL_USART_SetDEAssertionTime(USART2, 0);
  LL_USART_SetDEDeassertionTime(USART2, 0);
  LL_USART_ConfigAsyncMode(USART2);
  LL_USART_Enable(USART2);
  /* USER CODE BEGIN USART2_Init 2 */

  /* Enable IDLE Interrupt */
  LL_USART_EnableIT_IDLE(USART2);

  /* Enable RX Not Empty Interrupt */
  LL_USART_EnableIT_RXNE(USART2);

  LL_USART_EnableDirectionRx(USART2);
  LL_USART_EnableDirectionTx(USART2);
  LL_USART_Enable(USART2);
  /* USER CODE END USART2_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{

  /* GPIO Ports Clock Enable */
  LL_IOP_GRP1_EnableClock(LL_IOP_GRP1_PERIPH_GPIOA);

}

/* USER CODE BEGIN 4 */
void USART2_TX_Buffer(uint8_t* buffer_tx, uint16_t buffer_tx_len)
{
	__disable_irq();
	for (uint16_t i = 0; i < buffer_tx_len; i++)
	{
		  LL_USART_TransmitData9(USART2, buffer_tx[i]);
		  while (!LL_USART_IsActiveFlag_TXE(USART2));
	}
	__enable_irq();
}

int8_t uart_disable_interrupts(void)
{
	LL_USART_Disable(USART2);
	LL_USART_DisableIT_IDLE(USART2);
	LL_USART_EnableIT_RXNE(USART2);

	return 0;
}

int8_t uart_enable_interrupts(void)
{
	LL_USART_Enable(USART2);
	LL_USART_EnableIT_IDLE(USART2);
	LL_USART_EnableIT_RXNE(USART2);

	return 0;
}

int8_t modbus_slave_callback(modbus_transaction_t *transaction)
{
	uint16_t register_number = transaction->register_number;
	switch (transaction->function_code)
	{
		case MODBUS_READ_INPUT_REGISTERS:
			for (int i = 0; i < transaction->register_count; i++, register_number++)
			{
				switch (register_number)
				{
					case REGISTER_NUM_T:
						transaction->input_registers_signed[i] = (int16_t)T_SHT4x;
						break;
					case REGISTER_NUM_T_F:
						transaction->input_registers_signed[i] = (int16_t)TF_SHT4x;
						break;
					case REGISTER_NUM_RH:
						transaction->input_registers[i] = (uint16_t)RH_SHT4x;
						break;
					case REGISTER_NUM_CO2:
						transaction->input_registers[i] = (uint16_t)CO2;
						break;
					case REGISTER_NUM_VOC_INDEX:
						MODBUS_ASSERT(sgp40_is_connected);
						transaction->input_registers[i] = (uint16_t)voc_index;
						break;
					case REGISTER_NUM_VOC_TICKS:
						MODBUS_ASSERT(sgp40_is_connected);
						transaction->input_registers[i] = (uint16_t)voc_ticks_compensated;
						break;
					case REGISTER_NUM_PMC_MASS_1_0:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.mass_concentration[PM1_0];
						break;
					case REGISTER_NUM_PMC_MASS_2_5:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.mass_concentration[PM2_5];
						break;
					case REGISTER_NUM_PMC_MASS_4_0:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.mass_concentration[PM4_0];
						break;
					case REGISTER_NUM_PMC_MASS_10_0:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.mass_concentration[PM10_0];
						break;
					case REGISTER_NUM_PMC_NUMBER_0_5:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.number_concentration[PM0_5];
						break;
					case REGISTER_NUM_PMC_NUMBER_1_0:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.number_concentration[PM1_0];
						break;
					case REGISTER_NUM_PMC_NUMBER_2_5:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.number_concentration[PM2_5];
						break;
					case REGISTER_NUM_PMC_NUMBER_4_0:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.number_concentration[PM4_0];
						break;
					case REGISTER_NUM_PMC_NUMBER_10_0:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.number_concentration[PM10_0];
						break;
					case REGISTER_NUM_TYPICAL_PARTICLE_SIZE:
						MODBUS_ASSERT(sps30_is_connected);
						transaction->input_registers[i] = (uint16_t)PM_SPS30.typical_particle_size;
						break;
					case REGISTER_NUM_T_SCD4x:
						transaction->input_registers_signed[i] = (int16_t)T_SCD4x;
						break;
					case REGISTER_NUM_T_SCD4x_F:
						transaction->input_registers_signed[i] = (int16_t)TF_SCD4x;
						break;
					case REGISTER_NUM_RH_SCD4x:
						transaction->input_registers[i] = (uint16_t)RH_SCD4x;
						break;
					case REGISTER_NUM_DEVICE_IDENTIFIER:
						transaction->input_registers[i] = (uint16_t)device_identifier;
					default:
						return MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED;
				}
			}
			return MODBUS_OK;
		case MODBUS_READ_HOLDING_REGISTERS:
			for (int i = 0; i < transaction->register_count; i++, register_number++)
			{
				switch (register_number)
				{
					case REGISTER_NUM_LED_ON:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.led_on);
						break;
					case REGISTER_NUM_LED_BRIGHTNESS:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.led_brightness);
						break;
					case REGISTER_NUM_LED_SMOOTH:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.led_smooth);
						break;
					case REGISTER_NUM_CO2_ALERT_LIMIT1:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.led_co2_alert_limit1);
						break;
					case REGISTER_NUM_CO2_ALERT_LIMIT2:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.led_co2_alert_limit2);
						break;
					case REGISTER_NUM_SCD4x_T_OFFSET:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.scd4x_t_offset);
						break;
					case REGISTER_NUM_MODBUS_ADDR:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.modbus_addr);
						break;
					case REGISTER_NUM_BAUDRATE:
						transaction->holding_registers[i] = (uint16_t)(sensor_config.baudrate_index);
						break;
					default:
						return MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED;
				}
			}
			return MODBUS_OK;
		case MODBUS_WRITE_SINGLE_REGISTER:
			transaction->register_count = 1;
		case MODBUS_WRITE_MULTIPLE_REGISTERS:
			for (int i = 0; i < transaction->register_count; i++, register_number++)
			{
				switch (register_number)
				{
					case REGISTER_NUM_LED_ON:
						/* allowed values: 0 (off), 1 (on) */
						if (transaction->holding_registers[i] > 1) {
							return MODBUS_ERROR_OUT_OF_BOUNDS; // todo is this the right error?
						}
						sensor_config.led_on = (uint8_t) transaction->holding_registers[i];
						break;
					case REGISTER_NUM_LED_BRIGHTNESS:
						/* allowed values: [0, 100] (from 0 to 100, where 0 is off and 100 is full power) */
						if (transaction->holding_registers[i] > 100) {
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						sensor_config.led_brightness = (uint16_t) transaction->holding_registers[i];
						rgbled_set_intensity(sensor_config.led_brightness);
						break;
					case REGISTER_NUM_LED_SMOOTH:
						/* allowed values: 0 (off), 1 (on) */
						if (transaction->holding_registers[i] > 1) {
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						sensor_config.led_smooth = (uint16_t) transaction->holding_registers[i];
						break;
					case REGISTER_NUM_CO2_ALERT_LIMIT1:
						/* allowed values: [0, 65536] ppm (from 0 to uint16_t maximum value)
						 * Note that alert limit 1 must ALWAYS be lower than alert limit 2 */
						if (transaction->holding_registers[i] >= sensor_config.led_co2_alert_limit2) {
							/* trying to set alert limit 1 higher than alert limit 2, return error */
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						sensor_config.led_co2_alert_limit1 = (uint16_t) transaction->holding_registers[i];
						break;
					case REGISTER_NUM_CO2_ALERT_LIMIT2:
						/* allowed values: [0, 65536] ppm (from 0 to uint16_t maximum value)
						 * Note that alert limit 1 must ALWAYS be lower than alert limit 2 */
						if (transaction->holding_registers[i] <= sensor_config.led_co2_alert_limit1) {
							/* trying to set alert limit 1 higher than alert limit 2, return error */
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						sensor_config.led_co2_alert_limit2 = (uint16_t) transaction->holding_registers[i];
						break;
					case REGISTER_NUM_SCD4x_T_OFFSET:
						/* allowed values: [0, 65536]
						 * Received value is interpreted as 16-bit signed (two's complement) number,
						 * multiplied by 10. Examples:
						 * +---------------+------------+----------------+
						 * | Offset [deg C] | 16-bit hex | decimal signed |
						 * +---------------+------------+----------------+
						 * |     -1.5      |   0xfff1    |      -15       |
						 * +---------------+------------+----------------+
						 * |      0.9      |   0x0009   |        9       |
						 * +---------------+------------+----------------+
						 */
						sensor_config.scd4x_t_offset = (int16_t) transaction->holding_registers[i];
						break;
					case REGISTER_NUM_MODBUS_ADDR:
						/* allowed values: [1, 247] (from 1 to 247) as given in section 2.1 of document
						 * MODBUS over Serial Line: Specification and Implementation Guide (V1.02) */
						if (transaction->holding_registers[i] < 1 || transaction->holding_registers[i] > 247) {
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						sensor_config.modbus_addr = (uint16_t) transaction->holding_registers[i];
						modbus_address_changed = 1;
						break;
					case REGISTER_NUM_BAUDRATE:
						/* allowed values: [0, 9] (from 0 to 9, where 9 == config_baudrates_length) */
						if (transaction->holding_registers[i] >= config_baudrates_length) {
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						sensor_config.baudrate_index = (uint16_t) (transaction->holding_registers[i]);
						baudrate_changed = 1;
						break;
					case REGISTER_NUM_RESET_DEVICE:
						if (transaction->holding_registers[i] == 0xABCD) {
							/* software reset */
							NVIC_SystemReset();
						} else {
							return MODBUS_ERROR_OUT_OF_BOUNDS;
						}
						break;
					default:
						return MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED;
				}
			}
			sensor_config_pending_write = 1;
			return MODBUS_OK;
		default:
			return MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED;
	}
	/* Catch-all error */
	return MODBUS_ERROR_FUNCTION_NOT_IMPLEMENTED; /* nothing implemented yet! TODO */
}

int8_t modbus_transmit_function(uint8_t *buffer, uint16_t data_len)
{
	 /* TODO */
	USART2_TX_Buffer(buffer, data_len);
	return MODBUS_OK;
}

void CO2_to_color(uint16_t co2, uint16_t co2_limit_yellow, uint16_t co2_limit_red)
{
	const uint16_t co2_limit_green = 400; /* fresh air, lowest possible value */

	uint32_t co2_interval_length;
	uint32_t co2_lower_limit;
	uint32_t intensity;
	uint32_t intensity_offset;

	if (co2 < co2_limit_green) {
		rgbled_set_color(RGBLED_GREEN);
	} else if (co2 > co2_limit_red) {
		rgbled_set_color(RGBLED_RED);
	} else {
		if (co2 < co2_limit_yellow) {
			/* first interval: from green to yellow (co2_alert_limit1) */
			co2_interval_length = co2_limit_yellow - co2_limit_green;
			co2_lower_limit = co2_limit_green;
			intensity_offset = 0;
		} else {
			/* second interval: from yellow to red (co2_alert_limit2) */
			co2_interval_length = co2_limit_red - co2_limit_yellow;
			co2_lower_limit = co2_limit_yellow;
			intensity_offset = 127;
		}
		intensity = (co2 - co2_lower_limit) * 128 / co2_interval_length + intensity_offset;
		rgbled_set_color(intensity, 255 - intensity, 0);
	}
}
/* 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 */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/
