esp-fc

#include "Utils/Stats.h"

#include "Hal/FastCode.hpp"
#include "Hal/Time.hpp"

#include <algorithm>
#include <cstddef>

namespace Espfc::Utils {

Stats::Stats(): _loop_last(0), _loop_time(0)
{
  for (size_t i = 0; i < COUNTER_COUNT; i++)
  {
    _start[i] = 0;
    _avg[i] = 0;
    _sum[i] = 0;
    _count[i] = 0;
    _freq[i] = 0;
  }
}

void FAST_CODE_ATTR Stats::start(StatCounter c)
{
  _start[c] = micros();
}

void FAST_CODE_ATTR Stats::end(StatCounter c)
{
  uint32_t diff = micros() - _start[c];
  _sum[c] += diff;
  _count[c]++;
}

void Stats::loopTick()
{
  uint32_t now = micros();
  uint32_t diff = now - _loop_last;
  //_loop_time = diff;
  _loop_time += (((int32_t)diff - _loop_time + 4) >> 3);
  _loop_last = now;
}

uint32_t Stats::loopTime() const
{
  return _loop_time;
}

void Stats::update()
{
  if (!timer.check()) return;
  for (size_t i = 0; i < COUNTER_COUNT; i++)
  {
    _avg[i] = (float)(_sum[i] + (_count[i] >> 1)) / timer.delta;
    _freq[i] = (float)_count[i] * 1e6 / timer.delta;
    _real[i] = _count[i] > 0 ? ((float)(_sum[i] + (_count[i] >> 1)) / _count[i]) : 0.0f;
    _sum[i] = 0;
    _count[i] = 0;
  }
}

float Stats::getLoad(StatCounter c) const
{
  return _avg[c] * 100.f;
}

/**
 * @brief Get the Time of counter normalized to 1 ms
 */
float Stats::getTime(StatCounter c) const
{
  return _avg[c] * 1000.0f;
}

float Stats::getReal(StatCounter c) const
{
  return _real[c];
}

float Stats::getFreq(StatCounter c) const
{
  return _freq[c];
}

float Stats::getTotalLoad() const
{
  float ret = 0;
  for (size_t i = 0; i < COUNTER_COUNT; i++)
  {
    if (i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue;
    ret += getLoad((StatCounter)i);
  }
  return ret;
}

float Stats::getTotalTime() const
{
  float ret = 0;
  for (size_t i = 0; i < COUNTER_COUNT; i++)
  {
    if (i == COUNTER_CPU_0 || i == COUNTER_CPU_1) continue;
    ret += getTime((StatCounter)i);
  }
  return ret;
}

float Stats::getCpuLoad() const
{
  float cpu0 = getLoad(COUNTER_CPU_0);
  float cpu1 = getLoad(COUNTER_CPU_1);
  float maxLoad = std::max(cpu0, cpu1);
  float minLoad = std::min(cpu0, cpu1);
  float alpha = maxLoad / (minLoad + maxLoad);
  return alpha * maxLoad + (1.f - alpha) * minLoad;
}

float Stats::getCpuTime() const
{
  return getTime(COUNTER_CPU_0) + getTime(COUNTER_CPU_1);
}

const char* Stats::getName(StatCounter c) const
{
  // clang-format off
  switch (c)
  {
    case COUNTER_GYRO_READ: return "gyro_r";
    case COUNTER_GYRO_FILTER: return "gyro_f";
    case COUNTER_GYRO_FFT: return "gyro_a";
    case COUNTER_RPM_UPDATE: return " rpm_u";
    case COUNTER_ACCEL_READ: return " acc_r";
    case COUNTER_ACCEL_FILTER: return " acc_f";
    case COUNTER_MAG_READ: return " mag_r";
    case COUNTER_MAG_FILTER: return " mag_f";
    case COUNTER_BARO: return "baro_p";
    case COUNTER_GPS_READ: return " gps_r";
    case COUNTER_IMU_FUSION: return " imu_p";
    case COUNTER_IMU_FUSION2: return " imu_c";
    case COUNTER_OUTER_PID: return " pid_o";
    case COUNTER_INNER_PID: return " pid_i";
    case COUNTER_MIXER: return " mix_p";
    case COUNTER_MIXER_WRITE: return " mix_w";
    case COUNTER_MIXER_READ: return " mix_r";
    case COUNTER_BLACKBOX: return " bblog";
    case COUNTER_INPUT_READ: return "  rx_r";
    case COUNTER_INPUT_FILTER: return "  rx_f";
    case COUNTER_FAILSAFE: return "  rx_s";
    case COUNTER_ACTUATOR: return "  rx_a";
    case COUNTER_SERIAL: return "serial";
    case COUNTER_WIFI: return "  wifi";
    case COUNTER_BATTERY: return "   bat";
    case COUNTER_TELEMETRY: return "   tlm";
    case COUNTER_CPU_0: return " cpu_0";
    case COUNTER_CPU_1: return " cpu_1";
    default: return "unknwn";
  }
  // clang-format on
}

} // namespace Espfc::Utils