stmbl

HAL_COMP(hv);

//process data from LS 
HAL_PIN(d_cmd) = 0.0;
HAL_PIN(q_cmd) = 0.0;
HAL_PIN(pos) = 0.0;
HAL_PIN(vel) = 0.0;
HAL_PIN(en) = 0.0;

// config data from LS
HAL_PIN(mode) = 1.0;
HAL_PIN(r) = 0.0;
HAL_PIN(l) = 0.0;
HAL_PIN(psi) = 0.0;
HAL_PIN(cur_p) = 0.0;
HAL_PIN(cur_i) = 0.0;
HAL_PIN(cur_ff) = 0.0;
HAL_PIN(cur_ind) = 0.0;
HAL_PIN(max_y) = 0.0;
HAL_PIN(max_cur) = 0.0;

// process data to LS
HAL_PIN(dc_volt) = 0.0;
HAL_PIN(d_fb) = 0.0;
HAL_PIN(q_fb) = 0.0;

// state data to LS
HAL_PIN(hv_temp) = 0.0;
HAL_PIN(mot_temp) = 0.0;
HAL_PIN(core_temp) = 0.0;
HAL_PIN(fault) = 0.0;
HAL_PIN(y) = 0.0;
HAL_PIN(u_fb) = 0.0;
HAL_PIN(v_fb) = 0.0;
HAL_PIN(w_fb) = 0.0;

// misc
HAL_PIN(polecount) = 1.0;
HAL_PIN(rev) = 0.0;
HAL_PIN(com_error) = 0.0;
HAL_PIN(pwm_volt) = 0.0;
HAL_PIN(uart_sr) = 0.0;
HAL_PIN(crc_error) = 0.0;

MEM(volatile packet_to_hv_t packet_to_hv);
MEM(volatile packet_from_hv_t packet_from_hv);

MEM(f3_config_data_t config);
MEM(f3_state_data_t state);
MEM(uint8_t addr) = 0;

INIT(
   //setup uart to f1. uses DMA to transfer to_hv struct.
   GPIO_InitTypeDef GPIO_InitStruct;
   USART_InitTypeDef USART_InitStruct;
   UART_DRV_CLOCK_COMMAND(UART_DRV_RCC, ENABLE);

   //USART TX
   GPIO_PinAFConfig(UART_DRV_TX_PORT, UART_DRV_TX_PIN_SOURCE, UART_DRV_TX_AF_SOURCE);
   GPIO_InitStruct.GPIO_Pin = UART_DRV_TX_PIN;
   GPIO_InitStruct.GPIO_Mode = GPIO_Mode_AF;
   GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
   GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
   GPIO_InitStruct.GPIO_PuPd = GPIO_PuPd_UP ;
   GPIO_Init(UART_DRV_TX_PORT, &GPIO_InitStruct);

   //USART RX
   GPIO_PinAFConfig(UART_DRV_RX_PORT, UART_DRV_RX_PIN_SOURCE, UART_DRV_RX_AF_SOURCE);
   GPIO_InitStruct.GPIO_Pin = UART_DRV_RX_PIN;
   GPIO_Init(UART_DRV_RX_PORT, &GPIO_InitStruct);

   USART_OverSampling8Cmd(UART_DRV,ENABLE);
   USART_InitStruct.USART_BaudRate = DATABAUD;
   USART_InitStruct.USART_WordLength = USART_WordLength_8b;
   USART_InitStruct.USART_StopBits = USART_StopBits_1;
   USART_InitStruct.USART_Parity = USART_Parity_No;
   USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
   USART_InitStruct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
   USART_Init(UART_DRV, &USART_InitStruct);

   /* Enable the USART */
   USART_Cmd(UART_DRV, ENABLE);

   // DMA-Disable
   DMA_Cmd(UART_DRV_TX_DMA, DISABLE);
   DMA_DeInit(UART_DRV_TX_DMA);

   // DMA2-Config
   DMA_InitStructure.DMA_Channel = UART_DRV_TX_DMA_CHAN;
   DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART_DRV->DR);
   DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&packet_to_hv;
   DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
   DMA_InitStructure.DMA_BufferSize = sizeof(packet_to_hv_t);
   DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
   DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
   DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
   DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
   DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
   DMA_InitStructure.DMA_Priority = DMA_Priority_High;
   DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
   DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
   DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
   DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
   DMA_Init(UART_DRV_TX_DMA, &DMA_InitStructure);

   //DMA_Cmd(UART_DRV_TX_DMA, ENABLE);

   USART_DMACmd(UART_DRV, USART_DMAReq_Tx, ENABLE);


  // DMA-Disable
  DMA_Cmd(UART_DRV_RX_DMA, DISABLE);
  DMA_DeInit(UART_DRV_RX_DMA);

  // DMA2-Config
  DMA_InitStructure.DMA_Channel = UART_DRV_RX_DMA_CHAN;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART_DRV->DR);
  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&packet_from_hv;
  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
  DMA_InitStructure.DMA_BufferSize = sizeof(packet_from_hv_t);
  DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
  DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable;
  DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_MemoryDataSize = DMA_PeripheralDataSize_Byte;
  DMA_InitStructure.DMA_Mode = DMA_Mode_Normal;
  DMA_InitStructure.DMA_Priority = DMA_Priority_High;
  DMA_InitStructure.DMA_FIFOMode = DMA_FIFOMode_Disable;
  DMA_InitStructure.DMA_FIFOThreshold = DMA_FIFOThreshold_HalfFull;
  DMA_InitStructure.DMA_MemoryBurst = DMA_MemoryBurst_Single;
  DMA_InitStructure.DMA_PeripheralBurst = DMA_PeripheralBurst_Single;
  DMA_Init(UART_DRV_RX_DMA, &DMA_InitStructure);


  USART_DMACmd(UART_DRV, USART_DMAReq_Rx, ENABLE);

  packet_to_hv.head.start = 255;
  packet_to_hv.head.key = 0;
  packet_from_hv.head.start = 0;
  packet_from_hv.head.key = 0;
)

RT(
   float e = PIN(en);
   float p = (int)MAX(PIN(polecount), 1.0);
   float pos = mod(PIN(pos) * p);
   float vel = PIN(vel) * p;
   float rev = 1.0;
   
   if(PIN(rev) > 0.0){//TODO: rev DC
      rev = -1.0;
   }
   
   config.pins.mode = PIN(mode);
   config.pins.r = PIN(r);
   config.pins.l = PIN(l);
   config.pins.psi = PIN(psi);
   config.pins.cur_p = PIN(cur_p);
   config.pins.cur_i = PIN(cur_i);
   config.pins.cur_ff = PIN(cur_ff);
   config.pins.cur_ind = PIN(cur_ind);
   config.pins.max_y = PIN(max_y);
   config.pins.max_cur = PIN(max_cur);

   if(packet_from_hv.head.start == 255){
      if(unbuff_packet((packet_header_t*)&packet_from_hv, sizeof(from_hv_t))){
         packet_from_hv.head.start = 0;
         PIN(d_fb) = packet_from_hv.data.d_fb;
         PIN(q_fb) = packet_from_hv.data.q_fb * rev;
         PIN(dc_volt) = packet_from_hv.data.dc_volt;
         PIN(pwm_volt) = packet_from_hv.data.pwm_volt;
      
         uint8_t a = packet_from_hv.data.addr;
         a = CLAMP(a, 0, sizeof(state) / 4);
         state.data[a] = packet_from_hv.data.value;
      
         PIN(u_fb) = state.pins.u_fb;
         PIN(v_fb) = state.pins.v_fb;
         PIN(w_fb) = state.pins.w_fb;
         PIN(hv_temp) = state.pins.hv_temp;
         PIN(mot_temp) = state.pins.mot_temp;
         PIN(core_temp) = state.pins.core_temp;
         PIN(fault) = state.pins.fault;
         PIN(y) = state.pins.y;
      
         PIN(com_error) = 0.0;//TODO: link to fault  
      }else{
         PIN(crc_error)++;
      }
   }
   else{
      PIN(com_error) = 1.0;
   }

   float d_cmd = PIN(d_cmd);
   float q_cmd = PIN(q_cmd) * rev;
  
  if(e > 0.0){
    packet_to_hv.data.d_cmd = d_cmd;
    packet_to_hv.data.q_cmd = q_cmd;
    packet_to_hv.data.enable = 1;
  }
  else{
    packet_to_hv.data.d_cmd = 0.0;
    packet_to_hv.data.q_cmd = 0.0;
    packet_to_hv.data.enable = 0;
  }
  packet_to_hv.data.pos = pos;
  packet_to_hv.data.vel = vel;
  packet_to_hv.data.addr = addr;
  packet_to_hv.data.value = config.data[addr++];
  addr %= sizeof(config) / 4;

  buff_packet((packet_header_t*)&packet_to_hv, sizeof(to_hv_t));

  //start DMA TX transfer
  DMA_Cmd(UART_DRV_TX_DMA, DISABLE);
  DMA_ClearFlag(UART_DRV_TX_DMA, UART_DRV_TX_DMA_TCIF);
  DMA_Cmd(UART_DRV_TX_DMA, ENABLE);

  //start DMA RX transfer
  DMA_Cmd(UART_DRV_RX_DMA, DISABLE);
  DMA_ClearFlag(UART_DRV_RX_DMA, UART_DRV_RX_DMA_TCIF);
  DMA_Cmd(UART_DRV_RX_DMA, ENABLE);

  PIN(uart_sr) = UART_DRV->SR;

  // PIN(power) = PIN(dc_cur) * PIN(dc_volt);
  // if(PIN(pwm_volt) > 0.0){
  //   PIN(dc_cur_sim) = ABS(PIN(iq)) / PIN(pwm_volt) * sqrtf(a*a + b*b)*0.5 + PIN(dc_cur_sim)*0.5;
  // }
  // if(ABS(a*b) > 0.01){
  //   PIN(ac_cur_sim) = PIN(dc_cur) / sqrtf(a*a+b*b) * PIN(pwm_volt);
  // }
);

ENDCOMP;