// Parasim - firmware
//   simulator paraglidingu -- hardware

// P. Wertheimer (wert.pa@gmail.com)
// 25.5.2011

/* HLAVICKOVE SOUBORY *******************************************************/

#include <ioavr.h>
#include <intrinsics.h>

/* DATOVE TYPY **************************************************************/

typedef unsigned char u8;
typedef signed char s8;
typedef unsigned short u16;
typedef signed short s16;
typedef unsigned long u32;
typedef signed long s32;

typedef enum{USB = 0, DEBUG = 1} Tusart; // vyber USARTU pro vypis

/* MAKRA ********************************************************************/

// hodinovy kmitocet mikrokontroleru [Hz]
#define MCU_FREQ 32000000

// slozi byte z jednotlivych bitu
#define BYTE(b7,b6,b5,b4,b3,b2,b1,b0) (((b7)<<7)|((b6)<<6)|((b5)<<5)|((b4)<<4)| \
  ((b3)<<3)|((b2)<<2)|((b1)<<1)|(b0))

// saturace promenne
#define SATURATE(var,lowlim,highlim) \
  { \
    if((var) < (lowlim)) \
      (var) = (lowlim); \
    else \
      if((var) > (highlim)) \
        (var) = (highlim); \
  }

// ovladani ledky na ukrajinske desticce
#define LED_ON()  {PORTQ.OUTSET = 1<<2;}
#define LED_OFF() {PORTQ.OUTCLR = 1<<2;}

// tlacitko na ukrajinske desticce
#define BUTTON (PORTQ.IN & (1<<3))

// cteni enkoderu
#define ENKODER_1     (TCD0.CNT)
#define ENKODER_2     (TCD1.CNT)

// cteni ADC - predtim musime ADC spustit !!!
#define SILA_1  (ADCA.CH0RES)
#define SILA_2  (ADCA.CH1RES)
#define PROUD_1 (ADCA.CH2RES)
#define PROUD_2 (ADCA.CH3RES)
#define AKCEL_X (ADCB.CH0RES)
#define AKCEL_Y (ADCB.CH1RES)
#define AKCEL_Z (ADCB.CH2RES)
#define NAPETI  (ADCB.CH3RES)

// stop tlacitko
#define STOP    ((PORTE.IN & (1<<2))?1:0)

// signaly z mustku
#define FLAGS   (PORTK.IN & 0x0F)

// signaly do mustku
#define RESET_ASSERT()   {PORTK.OUTSET = (1<<4) | (1<<5);}
#define RESET_DEASSERT() {PORTK.OUTCLR = (1<<4) | (1<<5);}

/* GLOBALNI PROMENNE ********************************************************/

volatile u8 out_of_sync = 0;
volatile s16 force = 0;
volatile s32 force_filtered = 0;
volatile s16 voltage = 0;
volatile u8 mode = 0; // 0 = prime rizeni motoru, 1 = P regulator sily

volatile s16 wish_1 = 0;
volatile s16 wish_2 = 0;
volatile u8 flagy = 0;
volatile u8 send_response = 0;

volatile s16 current = 0;
volatile s16 current_min = 0;
volatile s16 current_max = 0;
volatile s16 odchylka_ven = 0;
volatile s32 odchylka_filtered = 0;

volatile u16 prijato;
volatile u16 poslano;
volatile u16 prijato_zasec;
volatile u16 poslano_zasec;

// filtrovane vystupy z akcelerometru
volatile s32 akcel_x = 0;
volatile s32 akcel_y = 0;
volatile s32 akcel_z = 0;

/* PROTOTYPY ****************************************************************/

#pragma vector = TCE0_OVF_vect
__interrupt void regulator(void); // vypocet regulacni smycky

#pragma vector = USARTF0_RXC_vect
__interrupt void prijem(void); // prijat znak z USB
s16 prevod(u8* p); // precte 4 byte z bufferu a udela z nich s16

u8 __low_level_init(void); // inicializace

// vypis pres USART (prvni argument voli, ktery USART bude pouzit)
void putchar(Tusart usart, u8 c); // vysle znak z USARTu
void puthex(Tusart usart, u8 h); // prevede cislo na jeden znak sestnactkove soustavy a vysle
void puthex8(Tusart usart, u8 i); // prevede byte na dve hexacislice a vysle
void puthex16(Tusart usart, s16 i); // prevede s16 na ctyri hexacislice a vysle
void putu16(Tusart usart, u16 i); // vysle cislo typu u16 jako 5 cislic
void puts16(Tusart usart, s16 i); // vysle cislo typu s16 jako znamenko a 5 cislic
void putstring(Tusart usart, u8 *p); // vysle retezec znaku ukonceny znakem 0

// spusteni ADC (s cekanim na dokonceni prevodu)
void convert_A(void); // ADCA
void convert_B(void); // ADCB pro prvni akcelerometr

// nastaveni napeti na motorech
void motor_1(s16 voltage); // -511 <= voltage <= +511
void motor_2(s16 voltage);

/* OBSLUHY PRERUSENI ********************************************************/

#pragma vector = TCE0_OVF_vect
__interrupt void regulator(void)
{
  static u8 busy = 0;
  if(busy) // regulator se nestiha provest
    out_of_sync = 1;
  else // vypocet regulatoru
  {
    busy = 1;
    __enable_interrupt();

    // mereni sily a proudu - prumer osmi vzorku
    s16 suma = 0;
    s16 suma2 = 0;
    for(u8 i=8; i; i--)
    {
      convert_A();
      suma += SILA_1;
      suma2 += PROUD_1; 
    }
    force = suma>>3;
    force = force - 2000; // odecteni offsetu
    current = suma2>>3;
    current = current - 2250; // odecteni offsetu

    // cteni enkoderu
    s16 enkoder = ENKODER_1;

    // P regulator s filtrem pozadovane hodnoty
      s16 odchylka = wish_1 - force; // regulacni odchylka
      odchylka_filtered -= odchylka_filtered>>8; // low-pass filtr
      odchylka_filtered += ((s32)odchylka<<16)>>8;
      voltage = odchylka_filtered>>16; // P slozka
      odchylka_ven = odchylka_filtered>>16;

    // nastaveni motoru s sw dorazy
    if(enkoder >= 0)
    {
      if(voltage > 0)
        voltage = 0;
    }
    else
      if(enkoder <= -700)
        if(voltage < 0)
          voltage = 0;
    odchylka_ven = voltage;
    motor_1(voltage); // nastaveni pozadovane PWM

    // filtr na akcelerometry
    convert_B();
    akcel_x -= akcel_x>>11;
    akcel_x += ((s32)AKCEL_X<<16)>>11;
    akcel_y -= akcel_y>>11;
    akcel_y += ((s32)AKCEL_Y<<16)>>11;
    akcel_z -= akcel_z>>11;
    akcel_z += ((s32)AKCEL_Z<<16)>>11;
    
    busy = 0;
  }
}

// --------------------------------------------------------------------------

#pragma vector = USARTF0_RXC_vect
__interrupt void prijem(void)
{
  static u8 bytes_received = 255;
  static u8 buffer[10];
  u8 c = USARTF0.DATA;
  
  if(c == 'x') // uvodni znak - reset prijmoveho engine
    bytes_received = 0;
  else
    if(bytes_received < 10) // pridame znak do zpravy
    {
      if((c >= '0') && (c <= '9')) // je to cislice
      {
        buffer[bytes_received] = c - '0';
        bytes_received++;
      }
      else
        if((c >= 'a') && (c <= 'f')) // je to pismeno a az f
        {
          buffer[bytes_received] = c - 'a'+10;
          bytes_received++;
        }
        else // je to nejaka blbost nebo cokoliv jineho, napr. uplne jina blbost
          bytes_received = 255;
    }
  
  if(bytes_received == 10) // mame celou zpravu
  {
    wish_1 = prevod(&buffer[0]);
    wish_2 = prevod(&buffer[4]);
    flagy = __swap_nibbles(buffer[8]) | buffer[9];
    send_response = 1;
    bytes_received = 255;
    prijato++;
  }
}

s16 prevod(u8* p)
{
  s16 vysledek = __swap_nibbles(*p++)<<8;
  vysledek |= (*p++)<<8;
  vysledek |= __swap_nibbles(*p++);
  vysledek |= *p;
  return vysledek;
}

/* HLAVNI PROGRAM ***********************************************************/

void main(void)
{
  __enable_interrupt();
  putstring(DEBUG, "\n\n\rSpoustim regulaci.");
  PORTK.OUTSET = (1<<4) | (1<<5); // deaktivujeme resety H-mustku, tj. spustime mustek
  
  for(;;)
  {
    // opatrne cteni
    __disable_interrupt();
    s16 copy_of_wish_1 = wish_1;
    s16 copy_of_wish_2 = wish_2;
    s16 copy_of_akcel_x = akcel_x>>16;
    s16 copy_of_akcel_y = akcel_y>>16;
    s16 copy_of_akcel_z = akcel_z>>16;
    s16 copy_of_odchylka_ven = odchylka_ven;
    __enable_interrupt();

    
    // odeslani odpovedi do PC
    if(send_response)
    {
      send_response = 0;
      
      PORTQ.OUTTGL = 1<<2;
      
      putchar(USB,'x'); // uvodni znak
      puthex16(USB,ENKODER_1); // stazeni leve
      puthex16(USB,124); // stazeni prave - zatim nepouzito
      puthex16(USB,copy_of_akcel_x);
      puthex16(USB,copy_of_akcel_y);
      puthex16(USB,copy_of_akcel_z);
      poslano++;
    }

  }
}

/* INICIALIZACE *************************************************************/

u8 __low_level_init(void)
{
  // prepnuti hodin na 32 MHz, odvozenych z krystalu 8 MHz pomoci PLL
  OSC.XOSCCTRL = OSC_FRQRANGE_2TO9_gc | OSC_XOSCSEL_XTAL_16KCLK_gc; // konfigurace krystaloveho oscilatoru
  OSC.CTRL |= OSC_XOSCEN_bm; // nahodime krystalovy oscilator
  OSC.PLLCTRL = OSC_PLLSRC_XOSC_gc | 4; // konfigurace PLL - nasobeni 4x
  while(!(OSC.STATUS & OSC_XOSCRDY_bm)); // pockej na stabilizaci krystaloveho oscilatoru
  OSC.CTRL |= OSC_PLLEN_bm; // nahodime PLL
  while(!(OSC.STATUS & OSC_PLLRDY_bm)); // pockej na stabilizaci PLL
  CCP = 0xD8; // povol zapis
  CLK.CTRL = CLK_SCLKSEL_PLL_gc; // prepnuti na PLL
  __delay_cycles(10); // par cyklu na provedeni prepnuti
  OSC.CTRL &= ~OSC_RC2MEN_bm; // vypneme interni oscilator

  // PORTQ - ledka a tlacitko primo na ukrajinske desticce
  PORTQ.PIN0CTRL = PORT_OPC_PULLUP_gc; // na piny 0, 1 pripojen nevyuzity krystal 32768 Hz - vstupy s pull-upem
  PORTQ.PIN1CTRL = PORT_OPC_PULLUP_gc;
  PORTQ.PIN2CTRL = PORT_INVEN_bm; // invertujeme vystup pro LED
  PORTQ.PIN3CTRL = PORT_INVEN_bm; // invertujeme vstup pro tlacitko
  PORTQ.DIRSET = 1<<2; // vystup pro LED

  // enkoder 1 - PD0+1, event channel 0, TCD0
  PORTD.PIN0CTRL |= PORT_ISC_LEVEL_gc;
  PORTD.PIN1CTRL |= PORT_ISC_LEVEL_gc;
  EVSYS.CH0MUX = EVSYS_CHMUX_PORTD_PIN0_gc;
  EVSYS.CH0CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_8SAMPLES_gc;
  TCD0.CTRLD = TC_EVACT_QDEC_gc | TC_EVSEL_CH0_gc;
  TCD0.CTRLA = TC_CLKSEL_DIV1_gc;

  // enkoder 2 - PD2+3, event channel 2, TCD1
  PORTD.PIN2CTRL |= PORT_ISC_LEVEL_gc;
  PORTD.PIN3CTRL |= PORT_ISC_LEVEL_gc;
  EVSYS.CH2MUX = EVSYS_CHMUX_PORTD_PIN2_gc;
  EVSYS.CH2CTRL = EVSYS_QDEN_bm | EVSYS_DIGFILT_8SAMPLES_gc;
  TCD1.CTRLD = TC_EVACT_QDEC_gc|TC_EVSEL_CH2_gc;
  TCD1.CTRLA = TC_CLKSEL_DIV1_gc;

  // USB USART @ 115200bps, 8, 1, N
  PORTF.PIN6CTRL = PORT_OPC_PULLUP_gc; // pull-up na RXD pinu
  PORTF.DIRSET = 1<<3; // nastavime TXD pin jako vystupni
  USARTF0.BAUDCTRLA = 131; // BSEL = 131
  USARTF0.BAUDCTRLB = (u8)((-3) << USART_BSCALE_gp); // BSCALE = -3
  USARTF0.CTRLA = USART_RXCINTLVL_LO_gc;
  USARTF0.CTRLC = USART_CHSIZE_8BIT_gc; // 8 datovych bitu
  USARTF0.CTRLB = USART_RXEN_bm | USART_TXEN_bm;

  // debugovaci USART @ 115200bps, 8, 1, N
  PORTF.PIN6CTRL = PORT_OPC_PULLUP_gc; // pull-up na RXD pinu
  PORTF.DIRSET = 1<<7; // nastavime TXD pin jako vystupni
  USARTF1.BAUDCTRLA = 131; // BSEL = 131
  USARTF1.BAUDCTRLB = (u8)((-3) << USART_BSCALE_gp); // BSCALE = -3
  USARTF1.CTRLC = USART_CHSIZE_8BIT_gc; // 8 datovych bitu
  USARTF1.CTRLB = USART_RXEN_bm | USART_TXEN_bm;

  // ADCA - mereni sily a proudu
  ADCA.REFCTRL = ADC_REFSEL_AREFA_gc;
  ADCA.PRESCALER = ADC_PRESCALER_DIV16_gc;
  ADCA.CTRLA = ADC_ENABLE_bm;
  ADCA.CH0.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCA.CH0.MUXCTRL = ADC_CH_MUXPOS_PIN1_gc; // sila 1
  ADCA.CH1.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCA.CH1.MUXCTRL = ADC_CH_MUXPOS_PIN2_gc; // sila 2
  ADCA.CH2.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCA.CH2.MUXCTRL = ADC_CH_MUXPOS_PIN3_gc; // proud 1
  ADCA.CH3.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCA.CH3.MUXCTRL = ADC_CH_MUXPOS_PIN4_gc; // proud 2

  // ADCB - mereni naklonu a napeti meziobvodu
  ADCB.REFCTRL = ADC_REFSEL_AREFA_gc;
  ADCB.PRESCALER = ADC_PRESCALER_DIV16_gc;
  ADCB.CTRLA = ADC_ENABLE_bm;
  ADCB.CH0.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc; // multiplexer pro kanaly 0 az 3 nastavime pred spustenim prevodu
  ADCB.CH0.MUXCTRL = ADC_CH_MUXPOS_PIN0_gc; // prvni akcelerometr, osa X
  ADCB.CH1.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCB.CH1.MUXCTRL = ADC_CH_MUXPOS_PIN1_gc; // prvni akcelerometr, osa Y
  ADCB.CH2.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCB.CH2.MUXCTRL = ADC_CH_MUXPOS_PIN2_gc; // prvni akcelerometr, osa Z
  ADCB.CH3.CTRL = ADC_CH_INPUTMODE_SINGLEENDED_gc;
  ADCB.CH3.MUXCTRL = ADC_CH_MUXPOS_PIN6_gc; // napeti meziobvodu
    
  // Self-test akcelerometru
  PORTJ.DIRSET = (1<<0) | (1<<1);
  // PORTJ.OUTCLR = (1<<0) | (1<<1); // ma to byt vypnute nebo zapnute ? normalne v 0 pro ST nahodit

  // TCC1 - PWM pro motor 1 - 15625 Hz na výstupu z Xmegy, 31250 Hz na motoru (diky unipolarni modulaci)
  PORTC.DIRSET = (1<<4) | (1<<5);
  //PORTC.PIN5CTRL = PORT_INVEN_bm; // TODO pouze pro bipolarni PWM
  TCC1.CTRLB = TC1_CCBEN_bm | TC1_CCAEN_bm | TC_WGMODE_DS_T_gc; // dual-slope
  TCC1.PER = 1024-1; // rozliseni 10 bitu
  TCC1.CCA = 511; // init stridy
  TCC1.CCB = 511;
  TCC1.CTRLA = TC_CLKSEL_DIV1_gc; // spusteni

  // TCE1 - PWM pro motor 2 -- zatim nepouzivame
  PORTC.DIRSET = (1<<4) | (1<<5);
  TCE1.CTRLB = TC1_CCBEN_bm | TC1_CCAEN_bm | TC_WGMODE_DS_T_gc; // dual-slope
  TCE1.PER = 1024-1; // rozliseni 10 bitu
  TCE1.CCA = 511; // init stridy
  TCE1.CCB = 511;
  TCE1.CTRLA = TC_CLKSEL_DIV1_gc; // spusteni

  // aktivujeme resety pro H-mustky, tj. mustek je vypnuty
  PORTK.DIRSET = (1<<4) | (1<<5); // proc to je az za rozjetim PWM ? prehodit

  // interrupt controller
  PMIC.CTRL = PMIC_HILVLEN_bm | PMIC_MEDLVLEN_bm | PMIC_LOLVLEN_bm;

  // TCE0 - spousteni vypoctu regulatoru s frekvenci 5 kHz
  TCE0.PER = 6400-1;
  TCE0.INTCTRLA = TC_OVFINTLVL_MED_gc;
  TCE0.CTRLA = TC_CLKSEL_DIV1_gc; // spusteni

  // TODO: pull-upy na nevyuzite piny
  // TODO: beh generatoru PWM v protifazi
  // TODO: mereni napeti meziobvodu
  
  return 1;
}

/* OBSLUHA USARTU ***********************************************************/

void putchar(Tusart usart, u8 c)
{
  if(usart == USB)
  {
    while(!(USARTF0.STATUS & USART_DREIF_bm));
    USARTF0.DATA = c;
  }
  else // tj. usart == DEBUG
  {
    while(!(USARTF1.STATUS & USART_DREIF_bm));
    USARTF1.DATA = c;
  }
}

// --------------------------------------------------------------------------

void puthex(Tusart usart, u8 h)
{
  h += '0';
  if(h>'9')
    h += 'a'-'9'-1;
  putchar(usart, h);
}

// --------------------------------------------------------------------------

void puthex8(Tusart usart, u8 i)
{
  puthex(usart, __swap_nibbles(i) & 0x0F);
  puthex(usart, i & 0x0F);
}

// --------------------------------------------------------------------------

void puthex16(Tusart usart, s16 i)
{
  puthex8(usart,i>>8);
  puthex8(usart,i);
}

// --------------------------------------------------------------------------

void putu16(Tusart usart, u16 i)
{
  // desetitisice
  u8 n = '0';
  while(i >= 10000)
    n++,i-=10000;
  putchar(usart, n);
  // tisice
  n = '0';
  while(i >= 1000)
    n++,i-=1000;
  putchar(usart, n);
  // stovky
  n = '0';
  while(i >= 100)
    n++,i-=100;
  putchar(usart, n);
  // desitky a jednotky
  n = '0';
  u8 iz = i; // i je ted zarucene mensi jak 100
  while(iz >= 10)
    n++,iz-=10;
  putchar(usart, n);
  putchar(usart, iz+'0');
}

// --------------------------------------------------------------------------

void puts16(Tusart usart, s16 i)
{
  if(i<0)
  {
    putchar(usart, '-');
    putu16(usart, -i);
  }
  else
  {
    putchar(usart, '+');
    putu16(usart, i);
  }
}

// --------------------------------------------------------------------------

void putstring(Tusart usart, u8 *p)
{
  u8 *copy_of_p = p;
  u8 c;
  while(c = *copy_of_p++)
    putchar(usart, c);
}

/* OBSLUHA PREVODNIKU *******************************************************/

void convert_A(void)
{
  ADCA.INTFLAGS |= ADC_CH0IF_bm | ADC_CH1IF_bm | ADC_CH2IF_bm | ADC_CH3IF_bm; // vymaz priznaky
  ADCA.CTRLA |= ADC_CH0START_bm | ADC_CH1START_bm | ADC_CH2START_bm | ADC_CH3START_bm; // spust prevod
  while(ADCA.INTFLAGS != (ADC_CH0IF_bm | ADC_CH1IF_bm | ADC_CH2IF_bm | ADC_CH3IF_bm)); // cekej na dokonceni prevodu
}

// --------------------------------------------------------------------------

void convert_B(void)
{
  ADCB.INTFLAGS |= ADC_CH0IF_bm | ADC_CH1IF_bm | ADC_CH2IF_bm | ADC_CH3IF_bm; // vymaz priznaky
  ADCB.CTRLA |= ADC_CH0START_bm | ADC_CH1START_bm | ADC_CH2START_bm | ADC_CH3START_bm; // spust prevod
  while(ADCB.INTFLAGS != (ADC_CH0IF_bm | ADC_CH1IF_bm | ADC_CH2IF_bm | ADC_CH3IF_bm)); // cekej na dokonceni prevodu
}

/* NASTAVENI GENERATORU PWM *************************************************/

void motor_1(s16 voltage)
{
  SATURATE(voltage,-511,511);
  TCC1.CTRLFSET = TC1_LUPD_bm;
  TCC1.CCABUF = 511+voltage; // unipolarni PWM
  TCC1.CCBBUF = 511-voltage;
  TCC1.CTRLFCLR = TC1_LUPD_bm;
}

// --------------------------------------------------------------------------

void motor_2(s16 voltage)
{
  SATURATE(voltage,-511,511);
  TCE1.CTRLFSET = TC1_LUPD_bm;
  TCE1.CCABUF = 511+voltage; // unipolarni PWM
  TCE1.CCBBUF = 511-voltage;
  TCE1.CTRLFCLR = TC1_LUPD_bm;
}
