stmbl

HAL_COMP(adc);

#define INPUT_REF (OP_REF * OP_R_OUT_LOW / (OP_R_OUT_HIGH + OP_R_OUT_LOW))
#define INPUT_GAIN (OP_R_FEEDBACK / OP_R_INPUT * OP_R_OUT_LOW / (OP_R_OUT_HIGH + OP_R_OUT_LOW))
#define V_DIFF(ADC) ((((float)ADC) / (float)ADC_ANZ / ADC_RES * ADC_REF - INPUT_REF) / INPUT_GAIN)
#define V_DIFF2(ADC) (((ADC) / ADC_RES * ADC_REF - INPUT_REF) / INPUT_GAIN)

HAL_PIN(sin) = 0.0;//sin output
HAL_PIN(cos) = 0.0;//cos output
HAL_PIN(sin3) = 0.0;//sin output, last quater only
HAL_PIN(cos3) = 0.0;//cos output, last quater only
HAL_PIN(quad) = 0.0;//quadrant of sin/cos

HAL_PIN(res_en) = 0.0;//flip polarity for resolvers

HAL_PIN(sin_gain) = 1.0;
HAL_PIN(cos_gain) = 1.0;

HAL_PIN(sin_offset) = 0.0;
HAL_PIN(cos_offset) = 0.0;

#define TERM_NUM_WAVES 8
 
HAL_PIN(send_step) = 0.0;//15.0;
HAL_PIN_ARRAY(gain, TERM_NUM_WAVES);
HAL_PIN_ARRAY(offset, TERM_NUM_WAVES);

MEM(volatile float txbuf[8][PID_WAVES*ADC_ANZ]);
MEM(uint32_t txpos) = 0;
MEM(uint32_t send_counter) = 0;//send_step counter
MEM(uint32_t send) = 0;//send buffer state

INIT(
  PIN_ARRAY(gain,0) = 200;
  PIN_ARRAY(gain,1) = 200;
);

RT(
   
   float si[PID_WAVES];
   float co[PID_WAVES];
   uint32_t sii;
   uint32_t coi;
   
   float s_o = PIN(sin_offset);
   float c_o = PIN(cos_offset);
   float s_g = PIN(sin_gain);
   float c_g = PIN(cos_gain);
   float s;
   float c;
   
   volatile uint32_t *ADC_DMA_Buffer;

   // if(DMA_GetCurrentMemoryTarget(DMA2_Stream0) == 0){
   //    ADC_DMA_Buffer = ADC_DMA_Buffer1;
   // }
   // else{
      ADC_DMA_Buffer = ADC_DMA_Buffer0;
   // }
   for(int i = 0; i < PID_WAVES; i++){
      sii = 0;
      coi = 0;
      for(int j = 0; j < ADC_ANZ; j++){
         //ADC dual mode puts both channels in one word, right aligned.
         sii += ADC_DMA_Buffer[i * ADC_ANZ + j] & 0x0000ffff;
         coi += ADC_DMA_Buffer[i * ADC_ANZ + j] >> 16;
         if(send == 0){
           txbuf[0][txpos] = (((i==0 || i==2) && (PIN(res_en) > 0.0))?-1.0:1.0) * V_DIFF2(ADC_DMA_Buffer[i * ADC_ANZ + j] & 0x0000ffff);
           txbuf[1][txpos] = (((i==0 || i==2) && (PIN(res_en) > 0.0))?-1.0:1.0) * V_DIFF2(ADC_DMA_Buffer[i * ADC_ANZ + j] >> 16);
           txpos++;
         }
      }
      si[i] = s_g * V_DIFF(sii) + s_o;
      co[i] = c_g * V_DIFF(coi) + c_o;
   }
   if(send == 0){
     send = 1;
     txpos = 0;
   }

   PIN(sin3) = si[3];
   PIN(cos3) = co[3];

   if(PIN(res_en) > 0.0){
      s = (si[3] - si[2] + si[1] - si[0]) / 4.0;
      c = (co[3] - co[2] + co[1] - co[0]) / 4.0;
   }
   else{
      s = (si[3] + si[2] + si[1] + si[0]) / 4.0;
      c = (co[3] + co[2] + co[1] + co[0]) / 4.0;
   }
   
   if(si[3]>=0){
      if(co[3]>0)
        PIN(quad) = 1;
      else
        PIN(quad) = 2;
   }else{
      if(co[3]>0)
        PIN(quad) = 4;
      else
        PIN(quad) = 3;
   }
   PIN(sin) = s;
   PIN(cos) = c;
);

NRT(
  if(send_counter++ >= PIN(send_step) - 1 && PIN(send_step) > 0){
    int tmp = 0;
    uint8_t buf[TERM_NUM_WAVES + 3];

    buf[0] = 255;
    for(int k = 0; k < PID_WAVES*ADC_ANZ;k++){
      for(int i = 0; i < TERM_NUM_WAVES; i++){
        tmp = (txbuf[i][k] + PIN_ARRAY(offset,i)) * PIN_ARRAY(gain,i) + 128;
        buf[i+1] = CLAMP(tmp,1,254);
      }
      buf[8 + 1] = 0;
      
      if (USB_CDC_is_connected()) {
        USB_VCP_send_string(buf);
      }
    }
    
    buf[0] = 0xfe;//trigger servoterm
    buf[1] = 0x00;
    if (USB_CDC_is_connected()) {
      USB_VCP_send_string(buf);
    }
    send_counter = 0;
    send = 0.0;
  }
);

ENDCOMP;