void SetupServo1()
{
    TIM2_PSCR = 0x03;       //  Prescaler = 4.
    TIM2_ARRH = 0xc3;       //  High byte of 50,000.
    TIM2_ARRL = 0x50;       //  Low byte of 50,000.
    TIM2_CCR1H = 0x0A;      //  High byte of 12,500
    TIM2_CCR1L = 0xCE;      //  Low byte of 12,500
    TIM2_CCER1_CC1P = 0;    //  Active high.
    TIM2_CCER1_CC1E = 1;    //  Enable compare mode for channel 1
    TIM2_CCMR1_OC1M = 6;    //  PWM Mode 1 - active if counter < CCR1, inactive otherwise.
    TIM2_CR1_CEN = 1;       //  Finally enable the timer.
}

void SetupServo2()
{
    TIM2_PSCR = 0x03;       //  Prescaler = 4.
    TIM2_ARRH = 0xc3;       //  High byte of 50,000.
    TIM2_ARRL = 0x50;       //  Low byte of 50,000.
    TIM2_CCR3H = 0x0A;      //  High byte of 12,500
    TIM2_CCR3L = 0xCE;      //  Low byte of 12,500
    TIM2_CCER1_CC2P = 0;    //  Active high.
    TIM2_CCER1_CC2E = 1;    //  Enable compare mode for channel 3?
    TIM2_CCMR2_OC2M = 6;    //  PWM Mode 1 - active if counter < CCR1, inactive otherwise.
    TIM2_CR1_CEN = 1;       //  Finally enable the timer.
}

    TIM2_CCR3H = 0x0A;      //  High byte of 12,500
    TIM2_CCR3L = 0xCE;      //  Low byte of 12,500
    TIM2_CCER2_CC3P = 0;    //  Active high.
    TIM2_CCER2_CC3E = 1;    //  Enable compare mode for channel 3
    TIM2_CCMR3_OC3M = 6;    //  PWM Mode 1 - active if counter < CCR1, inactive otherwise.
    TIM2_CR1_CEN = 1;       //  Finally enable the timer.

void servo1Write(unsigned char angle)
{
  if(angle > 180)       angle = 180;
 
  unsigned short temp = (SERVO_RIGHT - SERVO_LEFT) / 180;
  temp *= angle;
  temp += SERVO_LEFT;
 
  TIM2_CCR1H = temp >> 8;      //  High byte of 12,500
  TIM2_CCR1L = temp && 0xFF;      //  Low byte of 12,500
}

void servo2Write(unsigned char angle)
{
  if(angle > 180)       angle = 180;
 
  unsigned short temp = (SERVO_RIGHT - SERVO_LEFT) / 180;
  temp *= angle;
  temp += SERVO_LEFT;
 
  TIM2_CCR3H = temp >> 8;      //  High byte of 12,500
  TIM2_CCR3L = temp && 0xFF;      //  Low byte of 12,500
}

   
        MODULE  asmmain
       
#include "iostm8s103f3.h"

        PUBLIC  __iar_program_start
        PUBLIC  main

        EXTERN  CSTACK$$Limit

        SECTION `.near_func.text`:CODE:NOROOT(0)

        __iar_program_start:
       
 
//***************************************************************************8
main:
Reset:

        LDW     X, #CSTACK$$Limit-1     ; Set stackpointer
        LDW     SP, X
                                 
;************************************************************************88
; настройка тактирования
         mov CLK_CKDIVR,#0      ;выключить все делители тактовой частоты

;Копирование кода в озу
  //dc32 (0x82000000+Tim2OV)
 
 LDw X,#( loop)
 LDW Y,#0

CopyCodeRam:
 ld a,(x)
 ld (y),a
 incw X
 incw Y
 cpw X,#loopEnd
 jrne CopyCodeRam
 
;настройка порта
        mov PD_DDR,#MASK_PD_DDR_DDR4 //Pd4 выход
        mov PD_CR1,#MASK_PD_CR1_C14  ; пушпул
        mov PD_CR2,#MASK_PD_CR2_C24  ; 10MHz
       
       
 //----------
 //настраиваем таймер
 //mov CLK_PCKENR1,1<<5 //включение тактирования таймера 2
 //mov TIM2_CNTRH,#0xFF
 //mov TIM2_CNTRL,#0xF0
 mov TIM2_PSCR,#0x0a //максимальный предделитель
 mov TIM2_CR1,#MASK_TIM2_CR1_CEN //запускаем таймер
 mov TIM2_IER,#MASK_TIM2_IER_UIE //разрешение прерывания по переполнению 

  RIM //разрешаем прерывания

           
 //********************************************************************
 
 
 //мигание портом
 loop:
        ld A,PD_ODR
        xor A,#MASK_PD_ODR_ODR4
        ld PD_ODR,A
        jra loop 
  loopEnd:     
 //*****************************************************************************
//прерывание по переполнению таймера
Tim2OV:
 
  mov TIM2_SR1,#0x00 //сброс флагов таймера2

  ld a, Flag
  //если флаг 0 уйдем в ОЗУ
  jrne TR1
  //ld a,#0
  ld (0x09,SP),a  //запишем адрес возврата на ОЗУ
  ld (0x08,SP),a
  mov Flag,#1
  iret //выход и прерывания
 
 TR1: //если флаг 1 уйдем в флеш
  ld A,#(low loop)
  ld (0x09,SP),a
  ld A,#(high loop)
  ld (0x08,SP),a
 
  mov Flag,#0
  iret
 
 
//*****************************************************************************
        SECTION `.RAM_sect`:CODE:ROOT(0)  //****************************************
//*****************************************************************************       

//мигание портом из ОЗУ
 loopRAM:
        ld A,PD_ODR
        xor A,#MASK_PD_ODR_ODR4
        ld PD_ODR,A         
        JRT loopRAM 
  //----------
  nop
  Flag: db 0

//*****************************************************************************
//вектора прерываний
       SECTION `.Vectors_sect`:CODE:ROOT(0)
       
        DC32  0x82000000+Reset                   ;; reset
                 
   dc32 0x82000000+Reset   ; trap
       
   dc32 0x82000000+Reset   ; irq0
   dc32 0x82000000+Reset   ; irq1
   dc32 0x82000000+Reset   ; irq2
   dc32 0x82000000+Reset   ; irq3
   dc32 0x82000000+Reset   ; irq4
   dc32 0x82000000+Reset   ; irq5
   dc32 0x82000000+Reset   ; irq6
   dc32 0x82000000+Reset   ; irq7
   dc32 0x82000000+Reset   ; irq8
   dc32 0x82000000+Reset   ; irq9
   dc32 0x82000000+Reset   ; irq10
   dc32 0x82000000+Reset   ; irq11
   dc32 0x82000000+Reset   ; irq12
   dc32 (0x82000000+Tim2OV)   ; irq13   Переполнение обновление таймера 2
   dc32 0x82000000+Reset   ; irq14
   dc32 0x82000000+Reset   ; irq15
   dc32 0x82000000+Reset   ; irq16
   dc32 0x82000000+Reset   ; irq17
   dc32 0x82000000+Reset   ; irq18
   dc32 0x82000000+Reset   ; irq19
   dc32 0x82000000+Reset   ; irq20
   dc32 0x82000000+Reset   ; irq21
   dc32 0x82000000+Reset   ; irq22
   dc32 0x82000000+Reset   ; irq23
   dc32 0x82000000+Reset   ; irq24
   dc32 0x82000000+Reset   ; irq25
   dc32 0x82000000+Reset   ; irq26
   dc32 0x82000000+Reset   ; irq27
   dc32 0x82000000+Reset   ; irq28
   dc32 0x82000000+Reset   ; irq29
       
       
  //----------     
        END

/////////////////////////////////////////////////////////////////
//      Example ILINK command file for
//      STM8 IAR C/C++ Compiler and Assembler.
//
//      Copyright 2015 IAR Systems AB.
//
/////////////////////////////////////////////////////////////////


define memory with size = 16M;

define region Vectors = [from 0x8000 to 0x807f];
place at start of Vectors  { ro section .Vectors_sect };

define region RAM = [from 0x0000 to 0x03FF];
place at start of RAM  { ro section .RAM_sect };





define region TinyData = [from 0x00 to 0xFF];

define region NearData = [from 0x0000 to 0x03FF];

define region Eeprom = [from 0x4000 to 0x427F];

define region NearFuncCode = [from 0x8080 to 0x9FFF];

define region FarFuncCode = [from 0x8000 to 0x9FFF];

define region HugeFuncCode = [from 0x8000 to 0x9FFF];


/////////////////////////////////////////////////////////////////

define block CSTACK with size = _CSTACK_SIZE  {};

define block HEAP  with size = _HEAP_SIZE {};

define block INTVEC with size = 0x80 { ro section .intvec };

// Initialization
initialize by copy { rw section .far.bss,
                     rw section .far.data,
                     rw section .far_func.textrw,
                     rw section .huge.bss,
                     rw section .huge.data,
                     rw section .huge_func.textrw,
                     rw section .iar.dynexit,
                     rw section .near.bss,
                     rw section .near.data,
                     rw section .near_func.textrw,
                     rw section .tiny.bss,
                     rw section .tiny.data,
                     ro section .tiny.rodata
                   
                     
                    };

initialize by copy with packing = none {section __DLIB_PERTHREAD };

do not initialize  { rw section .eeprom.noinit,
                     rw section .far.noinit,
                     rw section .huge.noinit,
                     rw section .near.noinit,
                     rw section .tiny.noinit,
                     rw section .vregs };

// Placement
place at start of TinyData      { rw section .vregs };
place in TinyData               { rw section .tiny.bss,
                                  rw section .tiny.data,
                                  rw section .tiny.noinit,
                                  rw section .tiny.rodata };

place at end of NearData        { block CSTACK };
place in NearData               { block HEAP,
                                  rw section __DLIB_PERTHREAD,
                                  rw section .far.bss,
                                  rw section .far.data,
                                  rw section .far.noinit,
                                  rw section .far_func.textrw,
                                  rw section .huge.bss,
                                  rw section .huge.data,
                                  rw section .huge.noinit,
                                  rw section .huge_func.textrw,
                                  rw section .iar.dynexit,
                                  rw section .near.bss,
                                  rw section .near.data,
                                  rw section .near.noinit,
                                  rw section .near_func.textrw };

place at start of NearFuncCode  { block INTVEC };
place in NearFuncCode           { ro section __DLIB_PERTHREAD_init,
                                  ro section .far.data_init,
                                  ro section .far_func.textrw_init,
                                  ro section .huge.data_init,
                                  ro section .huge_func.textrw_init,
                                  ro section .iar.init_table,
                        ro section .init_array,
                                  ro section .near.data_init,
                                  ro section .near.rodata,
              ro section .near_func.text,
              ro section .near_func.textrw_init,
                                  ro section .tiny.data_init,
                                  ro section .tiny.rodata_init };

place in FarFuncCode            { ro section .far.rodata,
                                  ro section .far_func.text };

place in HugeFuncCode           { ro section .huge.rodata,
                                  ro section .huge_func.text };

place in Eeprom                 {    section .eeprom.noinit };

place in Eeprom                 {    section .eeprom.data };

place in Eeprom                 {    section .eeprom.rodata };

/////////////////////////////////////////////////////////////////