Merge branch 'master' into f3boot

Makefile

@@ -41,6 +41,7 @@ else
 	COMPS += src/comps/sserial.c
 	COMPS += src/comps/yaskawa.c
 	COMPS += src/comps/encs.c
+	COMPS += src/comps/encf.c
 	CFLAGS += -DV4
 endif
 

conf/template/pmsm.txt

@@ -13,7 +13,7 @@ pmsm_ttc0.polecount = conf0.polecount
 pmsm_limits0.ac_volt = hv0.pwm_volt
 pmsm_limits0.iq = hv0.q_fb
 pmsm_limits0.id = hv0.d_fb
-pmsm_ttc0.pos = vel2.pos_out
+pmsm_ttc0.pos_in = vel2.pos_out
 pid0.max_torque = pmsm_limits0.max_torque
 pid0.min_torque = pmsm_limits0.min_torque
 pid0.max_vel = pmsm_limits0.abs_max_vel
@@ -22,3 +22,4 @@ hv0.d_cmd = fb_switch0.id
 hv0.q_cmd = pmsm_ttc0.cur
 hv0.cmd_mode = 1
 hv0.phase_mode = 2
+hv0.pos = pmsm_ttc0.pos_out

shared/hal.c

@@ -561,6 +561,17 @@ void list(char *ptr) {
 }
 COMMAND("list", list, "show comp instances");
 
+void show_hal(char *ptr) {
+  for(int i = 0; i < hal.comp_inst_count; i++) {
+    for(int j = 0; j < hal.comp_insts[i].comp->pin_count; j++) {
+      volatile hal_comp_inst_t *comp = comp_inst_by_pin_inst(hal.comp_insts[i].pin_insts[j].source->source);
+      printf("%s%lu.%s <= %s%lu.%s = %f\n", hal.comp_insts[i].comp->name, hal.comp_insts[i].instance, hal.comp_insts[i].pins[j], comp->comp->name, comp->instance, (char *)pin_by_pin_inst(hal.comp_insts[i].pin_insts[j].source->source), hal.comp_insts[i].pin_insts[j].source->source->value);
+    }
+  }
+}
+COMMAND("show_hal", show_hal, "show hal structure");
+
+
 void sort_rt() {
   float min     = INFINITY;
   int min_index = -1;

src/comps/encf.c

@@ -0,0 +1,199 @@
+#include "commands.h"
+#include "hal.h"
+#include "math.h"
+#include "defines.h"
+#include "angle.h"
+#include "stm32f4xx_conf.h"
+#include "hw/hw.h"
+
+HAL_COMP(encf);
+
+HAL_PIN(error);
+HAL_PIN(dma);  //dma transfers
+HAL_PIN(dump);
+
+HAL_PIN(pos);
+HAL_PIN(turns);
+HAL_PIN(com_pos);
+HAL_PIN(index);
+
+volatile uint16_t tim_data[160];
+uint32_t count;
+
+#pragma pack(1)
+typedef struct{
+  uint32_t flag0 : 4; // 0101
+  uint32_t bat : 2;
+  uint32_t flag1 : 3; // 101
+  uint32_t no_index : 1; 
+  uint32_t flag2 : 1; // 0
+  uint32_t pos_lo : 6;
+  uint32_t flag3 : 2; // 10
+  uint32_t pos_hi : 16;
+  uint32_t flag4 : 2; // 10
+  uint32_t turns : 16;
+  uint32_t flag5 : 2; // 10
+  uint32_t com_pos : 10;
+  uint32_t flag6 : 7; // 0000011
+  uint32_t crc : 5;
+  uint32_t flag7 : 3; // 000
+} fanuc_t;
+
+union {
+  uint8_t enc_data[10];
+  fanuc_t fanuc;
+} data;
+
+static void nrt_init(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  // struct encf_ctx_t *ctx = (struct encf_ctx_t *)ctx_ptr;
+  // struct encf_pin_ctx_t * pins = (struct encf_pin_ctx_t *)pin_ptr;
+  GPIO_InitTypeDef GPIO_InitStruct;
+  USART_InitTypeDef USART_InitStruct;
+  DMA_InitTypeDef DMA_InitStructurerx;
+
+  RCC_APB1PeriphClockCmd(FB0_ENC_TIM_RCC, ENABLE);
+
+  //TX enable
+  GPIO_InitStruct.GPIO_Pin   = GPIO_Pin_15;
+  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(GPIOD, &GPIO_InitStruct);
+
+  RCC_APB2PeriphClockCmd(RCC_APB2Periph_USART6, ENABLE);
+
+  //USART TX
+  GPIO_PinAFConfig(GPIOC, GPIO_PinSource6, GPIO_AF_USART6);
+  GPIO_InitStruct.GPIO_Pin   = GPIO_Pin_6;
+  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);
+
+  //TIM CH1 A
+  GPIO_InitStruct.GPIO_Pin   = FB0_A_PIN;
+  GPIO_InitStruct.GPIO_Mode  = GPIO_Mode_AF;
+  GPIO_InitStruct.GPIO_Speed = GPIO_Speed_100MHz;
+  GPIO_InitStruct.GPIO_OType = GPIO_OType_PP;
+  GPIO_InitStruct.GPIO_PuPd  = GPIO_PuPd_UP;
+  GPIO_Init(FB0_A_PORT, &GPIO_InitStruct);
+  GPIO_PinAFConfig(FB0_A_PORT, FB0_A_PIN_SOURCE, FB0_ENC_TIM_AF);
+
+  USART_InitStruct.USART_BaudRate            = 128000;
+  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_Tx;
+  USART_Init(USART6, &USART_InitStruct);
+
+  /* Enable the USART */
+  USART_Cmd(USART6, ENABLE);
+
+  //TIM rx dma
+  //fb0 rx auf 12: tim4_ch1 dma1_0_2
+  //fb1 rx auf 12: tim1_ch1 dma2_3_6
+  DMA_InitStructurerx.DMA_Channel            = DMA_Channel_2;
+  DMA_InitStructurerx.DMA_PeripheralBaseAddr = (uint32_t)&FB0_ENC_TIM->CCR1;
+  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;
+  DMA_Cmd(DMA1_Stream0, DISABLE);
+  DMA_DeInit(DMA1_Stream0);
+  DMA_Init(DMA1_Stream0, &DMA_InitStructurerx);
+  //timer setup
+  FB0_ENC_TIM->CR1 &= ~TIM_CR1_CEN; 
+  FB0_ENC_TIM->CCMR1 = TIM_CCMR1_CC1S_0; // cc1 input ti1
+  FB0_ENC_TIM->CCER = TIM_CCER_CC1E | TIM_CCER_CC1P | TIM_CCER_CC1NP; // cc1 en, rising edge, falling edge
+  FB0_ENC_TIM->ARR = 65535;
+  FB0_ENC_TIM->DIER = TIM_DIER_CC1DE; // enable cc1 dma reeuest
+
+  GPIO_SetBits(GPIOD, GPIO_Pin_15);  //tx enable
+}
+
+static void rt_func(float period, volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  // struct encf_ctx_t *ctx      = (struct encf_ctx_t *)ctx_ptr;
+  struct encf_pin_ctx_t *pins = (struct encf_pin_ctx_t *)pin_ptr;
+
+  count = ARRAY_SIZE(tim_data) - DMA1_Stream0->NDTR;
+  PIN(dma) = count;
+
+  for(int i = 0; i < 10; i++){
+    data.enc_data[i] = 0;
+  }
+  
+  uint8_t bits_sum = 0;
+  for(int i = 1; i < count; i++){
+    uint16_t diff = tim_data[i] - tim_data[i-1];
+    //1 bit = 82 ticks
+    //(1.024e-6)/(1/82e6)
+    int bits = (float)diff/(float)81;
+    if(i%2 == 0){
+      for(int j = bits_sum; j < bits + bits_sum; j++){
+        data.enc_data[j/8] |= (1 << j%8);
+      }
+    }
+    bits_sum += bits;
+  }
+
+  uint32_t pos;
+  pos = data.fanuc.pos_lo + (data.fanuc.pos_hi << 6);
+  PIN(pos) = ((float)pos * 2.0 * M_PI / (1<<22)) - M_PI;
+  PIN(turns) = data.fanuc.turns;
+  pos = data.fanuc.com_pos;
+  PIN(com_pos) = (pos * 2.0 * M_PI / 1024) - M_PI;
+  PIN(index) = data.fanuc.no_index;
+
+  //reset timer
+  FB0_ENC_TIM->CNT = 0;
+  FB0_ENC_TIM->CCR1 = 0;
+  FB0_ENC_TIM->CR1 |= TIM_CR1_CEN; // enable tim
+
+  //send request
+  USART6->DR = 0xff;
+  //start DMA
+  DMA_Cmd(DMA1_Stream0, DISABLE);
+  DMA_ClearFlag(DMA1_Stream0, DMA_FLAG_TCIF0);
+  DMA_Cmd(DMA1_Stream0, ENABLE);
+}
+
+static void nrt_func(volatile void *ctx_ptr, volatile hal_pin_inst_t *pin_ptr) {
+  // struct encf_ctx_t *ctx = (struct encf_ctx_t *)ctx_ptr;
+  struct encf_pin_ctx_t * pins = (struct encf_pin_ctx_t *)pin_ptr;
+  if(RISING_EDGE(PIN(dump))) {
+    for(int i = 0; i < 80; i++){
+      if(data.enc_data[i/8] & (1 << i%8)){
+        printf("1");
+      }else{
+        printf("0");
+      }
+    }
+    printf("\n");
+  }
+}
+
+hal_comp_t encf_comp_struct = {
+    .name      = "encf",
+    .nrt       = nrt_func,
+    .rt        = rt_func,
+    .frt       = 0,
+    .nrt_init  = nrt_init,
+    .rt_start  = 0,
+    .frt_start = 0,
+    .rt_stop   = 0,
+    .frt_stop  = 0,
+    .ctx_size  = 0,
+    .pin_count = sizeof(struct encf_pin_ctx_t) / sizeof(struct hal_pin_inst_t),
+};