stmbl

/*
* This file is part of the stmbl project.
*
* Copyright (C) 2016 Rene Hopf <renehopf@mac.com>
*
* This program is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program.  If not, see <http://www.gnu.org/licenses/>.
*/

HAL_COMP(netbob);

// pins
HAL_PIN(dump_pd_vals) = 0.0;
HAL_PIN(error) = 0.0;//counts unknown commands
HAL_PIN(crc_error) = 0.0;//counts crc errors
HAL_PIN(connected) = 0.0;//connection status TODO: not stable during startup, needs link to pd
HAL_PIN(timeout) = 100.0;// 20khz / 1khz * 2 reads = 40
HAL_PIN(rxfoo) = 0.0;

HAL_PIN(pos_cmd) = 0.0;
HAL_PIN(pos_cmd_d) = 0.0;
HAL_PIN(pos_fb) = 0.0;

HAL_PIN(in0) = 0.0;
HAL_PIN(in1) = 0.0;
HAL_PIN(in2) = 0.0;
HAL_PIN(in3) = 0.0;
HAL_PIN(fault) = 0.0;

HAL_PIN(out0) = 0.0;
HAL_PIN(out1) = 0.0;
HAL_PIN(out2) = 0.0;
HAL_PIN(out3) = 0.0;
HAL_PIN(enable) = 0.0;

#define DRV_COUNT 6

typedef struct{
   uint8_t sink;
   uint8_t addr;
   uint8_t data;
   struct {
      uint8_t enable : 1;
      uint8_t brake : 1;
      uint8_t tmp : 2;
      uint8_t io : 4;
   } flags;
   float pos;
   float vel;
} p_data_t;

typedef struct{
   uint8_t packed_type;
   uint8_t length;
   uint8_t packed_cnt;
   uint8_t source;
   p_data_t drive[DRV_COUNT];
   uint32_t crc;
} packet_t;

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

MEM(uint16_t address);//current address pointer
MEM(int rxpos);
MEM(uint32_t timeout);
MEM(int bufferpos);
MEM(int available);
MEM(uint8_t cnt) = 0;

//pb13 txen
//pc12 UART5 tx
//pd2  UART5 rx
void init_hardware() {
  GPIO_InitTypeDef GPIO_InitStruct;
  USART_InitTypeDef USART_InitStruct;
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_UART5, ENABLE);
  //USART TX
  GPIO_PinAFConfig(GPIOC, GPIO_PinSource12, GPIO_AF_UART5);
  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_12;
  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(GPIOC, &GPIO_InitStruct);

  //USART RX
  GPIO_PinAFConfig(GPIOD, GPIO_PinSource2, GPIO_AF_UART5);
  GPIO_InitStruct.GPIO_Pin = GPIO_Pin_2;
  GPIO_Init(GPIOD, &GPIO_InitStruct);

  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13;
  GPIO_Init(GPIOB, &GPIO_InitStructure);

  USART_InitStruct.USART_BaudRate = 5000000;
  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_OverSampling8Cmd(UART5,ENABLE);//16 bit: 2.625, 8bit:5.25
  USART_Init(UART5, &USART_InitStruct);

  USART_Cmd(UART5, ENABLE);

  //RX DMA

  DMA_Cmd(DMA1_Stream0, DISABLE);
  DMA_DeInit(DMA1_Stream0);

  // DMA1-Config
  DMA_InitStructure.DMA_Channel = DMA_Channel_4;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->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_Circular;
  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(DMA1_Stream0, &DMA_InitStructure);

  DMA_Cmd(DMA1_Stream0, ENABLE);

  USART_DMACmd(UART5, USART_DMAReq_Rx, ENABLE);

  //TX DMA

  DMA_Cmd(DMA1_Stream7, DISABLE);
  DMA_DeInit(DMA1_Stream7);

  // DMA2-Config
  DMA_InitStructure.DMA_Channel = DMA_Channel_4;
  DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&(UART5->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(DMA1_Stream7, &DMA_InitStructure);

  //DMA_Cmd(DMA1_Stream7, ENABLE);

  USART_DMACmd(UART5, USART_DMAReq_Tx, ENABLE);
  
  //tx enable
  GPIO_InitStructure.GPIO_Mode  = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
  GPIO_InitStructure.GPIO_PuPd  = GPIO_PuPd_NOPULL;
  GPIO_InitStructure.GPIO_Pin   = GPIO_Pin_13;
  GPIO_Init(GPIOB, &GPIO_InitStructure);
   
  GPIO_ResetBits(GPIOB, GPIO_Pin_13);
}


RT_INIT(
  init_hardware();
  rxpos = 0;
  timeout = 1000;//make sure we start in timeout
);


RT (
   packet_t packet;
   //next received packet will be written to bufferpos
   bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA1_Stream0);
   //how many packets we have the the rx buffer for processing
   available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf);
   if(!EDGE(available)){
      
      if(available == sizeof(packet)){
         //copy packet from dma buffer to struct. TODO: zero copy by writing directly to struct.
         for(int i = 0;i<sizeof(packet);i++){
            ((uint8_t*)&packet)[i] = rxbuf[(rxpos + i)%sizeof(rxbuf)];
         }
         
         //check crc
         CRC_ResetDR();
         if(packet.crc != CRC_CalcBlockCRC((uint32_t *) &packet, sizeof(packet) / 4 - 1)){
            PIN(crc_error)++;
         }else{
            //check for lost packets
            if(packet.packed_cnt != ++cnt){
               cnt = packet.packed_cnt;
               PIN(error) = 1;
            }else{
               PIN(error) = 0;
            }
            PIN(pos_cmd) = packet.drive[0].pos;
         }
         
      }
            
      rxpos += available;
      rxpos %= sizeof(rxbuf);
      PIN(rxfoo) = available;
   }
);

FRT(

);


NRT(

);

ENDCOMP;