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,9 @@ void Main::run()
         break;
       case data::State::kCalibrating:
         if (state_processor_.isInitialised()) {
+          // TODO: aruments read in from somewhere and processed
+          utils::math::Regression regression;
+          regression.GetCoeffs(std::vector<double>{1, 1, 1}, std::vector<double>{2, 2, 2});
           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()
@@ -18,9 +20,12 @@ int32_t RpmRegulator::calculateRpm(const data::nav_t actual_velocity, const int3
 
 int32_t RpmRegulator::calculateOptimalRpm(const data::nav_t actual_velocity)
 {
+  hyped::utils::math::Regression regression;
+  const double beta1 = regression.Coefficients.beta1;
+  const double beta0 = regression.Coefficients.beta0;
+
   // 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);
 }
 
 int32_t RpmRegulator::step(const int32_t optimal_rpm, const int32_t actual_rpm)

src/utils/io/spi.cpp

@@ -217,7 +217,7 @@ void SPI::transfer(uint8_t *tx, uint8_t *, uint16_t len)
   }
 
 #endif
-}
+}  // namespace io
 
 void SPI::read(uint8_t addr, uint8_t *rx, uint16_t len)
 {
@@ -266,6 +266,6 @@ SPI::~SPI()
 
   close(spi_fd_);
 }
-}  // namespace io
 }  // namespace utils
 }  // namespace hyped
+}  // namespace hyped

src/utils/math/regression.hpp

@@ -0,0 +1,52 @@
+#pragma once
+#include <cmath>
+
+#include <vector>
+
+namespace hyped::utils::math {
+
+class Regression {
+ public:
+  Regression();
+  void GetCoeffs(std::vector<double> x_data, std::vector<double> y_data);
+
+  struct Coefficients {
+    const double beta0;
+    const double beta1;
+  };
+  Coefficients coefficients;
+
+ private:
+  std::vector<double> coefficients_;
+};
+
+void Regression::GetCoeffs(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);
+
+  std::vector<double> coefficients{beta1, beta0};
+  Coefficients.beta0 = coefficients.at(1);
+  Coefficients.beta1 = coefficients.at(0);
+}
+}  // namespace hyped::utils::math