stmbl

HAL_COMP(hyper);// https://www.youtube.com/watch?v=aoXOdAUD7IM&t=44

HAL_PIN(pos) = 0.0;
HAL_PIN(error) = 0;
HAL_PIN(status) = -1;
HAL_PIN(read_status) = 0;

MEM(volatile uint8_t rxbuf[128]);
MEM(volatile uint8_t txbuf[20]);

INIT(
  GPIO_InitTypeDef GPIO_InitStruct;
  USART_InitTypeDef USART_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);

  FB0_UART_CLOCK_COMMAND(FB0_UART_RCC, ENABLE);

  //USART TX
  GPIO_PinAFConfig(FB0_UART_TX_PORT, FB0_UART_TX_PIN_SOURCE, FB0_UART_TX_AF_SOURCE);
  GPIO_InitStruct.GPIO_Pin = FB0_UART_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(FB0_UART_TX_PORT, &GPIO_InitStruct);

  //USART RX
#ifdef FB0_UART_RX_PORT
  GPIO_PinAFConfig(FB0_UART_RX_PORT, FB0_UART_RX_PIN_SOURCE, FB0_UART_RX_AF_SOURCE);
  GPIO_InitStruct.GPIO_Pin = FB0_UART_RX_PIN;
  GPIO_Init(FB0_UART_RX_PORT, &GPIO_InitStruct);
#endif
  USART_InitStruct.USART_BaudRate = 9600;
  USART_InitStruct.USART_WordLength = USART_WordLength_9b;
  USART_InitStruct.USART_StopBits = USART_StopBits_1;
  USART_InitStruct.USART_Parity = USART_Parity_Even;
  USART_InitStruct.USART_HardwareFlowControl = USART_HardwareFlowControl_None;
  USART_InitStruct.USART_Mode = USART_Mode_Rx | USART_Mode_Tx;
  USART_Init(FB0_UART, &USART_InitStruct);

  /* Enable the USART */
  USART_Cmd(FB0_UART, ENABLE);
#ifndef FB0_UART_RX_PORT
  USART_HalfDuplexCmd(FB0_UART,ENABLE);
#endif
  //TX DMA

  DMA_Cmd(FB0_UART_TX_DMA, DISABLE);
  DMA_DeInit(FB0_UART_TX_DMA);

  // DMA2-Config
  DMA_InitStructure.DMA_Channel = FB0_UART_TX_DMA_CHAN;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(FB0_UART->DR);
  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&txbuf;
  DMA_InitStructure.DMA_DIR = DMA_DIR_MemoryToPeripheral;
  DMA_InitStructure.DMA_BufferSize = sizeof(txbuf);
  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_Priority_Low;
  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(FB0_UART_TX_DMA, &DMA_InitStructure);
  USART_DMACmd(FB0_UART, USART_DMAReq_Tx, ENABLE);

  //RX DMA

  DMA_Cmd(FB0_UART_RX_DMA, DISABLE);
  DMA_DeInit(FB0_UART_RX_DMA);

  // DMA2-Config
  DMA_InitStructure.DMA_Channel = FB0_UART_RX_DMA_CHAN;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(FB0_UART->DR);
  DMA_InitStructure.DMA_Memory0BaseAddr = (uint32_t)&rxbuf;
  DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralToMemory;
  DMA_InitStructure.DMA_BufferSize = sizeof(rxbuf);
  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_Low;
  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(FB0_UART_RX_DMA, &DMA_InitStructure);
  USART_DMACmd(FB0_UART, USART_DMAReq_Rx, ENABLE);
);

void start_tx(uint32_t len){
   DMA_SetCurrDataCounter(FB0_UART_TX_DMA,len);
   DMA_Cmd(FB0_UART_TX_DMA, DISABLE);
   DMA_ClearFlag(FB0_UART_TX_DMA, FB0_UART_TX_DMA_TCIF);
   DMA_Cmd(FB0_UART_TX_DMA, ENABLE);
}

void start_rx(){
   DMA_Cmd(FB0_UART_RX_DMA, DISABLE);
   DMA_ClearFlag(FB0_UART_RX_DMA, FB0_UART_RX_DMA_TCIF);
   DMA_Cmd(FB0_UART_RX_DMA, ENABLE);
}

RT(
   int available;
   uint32_t pos = 0;
   //const float byte_time = 11.0/9600.0; //TODO: timeouts/sleep based on this?

   HT(
   GOTO(0);
   STATE(0){//reset
      txbuf[0] = 0xff;//broadcast
      txbuf[1] = 0x53;//reset
      txbuf[2] = txbuf[0] ^ txbuf[1];//xor checksum
      GPIO_SetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);//tx enable
      SLEEP(0.001);
      start_tx(3);
      SLEEP(0.004);
      GPIO_ResetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);//tx disable
      SLEEP(0.1);
      GOTO(1);
   }
   STATE(1){//get state
      txbuf[0] = 0xff;//broadcast
      txbuf[1] = 0x50;//get state
      txbuf[2] = txbuf[0] ^ txbuf[1];//xor checksum
      GPIO_SetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);//tx enable
      SLEEP(0.001);
      start_tx(3);
      start_rx();
      SLEEP(0.004);
      GPIO_ResetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);//tx disable
      SLEEP(0.2);
      available = sizeof(rxbuf) - DMA_GetCurrDataCounter(FB0_UART_RX_DMA);
      if(available == 4 && rxbuf[3] == (rxbuf[0] ^ rxbuf[1] ^ rxbuf[2])){//size and checksum ok
         if(rxbuf[2] == 0){
            PIN(status) = 0;
            GOTO(2);
         }else{
            PIN(status) = rxbuf[2];
            GOTO(1);
         }
      }else{
         PIN(status) = -1;
         GOTO(0);
      }
   }
   STATE(2){//read pos
      txbuf[0] = 0xff;//broadcast
      txbuf[1] = 0x42;//read position
      txbuf[2] = txbuf[0] ^ txbuf[1];//xor checksum
      GPIO_SetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);//tx enable
      SLEEP(0.001);
      start_tx(3);
      start_rx();
      SLEEP(0.004);
      GPIO_ResetBits(FB0_Z_TXEN_PORT, FB0_Z_TXEN_PIN);//tx disable
      SLEEP(0.02);
      available = sizeof(rxbuf) - DMA_GetCurrDataCounter(FB0_UART_RX_DMA);
      if(available == 7 && rxbuf[1] == 0x42 && rxbuf[6] == (rxbuf[0] ^ rxbuf[1] ^ rxbuf[2] ^ rxbuf[3] ^ rxbuf[4] ^ rxbuf[5])){
         pos = (rxbuf[2] << 24) + (rxbuf[3] << 16) + (rxbuf[4] << 8) + rxbuf[5];
         pos = pos % (128 * 32);
         PIN(pos) = pos * 2.0 * M_PI / (128 * 32) - M_PI;
         PIN(read_status) = 0;
      }else if(available == 4 && rxbuf[1] == 0x50 && rxbuf[3] == (rxbuf[0] ^ rxbuf[1] ^ rxbuf[2])){
         PIN(read_status) = rxbuf[2];
         GOTO(0);
      }else{
         GOTO(0);
      }
      SLEEP(0.02);
   }
   );
);

ENDCOMP;