cmd clock sync, hv in rt

conf/template/pid.txt

@@ -13,7 +13,6 @@ load io
 load iit
 link conf
 hv0.rt_prio = 0.9
-hv0.frt_prio = 1
 adc0.rt_prio = 1
 reslimit0.rt_prio = 3
 rev0.rt_prio = 4
@@ -22,6 +21,7 @@ vel0.rt_prio = 6
 vel1.rt_prio = 6
 vel2.rt_prio = 6
 io0.rt_prio = 7
+io0.frt_prio = 3
 pid0.rt_prio = 8
 fault0.rt_prio = 10
 iit0.rt_prio = 13

conf/template/smartabs_fb0.txt

@@ -1,5 +1,5 @@
 load smartabs
-smartabs0.rt_prio = 2
+smartabs0.rt_prio = 0.8
 fb_switch0.mot_pos = smartabs0.pos
 fb_switch0.mot_abs_pos = smartabs0.pos
 fb_switch0.mot_polecount = conf0.mot_fb_polecount

conf/template/sserial.txt

@@ -42,3 +42,4 @@ fault0.brake_release = sserial0.out3
 term0.wave2 = linrev0.cmd_d_out
 sserial0.current = hv0.iq_fb
 linrev0.scale = sserial0.scale
+io0.clock_scale = sserial0.clock_scale

src/comps/hv.c

@@ -78,7 +78,6 @@ struct hv_ctx_t {
   uint32_t addr;
   uint16_t timeout;
   uint8_t conf_addr;
-  uint8_t frt_slot;
   uint8_t send_state;
 };
 
@@ -213,15 +212,8 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 }
 
 static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
-  struct hv_ctx_t *ctx = (struct hv_ctx_t *)ctx_ptr;
-  ctx->frt_slot        = 0;
-}
-
-static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct hv_ctx_t *ctx      = (struct hv_ctx_t *)ctx_ptr;
   struct hv_pin_ctx_t *pins = (struct hv_pin_ctx_t *)pin_ptr;
-  ctx->frt_slot++;
-  if(ctx->frt_slot == 3) {
   float e   = PIN(en);
   float pos = PIN(pos);
   float vel = PIN(vel);
@@ -524,7 +516,7 @@ static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
     DMA_ClearFlag(UART_DRV_RX_DMA, UART_DRV_RX_DMA_TCIF);
     DMA_Cmd(UART_DRV_RX_DMA, ENABLE);
   }
-  }
+
 
   PIN(state) = flash_state;
 }
@@ -587,7 +579,7 @@ hal_comp_t hv_comp_struct = {
     .name      = "hv",
     .nrt       = nrt_func,
     .rt        = rt_func,
-    .frt       = frt_func,
+    .frt       = 0,
     .nrt_init  = nrt_init,
     .rt_start  = 0,
     .frt_start = 0,

src/comps/hw/io4.c

@@ -76,6 +76,9 @@ HAL_PIN(fb1z);
 
 HAL_PIN(fbsd);  //fb shutdown
 
+HAL_PIN(master_arr);
+HAL_PIN(clock_scale);
+
 static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr;
   PIN(th0)                  = 12.0;
@@ -83,6 +86,8 @@ static void nrt_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   PIN(fbth0)                = 2.5;
   PIN(fbth1)                = 2.5;
   PIN(out_freq)             = 15000;
+  PIN(master_arr) = TIM3->ARR;
+  PIN(clock_scale) = 1.0;
 }
 
 static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
@@ -253,6 +258,19 @@ static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
 #define ARES 4096.0
 #define AREF 3.3
 
+static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+  struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr;
+  if(PIN(clock_scale) > 1.01){
+    TIM3->ARR = PIN(master_arr) + 1;
+  }
+  else if(PIN(clock_scale) < 0.99){
+    TIM3->ARR = PIN(master_arr) - 1;
+  }
+  else{
+    TIM3->ARR = PIN(master_arr);
+  }
+}
+
 static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   // struct io_ctx_t * ctx = (struct io_ctx_t *)ctx_ptr;
   struct io_pin_ctx_t *pins = (struct io_pin_ctx_t *)pin_ptr;
@@ -502,7 +520,7 @@ hal_comp_t io_comp_struct = {
     .name      = "io",
     .nrt       = 0,
     .rt        = rt_func,
-    .frt       = 0,
+    .frt       = frt_func,
     .nrt_init  = nrt_init,
     .hw_init   = hw_init,
     .rt_start  = 0,

src/comps/sserial.c

@@ -45,6 +45,10 @@ HAL_PIN(vel_fb);
 HAL_PIN(current);
 HAL_PIN(scale);
 
+HAL_PIN(clock_scale);
+HAL_PIN(available);
+HAL_PIN(phase);
+
 HAL_PIN(in0);
 HAL_PIN(in1);
 HAL_PIN(in2);
@@ -62,7 +66,7 @@ HAL_PIN(pos_advance);
 
 //TODO: move to ctx
 struct sserial_ctx_t {
-  uint32_t foo;
+  uint32_t phase;
 };
 
 volatile uint8_t rxbuf[128];  //rx dma buffer
@@ -327,16 +331,41 @@ static void hw_init(void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   block_bytes = 5;
   //calculate timeout in systicks for block_bytes
   max_waste_ticks = (1.0 / 2500000.0) * 11.0 * (float)block_bytes / (1.0f / (float)hal_get_systick_freq());
+
+  PIN(clock_scale) = 1.0;
+  PIN(phase) = 0;
 }
 
+// static void rt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
+//   struct sserial_pin_ctx_t *pins = (struct sserial_pin_ctx_t *)pin_ptr;
+//   //struct sserial_ctx_t *mem = (struct sserial_ctx_t *)ctx_ptr;
+//   PIN(phase) = 0;
+// }
 
 static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
   struct sserial_pin_ctx_t *pins = (struct sserial_pin_ctx_t *)pin_ptr;
+  //struct sserial_ctx_t *mem = (struct sserial_ctx_t *)ctx_ptr;
   //next received packet will be written to bufferpos
   uint32_t bufferpos = sizeof(rxbuf) - DMA_GetCurrDataCounter(DMA2_Stream5);
   //how many packets we have the the rx buffer for processing
   uint32_t available = (bufferpos - rxpos + sizeof(rxbuf)) % sizeof(rxbuf);
 
+  PIN(phase) += 1.0;
+  
+  uint32_t goal = 5;
+  PIN(clock_scale) = 1.0;
+  if(PIN(phase) > 3){
+    PIN(phase) = 0;
+    if(available > goal){
+    PIN(clock_scale) = 0.9;
+    }
+    else if(available < goal && available > 0){
+      PIN(clock_scale) = 1.1;
+    }
+  }
+
+  PIN(available) = available;
+
   if(available >= 1) {
     lbp.byte = rxbuf[rxpos];
 
@@ -436,6 +465,7 @@ static void frt_func(float period, void *ctx_ptr, hal_pin_inst_t *pin_ptr) {
           //send(discovery.input, 1);
           timeout = 0;
           //set output pins
+
           PIN(pos_cmd)   = data_out.pos_cmd;
           PIN(pos_cmd_d) = data_out.vel_cmd;
           PIN(out0)      = data_out.out_0;

src/setup.c

@@ -80,7 +80,7 @@ void setup_res() {
   TIM_SelectSlaveMode(TIM_SLAVE, TIM_SlaveMode_External1);  //Rising edges of the selected trigger (TRGI) clock the counter
   TIM_ITRxExternalClockConfig(TIM_SLAVE, TIM_SLAVE_ITR);    // clk = TIM_MASTER trigger out
   TIM_ARRPreloadConfig(TIM_SLAVE, ENABLE);
-
+  TIM_SLAVE->CNT = (TIM_SLAVE->ARR + 1) / 2;
   TIM_Cmd(TIM_SLAVE, ENABLE);
 
   /* ADC clock enable */