stmbl

#include "commands.h"
#include "hal.h"
#include "math.h"
#include "defines.h"
#include "angle.h"
#include "stm32f4xx_conf.h"
#include "hw/hw.h"
#include "yaskawa_crc16.h"

HAL_COMP(yaskawa);

HAL_PIN(pos);
HAL_PIN(error);
HAL_PIN(error_sum);
HAL_PIN(dump);
HAL_PIN(len);
HAL_PIN(off);

HAL_PIN(len2);
HAL_PIN(off2);
HAL_PIN(probe2);

HAL_PIN(len3);
HAL_PIN(len4);
HAL_PIN(len5);

HAL_PIN(probe3);
HAL_PIN(probe4);
HAL_PIN(probe5);

HAL_PIN(send);
HAL_PIN(crc_ok);
HAL_PIN(crc_error);

//TODO: use context
volatile uint32_t txbuf[128];
int pos;
int dfdf;
volatile char m_data[150];
volatile char m_data2[150];
volatile uint16_t tim_data[300];
DMA_InitTypeDef DMA_InitStructuretx;
DMA_InitTypeDef DMA_InitStructurerx;
uint8_t yaskawa_reply[14];
//uint8_t yaskara_reply_len;

static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
  // struct yaskawa_ctx_t *ctx = (struct yaskawa_ctx_t *)ctx_ptr;
  struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;

  PIN(len) = 15;
  PIN(off) = 64;
  PIN(len3) = 57;
  PIN(len4) = 58;
  PIN(len5) = 59;
}

static void hw_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
  // struct yaskawa_ctx_t *ctx = (struct yaskawa_ctx_t *)ctx_ptr;
  // struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
  
  GPIO_InitTypeDef GPIO_InitStruct;

  //TX enable
  GPIO_InitStruct.GPIO_Pin   = FB0_Z_TXEN_PIN;
  GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_Init(FB0_Z_TXEN_PORT, &GPIO_InitStruct);
  GPIO_ResetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);

  //TX
  GPIO_InitStruct.GPIO_Pin   = FB0_Z_PIN;
  GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_OUT;
  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_Init(FB0_Z_PORT, &GPIO_InitStruct);
  GPIO_PinAFConfig(FB0_Z_PORT, FB0_Z_PIN_SOURCE, FB0_ENC_TIM_AF);

  RCC_APB1PeriphClockCmd(FB0_ENC_TIM_RCC, ENABLE);


  //manchaster
  //0 -> 01
  //1 -> 10
  //SYNC                framing                         framing
  //0101010101010101010 01111110 111110 111110 111110 1 01111110;
  //010101... are to sync receiver
  //01111110 is hdlc start and end flag for framing
  //0 is appended after 5 ones => transmitted data is 0xffff
  //reply has the same format
  const char request[] = "0101010101010101010 01111110 111110 111110 111110 1 01111110";
  char request_m[sizeof(request) * 2];
  uint32_t tim_a = FB0_Z_PIN << 16;
  uint32_t tim_b = FB0_Z_PIN;

  //encode in manchaster
  int j = 0;
  for(int i = 0; request[i]; i++) {
    if(request[i] == '0') {
      request_m[j++] = '0';
      request_m[j++] = '1';
    } else if(request[i] == '1') {
      request_m[j++] = '1';
      request_m[j++] = '0';
    }
  }
  request_m[j] = '\0';

  //build txbuf for dma
  for(int i = 0; request_m[i]; i++) {
    if(request_m[i] == '0') {
      txbuf[pos++] = tim_a;
    } else if(request_m[i] == '1') {
      txbuf[pos++] = tim_b;
    }
  }
  txbuf[pos++] = tim_a;
  txbuf[pos++] = tim_a;

  DMA_InitStructuretx.DMA_Channel            = DMA_Channel_7;  
  DMA_InitStructuretx.DMA_PeripheralBaseAddr = (uint32_t)&FB0_Z_PORT->BSRRL;  //TODO: change
  DMA_InitStructuretx.DMA_Memory0BaseAddr    = (uint32_t)&txbuf;
  DMA_InitStructuretx.DMA_DIR                = DMA_DIR_MemoryToPeripheral;
  DMA_InitStructuretx.DMA_BufferSize         = pos;
  DMA_InitStructuretx.DMA_PeripheralInc      = DMA_PeripheralInc_Disable;
  DMA_InitStructuretx.DMA_MemoryInc          = DMA_MemoryInc_Enable;
  DMA_InitStructuretx.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word;
  DMA_InitStructuretx.DMA_MemoryDataSize     = DMA_PeripheralDataSize_Word;
  DMA_InitStructuretx.DMA_Mode               = DMA_Mode_Normal;
  DMA_InitStructuretx.DMA_Priority           = DMA_Priority_VeryHigh;
  DMA_InitStructuretx.DMA_FIFOMode           = DMA_FIFOMode_Disable;
  DMA_InitStructuretx.DMA_FIFOThreshold      = DMA_FIFOThreshold_HalfFull;
  DMA_InitStructuretx.DMA_MemoryBurst        = DMA_MemoryBurst_Single;
  DMA_InitStructuretx.DMA_PeripheralBurst    = DMA_PeripheralBurst_Single;

  DMA_InitStructurerx.DMA_Channel            = DMA_Channel_2;  
  DMA_InitStructurerx.DMA_PeripheralBaseAddr = (uint32_t)&FB0_ENC_TIM->CCR3;  //TODO: change
  DMA_InitStructurerx.DMA_Memory0BaseAddr    = (uint32_t)&tim_data;
  DMA_InitStructurerx.DMA_DIR                = DMA_DIR_PeripheralToMemory;
  DMA_InitStructurerx.DMA_BufferSize         = ARRAY_SIZE(tim_data);
  DMA_InitStructurerx.DMA_PeripheralInc      = DMA_PeripheralInc_Disable;
  DMA_InitStructurerx.DMA_MemoryInc          = DMA_MemoryInc_Enable;
  DMA_InitStructurerx.DMA_PeripheralDataSize = DMA_PeripheralDataSize_HalfWord;
  DMA_InitStructurerx.DMA_MemoryDataSize     = DMA_PeripheralDataSize_HalfWord;
  DMA_InitStructurerx.DMA_Mode               = DMA_Mode_Normal;
  DMA_InitStructurerx.DMA_Priority           = DMA_Priority_VeryHigh;
  DMA_InitStructurerx.DMA_FIFOMode           = DMA_FIFOMode_Disable;
  DMA_InitStructurerx.DMA_FIFOThreshold      = DMA_FIFOThreshold_HalfFull;
  DMA_InitStructurerx.DMA_MemoryBurst        = DMA_MemoryBurst_Single;
  DMA_InitStructurerx.DMA_PeripheralBurst    = DMA_PeripheralBurst_Single;

  RCC_APB2PeriphClockCmd(RCC_APB2Periph_TIM8, ENABLE);
  TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure;
  TIM_TimeBaseStructure.TIM_ClockDivision     = TIM_CKD_DIV1;
  TIM_TimeBaseStructure.TIM_CounterMode       = TIM_CounterMode_Up;
  TIM_TimeBaseStructure.TIM_Period            = 20;  // 168 / (20 + 1) = 8MHz
  TIM_TimeBaseStructure.TIM_Prescaler         = 0;
  TIM_TimeBaseStructure.TIM_RepetitionCounter = 0;
  TIM_TimeBaseInit(TIM8, &TIM_TimeBaseStructure);
  TIM_ARRPreloadConfig(TIM8, ENABLE);
  TIM_DMACmd(TIM8, TIM_DMA_Update, ENABLE);
  TIM_Cmd(TIM8, ENABLE);

  FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN; 
  FB0_ENC_TIM->ARR = 65535;
  FB0_ENC_TIM->CNT = 3300;
  FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
  
  DMA_Cmd(DMA1_Stream7, DISABLE);
  DMA_DeInit(DMA1_Stream7);
  DMA_Init(DMA1_Stream7, &DMA_InitStructurerx);

  DMA_Cmd(DMA2_Stream1, DISABLE);
  DMA_DeInit(DMA2_Stream1);
  DMA_Init(DMA2_Stream1, &DMA_InitStructuretx);
  dfdf = 0;
}

static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
  // struct yaskawa_ctx_t *ctx      = (struct yaskawa_ctx_t *)ctx_ptr;
  struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
 
  while(FB0_ENC_TIM->CNT < 3300){

  }

  int count = ARRAY_SIZE(tim_data) - DMA1_Stream7->NDTR;
  DMA_Cmd(DMA1_Stream7, DISABLE);

  uint16_t bit_time = 15;
 
  if(count > 80){
    int pol = 0;
    int read_counter = 0;
    int counter = 0;

    for(int i = 0;i < ARRAY_SIZE(yaskawa_reply);i++){
      yaskawa_reply[i] = 0;
    }

    for(int i = 1; i < count; i++){
      if(tim_data[i + 1] - tim_data[i] < bit_time){
        counter++;
      }
      else if(counter == 10){
        read_counter = i + 1;
        pol = 0;
        break;
      }
      else{
        counter = 0;
      }
    }

    int write_counter2 = 0;
    for(int i = read_counter; i < count; i++){
      if(tim_data[i + 1] - tim_data[i] < bit_time){
        i++;
        if(tim_data[i + 1] - tim_data[i] < bit_time){
          //data[write_counter++] = '0' + pol;
        }
        else{
          //error
          PIN(error) = 1.0;
          break;
        }

      }
      else{
        pol = 1 - pol;
        //data[write_counter++] = '0' + pol;
      }

      if(pol == 1){
        counter++;
        m_data[write_counter2] = '1';
        yaskawa_reply[write_counter2/8] |= 1 << (7-(write_counter2%8));
        write_counter2++;
      }
      else if(counter == 5){
        counter = 0;
        // unstuff
      }
      else if(counter == 6){
        // hldc
        m_data[write_counter2++] = 'H';
        break;
      }
      else{
        counter = 0;
        m_data[write_counter2++] = '0';
      }
    }

    yaskawa_crc16_t crc;
    yaskawa_crc16_t data = (yaskawa_reply[13] & 0xff) | (yaskawa_reply[12] << 8);
    crc = yaskawa_crc16_init();
    crc = yaskawa_crc16_update(crc, yaskawa_reply, 12);
    crc = yaskawa_crc16_finalize(crc);
    if(data == crc){
      PIN(crc_ok)++;
    }else{
      PIN(crc_error)++;
    }

    m_data[write_counter2] = 0;

    uint32_t pos = 0;
    //extract position data
    for(int i = 0; i < PIN(len); i++) {
      pos += (m_data[i + (int)PIN(off)] == '1') << i;
    }

    uint32_t probe = 0;
    for(int i = 0; i < PIN(len2); i++) {
      probe += (m_data[i + (int)PIN(off2)] == '1') << i;
    }
    
    PIN(pos)   = (float)pos / (float)(1 << (int)PIN(len)) * M_PI * 2.0 - M_PI;
    PIN(probe2) = probe;

    PIN(probe3) = m_data[(int)PIN(len3)] == '1';
    PIN(probe4) = m_data[(int)PIN(len4)] == '1';
    PIN(probe5) = m_data[(int)PIN(len5)] == '1';
    PIN(error) = 0.0;

    if(dfdf < 1){
      for(int i = 0; i < ARRAY_SIZE(m_data2); i++){
        m_data2[i] = m_data[i];
      }
      dfdf = 1;
    }
  }
  else{
    PIN(error) = 1.0;
    // error
  }  

  // PIN(send) = send;

  FB0_Z_TXEN_PORT->BSRRL = FB0_Z_TXEN_PIN;  //TX enable
  FB0_Z_PORT->MODER &= ~GPIO_MODER_MODER14_1;
  FB0_Z_PORT->MODER |= GPIO_MODER_MODER14_0;  //set tx pin to output

  DMA_Cmd(DMA2_Stream1, DISABLE);
  DMA_DeInit(DMA2_Stream1);
  DMA_Init(DMA2_Stream1, &DMA_InitStructuretx);
  DMA_ClearFlag(DMA2_Stream1, DMA_FLAG_TCIF7);
  DMA_Cmd(DMA2_Stream1, ENABLE);  //transmit request

  TIM8->CR1 &= ~TIM_CR1_CEN; // disable tim
  TIM8->ARR = 20; // 168 / 2 / (9 + 1) = 8.4MHz
  TIM8->DIER = TIM_DIER_UDE; // cc3 dma
  // TIM8->CCMR2 = 0; // cc3 output
  // TIM8->CCR3 = 1;
  TIM8->CNT = 0;
  TIM8->CR1 |= TIM_CR1_CEN; 

  DMA_Cmd(DMA1_Stream7, DISABLE);
  DMA_ClearFlag(DMA1_Stream7, DMA_FLAG_TCIF7);
  DMA_Cmd(DMA1_Stream7, ENABLE);

  FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN; 
  FB0_ENC_TIM->CCMR2 = TIM_CCMR2_CC3S_0; // cc3 input ti3
  FB0_ENC_TIM->CCER = TIM_CCER_CC3E | TIM_CCER_CC3P | TIM_CCER_CC3NP; // cc3 en, rising edge, falling edge
  FB0_ENC_TIM->ARR = 65535;
  FB0_ENC_TIM->DIER = TIM_DIER_CC3DE; // cc3 dma
  FB0_ENC_TIM->CNT = 0;
  FB0_ENC_TIM->CCR3 = 0;

  while(!(DMA2->LISR & DMA_FLAG_TCIF1));  //wait for request
  
  FB0_Z_TXEN_PORT->BSRRH = FB0_Z_TXEN_PIN;  //TX disable
  FB0_Z_PORT->MODER &= ~GPIO_MODER_MODER14_0;  //set tx pin to af
  FB0_Z_PORT->MODER |= GPIO_MODER_MODER14_1;

  FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim

  if(PIN(error) > 0.0){
    PIN(error_sum)++;
  }
}

static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
  // struct yaskawa_ctx_t *ctx      = (struct yaskawa_ctx_t *)ctx_ptr;
  struct yaskawa_pin_ctx_t *pins = (struct yaskawa_pin_ctx_t *)pin_ptr;
  if(RISING_EDGE(PIN(dump))) {
    for(int i = 0; i < 14; i++){
      for(int j = 0; j < 8; j++){
        printf("%c", m_data2[i * 8 + j]);
      }
      printf("|");
    }
    printf("\n");
    dfdf = 0;
  }
}

hal_comp_t yaskawa_comp_struct = {
    .name      = "yaskawa",
    .nrt       = nrt_func,
    .rt        = rt_func,
    .frt       = 0,
    .nrt_init  = nrt_init,
    .hw_init   = hw_init,
    .rt_start  = 0,
    .frt_start = 0,
    .rt_stop   = 0,
    .frt_stop  = 0,
    .ctx_size  = 0,
    .pin_count = sizeof(struct yaskawa_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
};