openinverter forum

Code: Select all

//Compensate rotor movement that happened between sampling and processing
syncOfs += FP_TOINT(dir * frq * 10);

Code: Select all

//TI encoder observer stuff
//float angle_last = 0; //angle_last = integrator2
static volatile uint16_t integrator1 = 0;
static volatile uint16_t integrator2 = 0;

Code: Select all

/** Calculates angle from sin and cos value
 * @param invert flip values to positive side in order to read a resolver modulated signal on negative edge
*/
uint16_t Encoder::DecodeAngle(bool invert)
{
   int sin = adc_read_injected(ADC1, sinChan);
   int cos = adc_read_injected(ADC1, cosChan);
   Param::SetInt(Param::sin, sin); //spit out for debugging
   Param::SetInt(Param::cos, cos);

   //Wait for signal to reach usable amplitude
   if ((resolverMax - resolverMin) > MIN_RES_AMP)
   {
//      if (invert)
//         return SineCore::Atan2(-sin, -cos);
//      return SineCore::Atan2(sin, cos);
      if (invert)
      {
         sin = -sin;
         cos = -cos;
      }
      int16_t angleatan = SineCore::Atan2(sin, cos);
      Param::SetFlt(Param::angleatan, FP_FROMINT(angleatan) / (65536 / 360)); //original atan2 angle calculation

      //TI observer based implementation
      uint16_t K1 = 550;
      uint16_t K2 = 10000;
      float period = 0.0001;// 1/hz?
      int32_t cos_sin_anglast = (cos - (4096/2))*SineCore::Sine(integrator2)/32767; //angle_last = integrator2
      int32_t sin_cos_anglast = (sin - (4096/2))*SineCore::Cosine(integrator2)/32767;
      int16_t sum_one = sin_cos_anglast - cos_sin_anglast;
      integrator1 += sum_one*K2*period;
      int32_t gain1 = sum_one*K1;
      uint32_t sum_two = integrator1 + gain1;
      integrator2 += sum_two*period;
      int32_t sample_delay_comp = period*integrator1/2;
      //angle_last = integrator2;
      uint16_t obs_angle = integrator2-sample_delay_comp; //digits, 2pi rad = 360 deg = 65536 digits
      return obs_angle;
   }
   else
   {
      int temp = MIN(sin, cos);
      resolverMin = MIN(temp, resolverMin);
      temp = MAX(sin, cos);
      resolverMax = MAX(temp, resolverMax);

      if (0 == startupDelay)
      {
         ErrorMessage::Post(ERR_LORESAMP);
      }
      return 0;
   }
}