Mtc: Optimal frequency regression

src/propulsion/main.cpp

@@ -1,5 +1,7 @@
 #include "main.hpp"
 
+#include "utils/math/regression.hpp"
+
 namespace hyped::propulsion {
 
 Main::Main()
@@ -53,6 +55,14 @@ void Main::run()
         break;
       case data::State::kCalibrating:
         if (state_processor_.isInitialised()) {
+          // set coefficients for rpm regulator
+          utils::math::Regression regression;
+          // TODO:find actual x and y data (and probably get from json or something)
+          regression.CaulculateCoefficients({1, 2, 3}, {1, 2, 3});
+          std::vector<double> coefficients = regression.coefficients_;
+          RpmRegulator rpm_regulator;
+          rpm_regulator.setCoefficients(coefficients);
+
           if (motor_data.module_status != data::ModuleStatus::kReady) {
             motor_data.module_status = data::ModuleStatus::kReady;
             data.setMotorData(motor_data);

src/propulsion/rpm_regulator.cpp

@@ -2,6 +2,8 @@
 
 #include <cmath>
 
+#include "utils/math/regression.hpp"
+
 namespace hyped::propulsion {
 
 RpmRegulator::RpmRegulator()
@@ -16,11 +18,19 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3
   return std::max(target, 0);
 }
 
+void RpmRegulator::setCoefficients(std::vector<double> coefficients)
+{
+  coefficients_ = coefficients;
+}
+
 int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity)
 {
+  double beta1 = coefficients_.at(0);
+  double beta0 = coefficients_.at(1);
+
   // polynomial values from simulation
-  return std::round(0.32047 * actual_velocity * actual_velocity + 297.72578 * actual_velocity
-                    + 1024.30824);
+  return std::round(beta1 * actual_velocity + beta0);
+  return 0;
 }
 
 int32_t RpmRegulator::step(const int32_t optimal_rpm, const int32_t actual_rpm)

src/propulsion/rpm_regulator.hpp

@@ -16,6 +16,14 @@ class RpmRegulator {
    */
   static int32_t calculateRpm(const data::nav_t actual_velocity, const int32_t actual_rpm);
 
+  /**
+   * @brief sets the coefficients calculated on startup in utils to be stored
+   * as private class members.
+   *
+   * @param actual_velocity
+   */
+  void setCoefficients(std::vector<double> coeffients);
+
  private:
   /*
    * @brief Construct a new rpm regulator object
@@ -36,7 +44,11 @@ class RpmRegulator {
    * @param actual_rpm - the rpm of the motor
    * @return int32_t - the step with which to increase the rpm
    */
-  static int32_t step(const int32_t optimal_rpm, const int32_t actual_rpm);
+  int32_t step(const int32_t optimal_rpm, const int32_t actual_rpm);
+
+  // coefficients vector of form {beta1, beta0}
+  std::vector<double> coefficients_;
+
 };
 
 }  // namespace hyped::propulsion

src/utils/io/spi.cpp

@@ -266,6 +266,6 @@ Spi::~Spi()
 
   close(spi_fd_);
 }
-}  // namespace io
 }  // namespace utils
 }  // namespace hyped
+}  // namespace hyped

src/utils/math/regression.cpp

@@ -0,0 +1,37 @@
+#include "regression.hpp"
+
+namespace hyped::utils::math {
+
+Regression::Regression()
+{
+}
+
+void Regression::CaulculateCoefficients(const std::vector<double> x_data,
+                                        const std::vector<double> y_data)
+{
+  double x_sum = 0;
+  double y_sum = 0;
+
+  for (size_t i = 0; i < x_data.size(); ++i) {
+    x_sum += x_data.at(i);
+    y_sum += y_data.at(i);
+  }
+
+  double x_mean = static_cast<double>(x_sum / x_data.size());
+  double y_mean = static_cast<double>(y_sum / y_data.size());
+  double s_xx   = 0;
+  double s_yy   = 0;
+  double s_xy   = 0;
+
+  for (size_t i = 0; i < x_data.size(); ++i) {
+    s_xx += std::pow(x_data.at(i) - x_mean, 2);
+    s_yy += std::pow(y_data.at(i) - y_mean, 2);
+    s_xy += (x_data.at(i) - x_mean) * (y_data.at(i) - y_mean);
+  }
+  // regressed function will have form y = beta1*x + beta0
+  double beta1 = s_xy / s_xx;
+  double beta0 = y_mean - (beta1 - x_mean);
+
+  coefficients_ = {beta1, beta0};
+}
+}  // namespace hyped::utils::math

src/utils/math/regression.hpp

@@ -0,0 +1,24 @@
+#pragma once
+#include <cmath>
+
+#include <vector>
+
+namespace hyped::utils::math {
+
+class Regression {
+ public:
+  Regression();
+
+  /**
+   * @brief calculates regressed coefficients using basic x-y linear regression.
+   * only works for straight lines!
+   *
+   * @param x_data vector of x-data points
+   * @param y_data vector of y-data points
+   */
+  void CaulculateCoefficients(std::vector<double> x_data, std::vector<double> y_data);
+
+  // calculated coefficients from regression function
+  std::vector<double> coefficients_;
+};
+}  // namespace hyped::utils::math