Bladeren bron

Branch for 200W PSU. Need to calculate mul/div.

Vladimir N. Shilov 1 jaar geleden
bovenliggende
commit
b76fa6adc9
13 gewijzigde bestanden met toevoegingen van 377 en 746 verwijderingen
  1. BIN
      .vscode/BROWSE.VC.DB
  2. 5 0
      .vscode/settings.json
  3. 8 8
      Makefile
  4. 1 46
      inc/board.h
  5. 6 0
      inc/delay.h
  6. 0 179
      lib/i2c.c
  7. 0 33
      lib/i2c.h
  8. 0 206
      lib/ina219.c
  9. 0 146
      lib/ina219.h
  10. 132 0
      lib/led.c
  11. 83 0
      lib/led.h
  12. 105 120
      src/main.c
  13. 37 8
      src/stm8s_it.c

BIN
.vscode/BROWSE.VC.DB


+ 5 - 0
.vscode/settings.json

@@ -0,0 +1,5 @@
+{
+  "makefile.extensionOutputFolder": "./.vscode",
+  "C_Cpp.errorSquiggles": "disabled",
+  "commentTranslate.multiLineMerge": true
+}

+ 8 - 8
Makefile

@@ -51,21 +51,21 @@ APP_OBJECTS = $(notdir $(patsubst %.c,%.o,$(wildcard $(APP_SRC)/*.c)))
 LIB_OBJECTS = $(notdir $(patsubst %.c,%.o,$(wildcard $(LIB_SRC)/*.c)))
 
 # STM8S Peripheral driver object files
-PERIPH_OBJECTS = stm8s_adc1.o
+#PERIPH_OBJECTS = stm8s_adc1.o
 #PERIPH_OBJECTS += stm8s_awu.o
 #PERIPH_OBJECTS += stm8s_beep.o
-PERIPH_OBJECTS += stm8s_clk.o
-PERIPH_OBJECTS += stm8s_exti.o
+#PERIPH_OBJECTS += stm8s_clk.o
+#PERIPH_OBJECTS += stm8s_exti.o
 #PERIPH_OBJECTS += stm8s_flash.o
-PERIPH_OBJECTS += stm8s_gpio.o
-PERIPH_OBJECTS += stm8s_i2c.o
-PERIPH_OBJECTS += stm8s_itc.o
+#PERIPH_OBJECTS += stm8s_gpio.o
+#PERIPH_OBJECTS += stm8s_i2c.o
+#PERIPH_OBJECTS += stm8s_itc.o
 #PERIPH_OBJECTS += stm8s_iwdg.o
 #PERIPH_OBJECTS += stm8s_rst.o
 #PERIPH_OBJECTS += stm8s_spi.o
-PERIPH_OBJECTS += stm8s_tim1.o
+#PERIPH_OBJECTS += stm8s_tim1.o
 #PERIPH_OBJECTS += stm8s_tim2.o
-PERIPH_OBJECTS += stm8s_tim4.o
+#PERIPH_OBJECTS += stm8s_tim4.o
 #PERIPH_OBJECTS += stm8s_uart1.o
 #PERIPH_OBJECTS += stm8s_wwdg.o
 

+ 1 - 46
inc/board.h

@@ -6,51 +6,6 @@
 #define GPIO_LOW(a,b)     a->ODR &= ~b
 #define GPIO_TOGGLE(a,b)  a->ODR ^= b
 
-/* LED
- AAA
-F   B
-F   B
- GGG
-E   C
-E   C
- DDD P
-*/
-#define LED_SEG1_PORT   GPIOA
-#define LED_SEG1_PINS   GPIO_PIN_3
-#define LED_SEG_DP      GPIO_PIN_3
-
-#define LED_SEG3_PORT   GPIOC
-#define LED_SEG3_PINS   (GPIO_PIN_4 | GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_3 | GPIO_PIN_5)
-#define LED_SEG_A       GPIO_PIN_3
-#define LED_SEG_F       GPIO_PIN_4
-#define LED_SEG_B       GPIO_PIN_5
-#define LED_SEG_E       GPIO_PIN_6
-#define LED_SEG_G       GPIO_PIN_7
-
-#define LED_SEG2_PORT   GPIOD
-#define LED_SEG2_PINS   (GPIO_PIN_1 | GPIO_PIN_4)
-#define LED_SEG_C       GPIO_PIN_1
-#define LED_SEG_D       GPIO_PIN_4
-
-#define LED_OUT_OFF   {LED_SEG1_PORT->ODR |= LED_SEG1_PINS; LED_SEG2_PORT->ODR |= LED_SEG2_PINS; LED_SEG3_PORT->ODR |= LED_SEG3_PINS;}
-#define LED_OUT_DP    GPIOA->ODR &= ~LED_SEG_DP
-#define LED_OUT_MM    GPIOC->ODR &= ~LED_SEG_G
-#define LED_OUT_0     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_E|LED_SEG_F); GPIOD->ODR &= ~(LED_SEG2_PINS)
-#define LED_OUT_1     GPIOC->ODR &= ~(LED_SEG_B); GPIOD->ODR &= ~(LED_SEG_C)
-#define LED_OUT_2     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_E|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG_D)
-#define LED_OUT_3     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
-#define LED_OUT_4     GPIOC->ODR &= ~(LED_SEG_B|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG_C)
-#define LED_OUT_5     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
-#define LED_OUT_6     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_E|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
-#define LED_OUT_7     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B); GPIOD->ODR &= ~(LED_SEG_C)
-#define LED_OUT_8     GPIOC->ODR &= ~(LED_SEG3_PINS); GPIOD->ODR &= ~(LED_SEG2_PINS)
-#define LED_OUT_9     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
-
-/* shift register control pins */
-#define SPI_PORT      GPIOA
-#define SPI_SCK       GPIO_PIN_1
-#define SPI_DATA      GPIO_PIN_2
-
 /* ADC */
 #define ADC_PORT  GPIOD
 #define ADC_PINU  GPIO_PIN_2
@@ -66,6 +21,6 @@ E   C
 #define CURRENT_MULT_TOP  3600
 #define CURRENT_MULT_BOT  680
 // shunt resistance in mili Ohms
-#define ADC_SHUNT 50
+#define ADC_SHUNT 10
 
 #endif /* __BOARD_H */

+ 6 - 0
inc/delay.h

@@ -0,0 +1,6 @@
+#ifndef __DELAY_H
+#define __DELAY_H
+
+void Delay(__IO uint16_t nTime);
+
+#endif /* __DELAY_H */

+ 0 - 179
lib/i2c.c

@@ -1,179 +0,0 @@
-/******************************************************************************
-* Драйвер модуля I2C микроконтроллера STM8S003F
-*
-* Автор: Осипов Андрей
-* Дата:  17 июля 2014
-* URL:   http://hamlab.net/mcu/stm8/i2c.html
-******************************************************************************/
-
-#include "i2c.h"
-
-/* Таймаут ожидания события I2C */
-extern __IO uint8_t I2C_timeout;
-
-/* Ожидание наступления события event в течении времени timeout в мс */
-#define wait_event(event, timeout) I2C_timeout = timeout;\
-                                   while(event && I2C_timeout);\
-                                   if(!I2C_timeout) return I2C_TIMEOUT;
-
-
-/**
- * @brief Инициализация I2C интерфейса
- */
-void i2c_master_init(void) {
-  uint16_t ccr;
-
-  /* Настройка GPIO SDA SCL - HiZ, Open drain, Fast */
-  GPIOB->DDR |= (GPIO_PIN_4 | GPIO_PIN_5);
-  GPIOB->ODR |= (GPIO_PIN_4 | GPIO_PIN_5);
-  GPIOB->CR2 |= (GPIO_PIN_5 | GPIO_PIN_5);
-
-  //Частота тактирования периферии MHz
-  I2C->FREQR = (uint8_t)(F_MASTER_MHZ);
-
-  //Отключаем I2C
-  I2C->CR1 &= (uint8_t)(~I2C_CR1_PE);
-
-  //Выбираем стандартный режим
-  I2C->CCRH = 0;
-
-  //CCR = Fmaster/2*Fiic = 16MHz/2*100kHz = 80
-  ccr = (uint16_t)(F_MASTER_HZ / (2 * F_I2C_HZ));
-
-  //Set Maximum Rise Time: 1000ns max in Standard Mode
-  //= [1000ns/(1/InputClockFrequencyMHz.10e6)]+1
-  //= InputClockFrequencyMHz+1
-  I2C->TRISER = (uint8_t)(F_MASTER_MHZ + 1);
-  I2C->CCRL = (uint8_t)ccr;
-  I2C->CCRH = (uint8_t)((uint8_t)(ccr >> 8) & I2C_CCRH_CCR);
-
-  //Включаем I2C
-  I2C->CR1 |= I2C_CR1_PE;
-
-  //Разрешаем подтверждение в конце посылки
-  I2C->CR2 |= I2C_CR2_ACK;
-}
-
-/**
- * @brief Запись регистра slave-устройства
- */
-t_i2c_status i2c_wr_reg(uint8_t address, uint8_t reg_addr, uint16_t data) {
-
-  //Ждем освобождения шины I2C
-  wait_event((I2C->SR3 & I2C_SR3_BUSY), 10);
-
-  //Генерация СТАРТ-посылки
-  I2C->CR2 |= I2C_CR2_START;
-  //Ждем установки бита SB
-  wait_event(!(I2C->SR1 & I2C_SR1_SB), 1);
-
-
-  //Записываем в регистр данных адрес ведомого устройства
-  I2C->DR = address & 0xFE;
-  //Ждем подтверждения передачи адреса
-  wait_event(!(I2C->SR1 & I2C_SR1_ADDR), 1);
-  //Очистка бита ADDR чтением регистра SR3
-  I2C->SR3;
-
-
-  //Ждем освобождения регистра данных
-  wait_event(!(I2C->SR1 & I2C_SR1_TXE), 1);
-  //Отправляем адрес регистра
-  I2C->DR = reg_addr;
-
-  //Отправка данных
-  //Ждем освобождения регистра данных
-  wait_event(!(I2C->SR1 & I2C_SR1_TXE), 1);
-  //Отправляем адрес регистра
-  I2C->DR = (uint8_t)(data >> 8);
-  //Ждем освобождения регистра данных
-  wait_event(!(I2C->SR1 & I2C_SR1_TXE), 1);
-  //Отправляем адрес регистра
-  I2C->DR = (uint8_t)data;
-
-  //Ловим момент, когда DR освободился и данные попали в сдвиговый регистр
-  wait_event(!((I2C->SR1 & I2C_SR1_TXE) && (I2C->SR1 & I2C_SR1_BTF)), 1);
-
-  //Посылаем СТОП-посылку
-  I2C->CR2 |= I2C_CR2_STOP;
-  //Ждем выполнения условия СТОП
-  wait_event((I2C->CR2 & I2C_CR2_STOP), 1);
-
-  return I2C_SUCCESS;
-}
-
-/**
- * @brief Чтение регистра slave-устройства
- * @note Start -> Slave Addr -> Reg. addr -> Restart -> Slave Addr <- data ... -> Stop
- */
-t_i2c_status i2c_rd_reg(uint8_t address, uint8_t reg_addr, uint16_t * data) {
-
-  //Ждем освобождения шины I2C
-  wait_event((I2C->SR3 & I2C_SR3_BUSY), 10);
-
-  //Разрешаем подтверждение в конце посылки
-  I2C->CR2 |= I2C_CR2_ACK;
-
-  //Генерация СТАРТ-посылки
-  I2C->CR2 |= I2C_CR2_START;
-  //Ждем установки бита SB
-  wait_event(!(I2C->SR1 & I2C_SR1_SB), 1);
-
-  //Записываем в регистр данных адрес ведомого устройства
-  I2C->DR = address & 0xFE;
-  //Ждем подтверждения передачи адреса
-  wait_event(!(I2C->SR1 & I2C_SR1_ADDR), 1);
-  //Очистка бита ADDR чтением регистра SR3
-  I2C->SR3;
-
-  //Ждем освобождения регистра данных RD
-  wait_event(!(I2C->SR1 & I2C_SR1_TXE), 1);
-
-  //Передаем адрес регистра slave-устройства, который хотим прочитать
-  I2C->DR = reg_addr;
-  //Ловим момент, когда DR освободился и данные попали в сдвиговый регистр
-  wait_event(!((I2C->SR1 & I2C_SR1_TXE) && (I2C->SR1 & I2C_SR1_BTF)), 1);
-
-  //Генерация СТАРТ-посылки (рестарт)
-  I2C->CR2 |= I2C_CR2_START;
-  //Ждем установки бита SB
-  wait_event(!(I2C->SR1 & I2C_SR1_SB), 1);
-
-  //Записываем в регистр данных адрес ведомого устройства и переходим
-  //в режим чтения (установкой младшего бита в 1)
-  I2C->DR = address | 0x01;
-
-  //Отправка двух байт данных
-  //Бит который разрешает NACK на следующем принятом байте
-  I2C->CR2 |= I2C_CR2_POS;
-  //Ждем подтверждения передачи адреса
-  wait_event(!(I2C->SR1 & I2C_SR1_ADDR), 1);
-  //Заплатка из Errata
-  disableInterrupts();
-  //Очистка бита ADDR чтением регистра SR3
-  I2C->SR3;
-  //Запрещаем подтверждение в конце посылки
-  I2C->CR2 &= (uint8_t)(~I2C_CR2_ACK);
-  //Заплатка из Errata
-  enableInterrupts();
-  //Ждем момента, когда первый байт окажется в DR,
-  //а второй в сдвиговом регистре
-  wait_event(!(I2C->SR1 & I2C_SR1_BTF), 1);
-
-  //Заплатка из Errata
-  disableInterrupts();
-  //Устанавлием бит STOP
-  I2C->CR2 |= I2C_CR2_STOP;
-  //Читаем принятые байты
-  *data = I2C->DR << 8;
-  //Заплатка из Errata
-  enableInterrupts();
-  *data |= I2C->DR;
-
-  //Ждем отправки СТОП посылки
-  wait_event((I2C->CR2 & I2C_CR2_STOP), 1);
-  //Сбрасывает бит POS, если вдруг он был установлен
-  I2C->CR2 &= ~I2C_CR2_POS;
-
-  return I2C_SUCCESS;
-}

+ 0 - 33
lib/i2c.h

@@ -1,33 +0,0 @@
-#pragma once
-#ifndef I2C_H
-#define I2C_H
-
-#include "stm8s.h"
-
-#define I2C_FAST        1
-#define F_MASTER_MHZ    16UL
-#define F_MASTER_HZ     16000000UL
-#ifdef I2C_FAST
-//400 кГц
-#define F_I2C_HZ        400000UL
-#else
-//100 кГц
-#define F_I2C_HZ        100000UL
-#endif // I2C_FAST
-
-//Результат выполнения операции с i2c
-typedef enum {
-    I2C_SUCCESS = 0,
-    I2C_TIMEOUT,
-    I2C_ERROR
-} t_i2c_status;
-
-// Инициализация I2C интерфейса
-extern void i2c_master_init(void);
-
-// Запись регистра slave-устройства
-extern t_i2c_status  i2c_wr_reg(uint8_t address, uint8_t reg_addr, uint16_t data);
-
-// Чтение регистра slave-устройства
-extern t_i2c_status  i2c_rd_reg(uint8_t address, uint8_t reg_addr, uint16_t * data);
-#endif // I2C_H

+ 0 - 206
lib/ina219.c

@@ -1,206 +0,0 @@
-/*
-INA219.cpp - Class file for the INA219 Zero-Drift, Bi-directional Current/Power Monitor Arduino Library.
-
-Version: 1.0.0
-(c) 2014 Korneliusz Jarzebski
-www.jarzebski.pl
-
-This program is free software: you can redistribute it and/or modify
-it under the terms of the version 3 GNU General Public License as
-published by the Free Software Foundation.
-
-This program is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with this program.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#include "INA219.h"
-#include "i2c.h"
-
-static uint16_t vShuntMax; // millivolt
-static uint16_t vBusMax; // millivolt
-static uint16_t rShunt; // milliohms
-
-static uint8_t flagCNVR; // Conversion Ready
-static uint8_t flagOVF; // Math Overflow Flag
-
-static t_i2c_status status = I2C_SUCCESS;
-
-/**
-  * Configure I2C, INA219
-  */
-void INA219_Config(void) {
-  uint16_t config;
-
-  vBusMax = 32000; // !!! Maximum input voltage only 26V !!!
-  vShuntMax = 80; // !! for shunt 10 mOhm and maximum 8A input current
-  rShunt = CURRENT_SHUNT_RESISTANCE; // in mOhm
-
-  /** INA219 configure */
-  config  = INA219_RESET_OFF; // INA219_RST
-  config |= INA219_RANGE_32V; // INA219_BVR
-  config |= INA219_GAIN_80MV; // INA219_PG
-  config |= INA219_BUS_RES_12BIT_128S; // INA219_BADC
-  config |= INA219_SHUNT_RES_12BIT_128S; //INA219_SADC
-  config |= INA219_MODE_SHUNT_BUS_CONT; //INA219_MODE
-
-  i2c_wr_reg(INA219_ADDRESS, INA219_REG_CONFIG, config);
-
-  i2c_wr_reg(INA219_ADDRESS, INA219_REG_CALIBRATION, CALIBRATION_VALUE);
-}
-
-/** */
-uint16_t getMaxPossibleCurrent(void) {
-  return (((1000 * vShuntMax) + (rShunt>>1)) / rShunt);
-}
-
-/** */
-uint16_t getMaxCurrent(void) {
-  uint16_t maxCurrent = ((CURRENT_LSB * 32767) + 500) / 1000;
-  uint16_t maxPossible = getMaxPossibleCurrent();
-
-  if (maxCurrent > maxPossible) {
-    return maxPossible;
-  } else {
-    return maxCurrent;
-  }
-}
-
-/** */
-uint16_t getMaxShuntVoltage(void) {
-  uint16_t maxVoltage = ((getMaxCurrent() * rShunt) + 500) / 1000;
-
-  if (maxVoltage >= vShuntMax) {
-    return vShuntMax;
-  } else {
-    return maxVoltage;
-  }
-}
-
-/** */
-uint32_t getMaxPower(void) {
-  return (((getMaxCurrent() * vBusMax) + 500000) / 1000000);
-}
-
-/** */
-uint16_t readBusCurrent(void) {
-  uint32_t current;;
-  int16_t tmp;
-
-  status = i2c_rd_reg(INA219_ADDRESS, INA219_REG_CURRENT, (uint16_t *)&tmp);
-  if (status != I2C_SUCCESS) {
-    return 9999;
-  }
-
-  if (tmp < 0) {
-    tmp = - tmp;
-  }
-  current = tmp;
-  current *= CURRENT_LSB;
-  current += 500;
-  current /= 1000;
-
-  return (uint16_t)current;
-}
-
-/** */
-uint32_t readBusPower(void) {
-  uint32_t power;
-  uint16_t tmp;
-
-  i2c_rd_reg(INA219_ADDRESS, INA219_REG_POWER, &tmp);
-  power = tmp;
-  power *= POWER_LSB;
-  power += 500;
-  power /= 1000;
-  return power;
-}
-
-/**
-  * Currently return raw value of shunt voltage in 10 uV
-  */
-int16_t readShuntVoltage(void) {
-  uint16_t shvolt;
-  status = i2c_rd_reg(INA219_ADDRESS, INA219_REG_SHUNTVOLTAGE, &shvolt);
-  if (status != I2C_SUCCESS) {
-    return 999;
-  }
-  return shvolt;
-}
-
-/**
-  * Return bus voltage in mV
-  */
-uint16_t readBusVoltage(void) {
-  uint16_t volt;
-
-  flagCNVR = 0;
-  flagOVF = 0;
-
-  status = i2c_rd_reg(INA219_ADDRESS, INA219_REG_BUSVOLTAGE, &volt);
-  if (status != I2C_SUCCESS) {
-    return 65535;
-  }
-
-  if ((volt & 0x0001) != 0) {
-    flagOVF = 1;
-  }
-  if ((volt & 0x0002) != 0) {
-    flagCNVR = 1;
-  }
-
-  return ((volt >> 3) * 4);
-}
-
-ina219_bvr_t getRange(void) {
-  uint16_t value;
-
-  i2c_rd_reg(INA219_ADDRESS, INA219_REG_CONFIG, &value);
-  value &= 0x2000;
-  value >>= 13;
-
-  return (ina219_bvr_t)value;
-}
-
-ina219_pg_t getGain(void) {
-  uint16_t value;
-
-  i2c_rd_reg(INA219_ADDRESS, INA219_REG_CONFIG, &value);
-  value &= 0x1800;
-  value >>= 11;
-
-  return (ina219_pg_t)value;
-}
-
-ina219_badc_t getBusRes(void) {
-  uint16_t value;
-
-  i2c_rd_reg(INA219_ADDRESS, INA219_REG_CONFIG, &value);
-  value &= 0x0780;
-  value >>= 7;
-
-  return (ina219_badc_t)value;
-}
-
-ina219_sadc_t getShuntRes(void) {
-  uint16_t value;
-
-  i2c_rd_reg(INA219_ADDRESS, INA219_REG_CONFIG, &value);
-  value &= 0x0078;
-  value >>= 3;
-
-  return (ina219_sadc_t)value;
-}
-
-ina219_mode_t getMode(void) {
-  uint16_t value;
-
-  i2c_rd_reg(INA219_ADDRESS, INA219_REG_CONFIG, &value);
-  value &= 0x0007;
-
-  return (ina219_mode_t)value;
-}

+ 0 - 146
lib/ina219.h

@@ -1,146 +0,0 @@
-/*
-INA219.h - Header file for the Zero-Drift, Bi-directional Current/Power Monitor Arduino Library.
-
-Version: 1.0.0
-(c) 2014 Korneliusz Jarzebski
-www.jarzebski.pl
-
-This program is free software: you can redistribute it and/or modify
-it under the terms of the version 3 GNU General Public License as
-published by the Free Software Foundation.
-
-This program is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-GNU General Public License for more details.
-
-You should have received a copy of the GNU General Public License
-along with this program.  If not, see <http://www.gnu.org/licenses/>.
-*/
-
-#ifndef INA219_h
-#define INA219_h
-
-#include "stm8s.h"
-
-#define CURRENT_SHUNT_RESISTANCE    10
-#define INA219_ADDRESS              (uint8_t)(0x40 << 1)
-#define INA219_ADDR_RD              (INA219_ADDRESS | 0x01)
-
-/**
- * Calibration value -- ôîðìóëû èç ìàíóàëà:
- * Current_LSB = Maximum Expected Current / 2^15
- * Power_LSB = 20 * Current_LSB
- * Calibration_Value = trunc( 0.04096 / (Current_LSB * Rshunt) )
- */
-// 8000000 uA / 32768
-#define CURRENT_LSB                 244
-#define POWER_LSB                   4880
-// 0.04096 / ((8 A/32768) * 0.01 Ohm)
-#define CALIBRATION_VALUE           16777
-
-
-/* INA219 Registers */
-#define INA219_REG_CONFIG           0x00
-#define INA219_REG_SHUNTVOLTAGE     0x01
-#define INA219_REG_BUSVOLTAGE       0x02
-#define INA219_REG_POWER            0x03
-#define INA219_REG_CURRENT          0x04
-#define INA219_REG_CALIBRATION      0x05
-
-typedef enum
-{
-  INA219_RESET_OFF            = 0x0000,
-  INA219_RESET_ON             = 0x8000
-} ina219_reset_t;
-
-typedef enum
-{
-  INA219_RANGE_16V            = 0x0000,
-  INA219_RANGE_32V            = 0x2000
-} ina219_bvr_t;
-
-typedef enum
-{
-  INA219_GAIN_40MV            = 0x0000,
-  INA219_GAIN_80MV            = 0x0800,
-  INA219_GAIN_160MV           = 0x1000,
-  INA219_GAIN_320MV           = 0x1800
-} ina219_pg_t;
-
-typedef enum
-{
-  INA219_BUS_RES_9BIT         = 0x0000,
-  INA219_BUS_RES_10BIT        = 0x0080,
-  INA219_BUS_RES_11BIT        = 0x0100,
-  INA219_BUS_RES_12BIT        = 0x0180,
-  INA219_BUS_RES_12BIT_1S     = 0x0400,
-  INA219_BUS_RES_12BIT_2S     = 0x0480,
-  INA219_BUS_RES_12BIT_4S     = 0x0500,
-  INA219_BUS_RES_12BIT_8S     = 0x0580,
-  INA219_BUS_RES_12BIT_16S    = 0x0600,
-  INA219_BUS_RES_12BIT_32S    = 0x0680,
-  INA219_BUS_RES_12BIT_64S    = 0x0700,
-  INA219_BUS_RES_12BIT_128S   = 0x0780
-} ina219_badc_t;
-
-typedef enum
-{
-  INA219_SHUNT_RES_9BIT       = 0x0000,
-  INA219_SHUNT_RES_10BIT      = 0x0008,
-  INA219_SHUNT_RES_11BIT      = 0x0010,
-  INA219_SHUNT_RES_12BIT      = 0x0018,
-  INA219_SHUNT_RES_12BIT_1S   = 0x0040,
-  INA219_SHUNT_RES_12BIT_2S   = 0x0048,
-  INA219_SHUNT_RES_12BIT_4S   = 0x0050,
-  INA219_SHUNT_RES_12BIT_8S   = 0x0058,
-  INA219_SHUNT_RES_12BIT_16S  = 0x0060,
-  INA219_SHUNT_RES_12BIT_32S  = 0x0068,
-  INA219_SHUNT_RES_12BIT_64S  = 0x0070,
-  INA219_SHUNT_RES_12BIT_128S = 0x0078
-} ina219_sadc_t;
-
-typedef enum
-{
-  INA219_MODE_POWER_DOWN      = 0x0000,
-  INA219_MODE_SHUNT_TRIG      = 0x0001,
-  INA219_MODE_BUS_TRIG        = 0x0002,
-  INA219_MODE_SHUNT_BUS_TRIG  = 0x0003,
-  INA219_MODE_ADC_OFF         = 0x0004,
-  INA219_MODE_SHUNT_CONT      = 0x0005,
-  INA219_MODE_BUS_CONT        = 0x0006,
-  INA219_MODE_SHUNT_BUS_CONT  = 0x0007
-} ina219_mode_t;
-
-/**
-  * @brief  INA219 Configuration register definition
-  */
-typedef struct {
-  uint8_t         INA219_Addr; /* I2C address */
-  ina219_reset_t  INA219_RST;  /* Reset Bit */
-  ina219_bvr_t    INA219_BVR;  /* Bus Voltage Range */
-  ina219_pg_t     INA219_PG;   /* PGA (Shunt Voltage Only) */
-  ina219_badc_t   INA219_BADC; /* Bus ADC Resolution/Averaging */
-  ina219_sadc_t   INA219_SADC; /* Shunt ADC Resolution/Averaging */
-  ina219_mode_t   INA219_MODE; /* Operating Mode */
-} INA219_InitTypeDef;
-
-void INA219_Config(void);
-
-ina219_bvr_t getRange(void);
-ina219_pg_t getGain(void);
-ina219_badc_t getBusRes(void);
-ina219_sadc_t getShuntRes(void);
-ina219_mode_t getMode(void);
-
-int16_t readShuntVoltage(void);
-uint16_t readBusVoltage(void);
-uint16_t readBusCurrent(void);
-uint32_t readBusPower(void);
-
-uint16_t getMaxPossibleCurrent(void);
-uint16_t getMaxCurrent(void);
-uint16_t getMaxShuntVoltage(void);
-uint32_t getMaxPower(void);
-
-#endif

+ 132 - 0
lib/led.c

@@ -0,0 +1,132 @@
+/**
+ * Драйвер модуля LED-индикаторов
+ */
+
+#include "stm8s.h"
+#include "led.h"
+
+#define GPIO_HIGH(a,b)    a->ODR |= b
+#define GPIO_LOW(a,b)     a->ODR &= ~b
+#define GPIO_TOGGLE(a,b)  a->ODR ^= b
+
+uint8_t LedDigits[LED_DIGITS_NUM] = {1, 2, 3, 4, 5, 6, 7, 8}; // digits to dsplay
+uint8_t LedPoint[LED_DIGITS_NUM] = {0, 1, 0, 0, 0, 0, 0, 1}; // dots for digits
+
+static const uint16_t led_num[8] = {0x10, 0x20, 0x80, 0x40, 0x01, 0x02, 0x08, 0x04};
+
+static void led_SelectDigit (const uint8_t pos);
+
+/*
+ * Output current value to next led
+ */
+void led_OutputValue(void) {
+  static uint8_t ledn = 0;
+
+  /* all off */
+  LED_OUT_OFF;
+
+  /* Fire on nex led */
+  led_SelectDigit(ledn);
+
+  /* out next value */
+  switch (LedDigits[ledn]) {
+    case 0:
+      LED_OUT_0;
+      break;
+    case 1:
+      LED_OUT_1;
+      break;
+    case 2:
+      LED_OUT_2;
+      break;
+    case 3:
+      LED_OUT_3;
+      break;
+    case 4:
+      LED_OUT_4;
+      break;
+    case 5:
+      LED_OUT_5;
+      break;
+    case 6:
+      LED_OUT_6;
+      break;
+    case 7:
+      LED_OUT_7;
+      break;
+    case 8:
+      LED_OUT_8;
+      break;
+    case 9:
+      LED_OUT_9;
+      break;
+/*
+    case led_A:
+      LED_OUT_A;
+      break;
+    case led_B:
+      LED_OUT_B;
+      break;
+    case led_C:
+      LED_OUT_C;
+      break;
+    case led_D:
+      LED_OUT_D;
+      break;
+    case led_E:
+      LED_OUT_E;
+      break;
+    case led_F:
+      LED_OUT_F;
+      break;
+    case led_H:
+      LED_OUT_H;
+      break;
+    case led_U:
+      LED_OUT_U;
+      break;
+    case led_Plus:
+      LED_OUT_PL;
+      break;
+    case led_O:
+      LED_OUT_O;
+      break;
+    case led_Off:
+      break;
+*/
+    case led_Minus:
+      LED_OUT_MM;
+      break;
+    default:
+      LED_OUT_MM;
+  }
+
+  if(LedPoint[ledn] != 0) {
+    LED_OUT_DP;
+  }
+
+  ledn ++;
+  if (ledn >= LED_DIGITS_NUM) {
+    ledn = 0;
+  }
+}
+
+/*
+ * Select LED Digit.
+ * led digits sequence (b - bottom, t - top):
+ * t1, t2, t4, t3, b1, b2, b3, b4.
+ */
+static void led_SelectDigit (const uint8_t pos) {
+  uint8_t i;
+  uint8_t data = led_num[pos];
+  for (i=0; i<8; i++) {
+    GPIO_LOW(SPI_PORT, SPI_SCK); // prepare CLK
+    if (data & 0x80) { // if msb == 1
+      GPIO_HIGH(SPI_PORT, SPI_DATA); // DATA = 1
+    } else {
+      GPIO_LOW(SPI_PORT, SPI_DATA); // DATA = 0
+    }
+    GPIO_HIGH(SPI_PORT, SPI_SCK); // shift bit
+    data <<= 1;
+  }
+}

+ 83 - 0
lib/led.h

@@ -0,0 +1,83 @@
+#pragma once
+#ifndef LED_H
+#define LED_H
+
+// time to show one led digit, ms
+#define LED_ONE_PERIOD  2
+#define LED_DIGITS_NUM  8
+
+/* LED segments pins
+ AAA
+F   B
+F   B
+ GGG
+E   C
+E   C
+ DDD P
+*/
+/* LED
+ AAA
+F   B
+F   B
+ GGG
+E   C
+E   C
+ DDD P
+*/
+#define LED_SEG1_PORT   GPIOA
+#define LED_SEG1_PINS   GPIO_PIN_3
+#define LED_SEG_DP      GPIO_PIN_3
+
+#define LED_SEG3_PORT   GPIOC
+#define LED_SEG3_PINS   (GPIO_PIN_4 | GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_3 | GPIO_PIN_5)
+#define LED_SEG_A       GPIO_PIN_3
+#define LED_SEG_F       GPIO_PIN_4
+#define LED_SEG_B       GPIO_PIN_5
+#define LED_SEG_E       GPIO_PIN_6
+#define LED_SEG_G       GPIO_PIN_7
+
+#define LED_SEG2_PORT   GPIOD
+#define LED_SEG2_PINS   (GPIO_PIN_1 | GPIO_PIN_4)
+#define LED_SEG_C       GPIO_PIN_1
+#define LED_SEG_D       GPIO_PIN_4
+
+#define LED_OUT_OFF   {LED_SEG1_PORT->ODR |= LED_SEG1_PINS; LED_SEG2_PORT->ODR |= LED_SEG2_PINS; LED_SEG3_PORT->ODR |= LED_SEG3_PINS;}
+#define LED_OUT_DP    GPIOA->ODR &= ~LED_SEG_DP
+#define LED_OUT_MM    GPIOC->ODR &= ~LED_SEG_G
+#define LED_OUT_0     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_E|LED_SEG_F); GPIOD->ODR &= ~(LED_SEG2_PINS)
+#define LED_OUT_1     GPIOC->ODR &= ~(LED_SEG_B); GPIOD->ODR &= ~(LED_SEG_C)
+#define LED_OUT_2     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_E|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG_D)
+#define LED_OUT_3     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
+#define LED_OUT_4     GPIOC->ODR &= ~(LED_SEG_B|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG_C)
+#define LED_OUT_5     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
+#define LED_OUT_6     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_E|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
+#define LED_OUT_7     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B); GPIOD->ODR &= ~(LED_SEG_C)
+#define LED_OUT_8     GPIOC->ODR &= ~(LED_SEG3_PINS); GPIOD->ODR &= ~(LED_SEG2_PINS)
+#define LED_OUT_9     GPIOC->ODR &= ~(LED_SEG_A|LED_SEG_B|LED_SEG_F|LED_SEG_G); GPIOD->ODR &= ~(LED_SEG2_PINS)
+
+/* shift register control pins */
+#define SPI_PORT      GPIOA
+#define SPI_SCK       GPIO_PIN_1
+#define SPI_DATA      GPIO_PIN_2
+
+typedef enum {
+  led_A = 0xa,
+  led_B = 0xb,
+  led_C = 0xc,
+  led_D = 0xd,
+  led_E = 0xe,
+  led_F = 0xf,
+  led_H = 0x11,
+  led_U = 0x12,
+  led_Plus = 0x21,
+  led_Minus = 0x22,
+  led_O = 0x23, /* gradus */
+  led_Off = 0xff
+} led_sym_t;
+
+extern uint8_t LedDigits[];
+extern uint8_t LedPoint[];
+
+void led_OutputValue(void);
+
+#endif // LED_H

+ 105 - 120
src/main.c

@@ -20,7 +20,8 @@
 /* Includes ------------------------------------------------------------------*/
 #include "stm8s.h"
 #include "board.h"
-#include "i2c.h"
+#include "led.h"
+#include "delay.h"
 
 /* Private define ------------------------------------------------------------*/
 #define TIM4_PERIOD  (uint8_t)124
@@ -28,98 +29,87 @@
 #define LED_ONE_PERIOD  2
 
 /* Private typedef -----------------------------------------------------------*/
-/* Private constants ---------------------------------------------------------*/
-static const uint16_t led_num[8] = {0x10, 0x20, 0x80, 0x40, 0x01, 0x02, 0x08, 0x04};
+typedef enum {
+  voltage_mode,
+  current_mode
+} measure_mode_t;
 
+/* Private constants ---------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
 __IO uint16_t ConversionBuffer[ADC_SMPLS];
 __IO uint8_t BufferIndex = 0;
-__IO uint8_t LedCnt = 0;
-static uint8_t LedDigits[8] = {1, 2, 3, 4, 5, 6, 7, 8}; // digits to dsplay
-static uint8_t LedPoint[8] = {0, 0, 0, 1, 0, 0, 0, 1}; // dots for digits
+static uint16_t Voltage, Current;
+static measure_mode_t MeasureMode = voltage_mode;
 
 /* Private function prototypes -----------------------------------------------*/
 static void boardInit(void);
-static void led_SelectDigit (uint8_t first);
-static void led_OutputValue(void);
+static void showV(void);
+static void showC(void);
 
 void main(void)
 {
   /* Board Configuration */
   boardInit();
-
-  /* I2C Configuration */
-  //i2c_master_init();
+  Delay(1000);
 
   /* Infinite loop */
   while (1) {
-    if (LedCnt >= LED_ONE_PERIOD) {
-      LedCnt = 0;
-      led_OutputValue();
-    }
-
     wfi();
+    if (BufferIndex >= ADC_SMPLS) {
+      BufferIndex = 0;
+      int8_t i;
+      uint32_t tbuf = 0;
+      for (i=0; i<ADC_SMPLS; i++) {
+        tbuf += ConversionBuffer[i];  
+      }
+
+      if (MeasureMode == voltage_mode) {
+        MeasureMode = current_mode;
+        ADC1->CSR &= (uint8_t)(~ADC1_CSR_CH);
+        ADC1->CSR |= (uint8_t)(ADC_CHNLI);
+
+        tbuf /= ADC_SMPLS;
+        //tbuf *= VOLTAGE_MUL;
+        //tbuf /= VOLT_MUL_MUL;
+        Voltage = tbuf;
+      } else {
+        MeasureMode = voltage_mode;
+        ADC1->CSR &= (uint8_t)(~ADC1_CSR_CH);
+        ADC1->CSR |= (uint8_t)(ADC_CHNLU);
+
+        tbuf /= ADC_SMPLS;
+        //tbuf *= CURRENT_MUL;
+        //tbuf /= CURR_MUL_MUL;
+        Current = tbuf;
+      }
+
+      /* arrived new data, show it */
+      showV();
+      showC();
+    }
   }
 
 }
 
 /* Private functions ---------------------------------------------------------*/
-/*
- * Output current value to next led
- */
-static void led_OutputValue(void) {
-  static uint8_t ledn = 0;
-
-  /* all off */
-  LED_OUT_OFF;
-
-  /* Fire on nex led */
-  led_SelectDigit(ledn);
-
-  /* out next value */
-  switch (LedDigits[ledn]) {
-    case 0:
-      LED_OUT_0;
-      break;
-    case 1:
-      LED_OUT_1;
-      break;
-    case 2:
-      LED_OUT_2;
-      break;
-    case 3:
-      LED_OUT_3;
-      break;
-    case 4:
-      LED_OUT_4;
-      break;
-    case 5:
-      LED_OUT_5;
-      break;
-    case 6:
-      LED_OUT_6;
-      break;
-    case 7:
-      LED_OUT_7;
-      break;
-    case 8:
-      LED_OUT_8;
-      break;
-    case 9:
-      LED_OUT_9;
-      break;
-    default:
-      LED_OUT_MM;
-  }
-  /*
-  if(LedPoint[ledn] != 0) {
-    LED_OUT_DP;
-  }
-  */
-  ledn ++;
-  if (ledn > 7) {
-    ledn = 0;
-  }
+static void showV(void) {
+  uint16_t a = Voltage;
+  LedDigits[0] = a / 1000;
+  uint16_t b = a % 1000;
+  LedDigits[1] = b / 100;
+  uint8_t c = b % 100;
+  LedDigits[2] = c / 10;
+  LedDigits[3] = c % 10;
+}
+
+static void showC(void) {
+  uint16_t a = Current;
+  LedDigits[4] = a / 1000;
+  uint16_t b = a % 1000;
+  LedDigits[5] = b / 100;
+  uint8_t c = b % 100;
+  LedDigits[6] = c / 10;
+  LedDigits[7] = c % 10;
 }
 
 static void boardInit(void) {
@@ -162,41 +152,58 @@ static void boardInit(void) {
   SPI_PORT->CR1 |= (uint8_t)(SPI_SCK|SPI_DATA);
   SPI_PORT->CR2 |= (uint8_t)(SPI_SCK|SPI_DATA);
 
-  /** Configure ADC */
-  /* De-Init ADC peripheral*/
-  //ADC1_DeInit();
-
-  /* Init ADC1 peripheral */
-  //ADC1_Init(ADC1_CONVERSIONMODE_SINGLE, ADC_CHNLU, ADC1_PRESSEL_FCPU_D12, \
-            ADC1_EXTTRIG_TIM, ENABLE, ADC1_ALIGN_RIGHT, ADC_SCHTU, DISABLE);
-
-  /* Enable EOC interrupt */
-  //ADC1_ITConfig(ADC1_IT_EOCIE, ENABLE);
-
-  /*Start Conversion */
-  ////ADC1_StartConversion();
-
-  /** Configure TIM1 */
-  //TIM1_DeInit();
-
-  /* Time Base configuration */
-  /*
+  /* I2C GPIO SDA SCL - HiZ, Open drain, Fast */
+  //GPIOB->DDR |= (GPIO_PIN_4 | GPIO_PIN_5);
+  //GPIOB->ODR |= (GPIO_PIN_4 | GPIO_PIN_5);
+  //GPIOB->CR2 |= (GPIO_PIN_4 | GPIO_PIN_5);
+
+  /** Configure ADC1 peripheral */
+  /* Configure the data alignment */
+  ADC1->CR2 |= (uint8_t)(ADC1_CR2_ALIGN);
+  /* Set the single conversion mode */
+  ADC1->CR1 &= (uint8_t)(~ADC1_CR1_CONT);
+  /* Clear the ADC1 channels */
+  ADC1->CSR &= (uint8_t)(~ADC1_CSR_CH);
+  /* Select the ADC1 channel */
+  ADC1->CSR |= (uint8_t)(ADC_CHNLU);
+  /* Clear the SPSEL bits */
+  ADC1->CR1 &= (uint8_t)(~ADC1_CR1_SPSEL);
+  /* Select the prescaler division factor according to ADC1_PrescalerSelection values */
+  ADC1->CR1 |= (uint8_t)(ADC1_PRESSEL_FCPU_D8);
+  /* Enable the external Trigger */
+  ADC1->CR2 |= (uint8_t)(ADC1_CR2_EXTTRIG);
+  /* Set the 'Internal TIM1 TRGO event' as external trigger */
+  ADC1->CR2 &= (uint8_t)~(ADC1_CR2_EXTSEL);
+  /* disables the ADC1 Schmitt Trigger on a selected channel(s) */
+  ADC1->TDRL |= (uint8_t)((1 << ADC_SCHTU) | (1 << ADC_SCHTI));
+  /* Enable the ADC1 EOC interrupt */
+  ADC1->CSR |= (uint8_t)ADC1_IT_EOCIE;
+  /* Enable the ADC1 peripheral */
+  ADC1->CR1 |= ADC1_CR1_ADON;
+
+  /** Configure TIM1 peripheral. Time Base configuration:
   Timer period - 3,125 ms
   TIM1_Period = 50
   TIM1_Prescaler = 1000
   TIM1_CounterMode = TIM1_COUNTERMODE_UP
   TIM1_RepetitionCounter = 0
   */
-  //TIM1_TimeBaseInit(999, TIM1_COUNTERMODE_UP, 49, 0);
-
+  /* Set the Autoreload value */
+  TIM1->ARRH = (uint8_t)(49 >> 8);
+  TIM1->ARRL = (uint8_t)(49);
+  /* Set the Prescaler value */
+  TIM1->PSCRH = (uint8_t)(499 >> 8);
+  TIM1->PSCRL = (uint8_t)(499);
+  /* Select the Counter Mode */
+  TIM1->CR1 = 0x0;
+  /* Set the Repetition Counter value */
+  TIM1->RCR = 0;
   /* Trigrer configuration */
-  //TIM1_SelectOutputTrigger(TIM1_TRGOSOURCE_UPDATE);
-
+  TIM1->CR2 = (uint8_t)TIM1_TRGOSOURCE_UPDATE;
   /* Update Interrupt Enable */
-  ////TIM1_ITConfig(TIM1_IT_UPDATE, ENABLE);
-
+  //TIM1->IER |= (uint8_t)TIM1_IT_UPDATE;
   /* Enable TIM1 */
-  //TIM1_Cmd(ENABLE);
+  TIM1->CR1 |= TIM1_CR1_CEN;
 
   /**
   TIM4 configuration:
@@ -225,28 +232,6 @@ static void boardInit(void) {
   TIM4->CR1 |= (uint8_t)TIM4_CR1_CEN;
 }
 
-/*
- * Select LED Digit.
- * If first != 0 - select next, by shift '1' on outputs,
- * else select first digit.
- * led digits sequence (b - bottom, t - top):
- * b1, b2, b4, b3, t1, t2, t4, t3.
- */
-static void led_SelectDigit (const uint8_t first) {
-  uint8_t i;
-  uint8_t data = led_num[first];
-  for (i=0; i<8; i++) {
-    GPIO_LOW(SPI_PORT, SPI_SCK); // prepare CLK
-    if (data & 0x80) { // if msb == 1
-      GPIO_HIGH(SPI_PORT, SPI_DATA); // DATA = 1
-    } else {
-      GPIO_LOW(SPI_PORT, SPI_DATA); // DATA = 0
-    }
-    GPIO_HIGH(SPI_PORT, SPI_SCK); // shift bit
-    data <<= 1;
-  }
-}
-
 #ifdef USE_FULL_ASSERT
 /**
   * @brief  Reports the name of the source file and the source line number

+ 37 - 8
src/stm8s_it.c

@@ -29,6 +29,9 @@
 
 /* Includes ------------------------------------------------------------------*/
 #include "stm8s_it.h"
+#include "led.h"
+#include "board.h"
+#include "delay.h"
 
 /** @addtogroup Template_Project
   * @{
@@ -38,9 +41,11 @@
 /* Private define ------------------------------------------------------------*/
 /* Private macro -------------------------------------------------------------*/
 /* Private variables ---------------------------------------------------------*/
-extern __IO uint16_t ConversionBuffer[64];
+extern __IO uint16_t ConversionBuffer[ADC_SMPLS];
 extern __IO uint8_t BufferIndex;
-extern __IO uint8_t LedCnt;
+static uint8_t LedCnt = 0;
+static __IO uint16_t TimingDelay;
+__IO uint8_t I2C_timeout;
 
 /* Private function prototypes -----------------------------------------------*/
 /* Private functions ---------------------------------------------------------*/
@@ -504,13 +509,25 @@ INTERRUPT_HANDLER(TIM6_UPD_OVF_TRG_IRQHandler, 23)
   */
  INTERRUPT_HANDLER(TIM4_UPD_OVF_IRQHandler, 23)
  {
-  /* In order to detect unexpected events during development,
-     it is recommended to set a breakpoint on the following instruction.
-  */
-  /* Cleat Interrupt Pending bit */
-  TIM4->SR1 = (uint8_t)(~(uint8_t)TIM4_IT_UPDATE);
 
-  LedCnt ++;
+   /* Cleat Interrupt Pending bit */
+   TIM4->SR1 = (uint8_t)(~(uint8_t)TIM4_IT_UPDATE);
+
+   LedCnt ++;
+   if (LedCnt >= LED_ONE_PERIOD) {
+      LedCnt = 0;
+      led_OutputValue();
+   }
+   
+   /* Decrements the TimingDelay variable */
+   if (TimingDelay > 0) {
+      TimingDelay --;
+   }
+
+  /* for I2C */
+  if (I2C_timeout != 0) {
+    I2C_timeout --;
+  }
  }
 #endif /* (STM8S903) || (STM8AF622x)*/
 
@@ -530,5 +547,17 @@ INTERRUPT_HANDLER(EEPROM_EEC_IRQHandler, 24)
   * @}
   */
 
+/**
+  * @brief  Inserts a delay time.
+  * @param  nTime: specifies the delay time length, in milliseconds.
+  * @retval None
+  */
+void Delay(__IO uint16_t nTime)
+{
+  TimingDelay = nTime;
+  while (TimingDelay != 0) {
+      wfi();
+  }
+}
 
 /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/