damping.c

#include <stdint.h>
#include <stdio.h>
#include <unistd.h>
#include <sys/time.h>

typedef struct
{
  float dampingFactor;
  float dampingCoeff;
  float target;
  float measured;
} rtDataType;

uint64_t getTime_ms(void)
{
  struct timeval tp;
  gettimeofday(&tp, NULL);
  const uint64_t now_ms = tp.tv_sec * 1000 + tp.tv_usec / 1000;
  static uint64_t start_ms = 0;
  if(start_ms==0)
    {
      start_ms = now_ms;
    }
  return now_ms - start_ms;
}

uint32_t getSignal(uint32_t min, uint32_t max, uint32_t period_ms )
{
  const uint32_t now_ms = getTime_ms();

  uint32_t value = min;
  if ((now_ms%period_ms)>=(period_ms/2))
    {
      value = max;
    }

  return value;
}

void delay_ms(uint32_t ms)
{
  const uint32_t now_ms = getTime_ms();
  uint32_t deadline_ms = now_ms;
  do
    {
      deadline_ms = getTime_ms();
    } while(deadline_ms<=(now_ms+ms));
}

void applyDampingFilter(rtDataType* rtData)
{
  if(rtData->target>rtData->measured)
    {
      rtData->dampingCoeff -= rtData->dampingFactor;
    }
  else if (rtData->target<rtData->measured)
    {
      rtData->dampingCoeff += rtData->dampingFactor;
    }
  else
    {
      rtData->dampingCoeff = 0.0f;
    }
  rtData->measured = ((rtData->target-rtData->measured)/(100-rtData->dampingCoeff))*rtData->dampingCoeff + rtData->measured;
}

void show(const rtDataType* const  rtData)
{
  printf("Target: %.2f # Damped PWM value: %.2f\n", rtData->target, rtData->measured);
}


int main()
{
  rtDataType rtData;
  rtData.dampingFactor = 0.1f;
  rtData.dampingCoeff = 0.0f;
  rtData.measured = 2.5f;

  while(1)
    {
      rtData.target = getSignal(0, 5, 15000);
      applyDampingFilter(&rtData);
      show(&rtData);

      delay_ms(50);
  }

  return 0;
}