move Math to Utils

lib/Espfc/src/Blackbox/Blackbox.cpp

@@ -1,7 +1,7 @@
 #include "Blackbox.h"
 #include "Hardware.h"
 #include "EscDriver.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 #include "BlackboxBridge.h"
 
 namespace Espfc {
@@ -237,8 +237,8 @@ void FAST_CODE_ATTR Blackbox::updateData()
 {
   for(size_t i = 0; i < AXIS_COUNT_RPY; i++)
   {
-    gyro.gyroADCf[i] = Math::toDeg(_model.state.gyro.adc[i]);
-    gyro.gyroADC[i] = Math::toDeg(_model.state.gyro.scaled[i]);
+    gyro.gyroADCf[i] = Utils::toDeg(_model.state.gyro.adc[i]);
+    gyro.gyroADC[i] = Utils::toDeg(_model.state.gyro.scaled[i]);
     pidData[i].P = _model.state.innerPid[i].pTerm * 1000.f;
     pidData[i].I = _model.state.innerPid[i].iTerm * 1000.f;
     pidData[i].D = _model.state.innerPid[i].dTerm * 1000.f;
@@ -257,7 +257,7 @@ void FAST_CODE_ATTR Blackbox::updateData()
   rcCommand[AXIS_THRUST] = _model.state.input.buffer[AXIS_THRUST];
   for(size_t i = 0; i < 4; i++)
   {
-    motor[i] = Math::clamp(_model.state.output.us[i], (int16_t)1000, (int16_t)2000);
+    motor[i] = Utils::clamp(_model.state.output.us[i], (int16_t)1000, (int16_t)2000);
     if(_model.state.mixer.digitalOutput)
     {
       motor[i] = PWM_TO_DSHOT(motor[i]);

lib/Espfc/src/Blackbox/BlackboxBridge.cpp

@@ -65,7 +65,7 @@ bool sensors(uint32_t mask)
 
 float pidGetPreviousSetpoint(int axis)
 {
-  return Espfc::Math::toDeg(_model_ptr->state.setpoint.rate[axis]);
+  return Espfc::Utils::toDeg(_model_ptr->state.setpoint.rate[axis]);
 }
 
 float mixerGetThrottle(void)

lib/Espfc/src/Blackbox/BlackboxFlashfs.cpp

@@ -5,7 +5,7 @@
 
 #include "Device/FlashDevice.h"
 #include "Utils/RingBuf.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 
 static const uint32_t FLASHFS_ERASED_VAL = 0xffffffff;
 
@@ -101,7 +101,7 @@ bool IRAM_ATTR flashfsFlushAsync(bool force)
     const size_t size = buffer->size();
     if(flashfs.partition && size > 0)
     {
-        //uint32_t newAddress = force ? (flashfs.address + size) : Espfc::Math::alignAddressToWrite(flashfs.address, size, FLASHFS_FLUSH_BUFFER_SIZE);
+        //uint32_t newAddress = force ? (flashfs.address + size) : Espfc::Utils::alignAddressToWrite(flashfs.address, size, FLASHFS_FLUSH_BUFFER_SIZE);
         //size_t toWrite = newAddress - flashfs.address;
         uint8_t tmp[FLASHFS_FLUSH_BUFFER_SIZE];
         size_t chunks = (size / FLASHFS_FLUSH_BUFFER_SIZE) + 1;

lib/Espfc/src/Connect/Cli.h

@@ -1042,9 +1042,9 @@ class Cli
           s.print(_model.config.gyro.bias[0]); s.print(' ');
           s.print(_model.config.gyro.bias[1]); s.print(' ');
           s.print(_model.config.gyro.bias[2]); s.print(F(" ["));
-          s.print(Math::toDeg(_model.state.gyro.bias[0])); s.print(' ');
-          s.print(Math::toDeg(_model.state.gyro.bias[1])); s.print(' ');
-          s.print(Math::toDeg(_model.state.gyro.bias[2])); s.println(F("]"));
+          s.print(Utils::toDeg(_model.state.gyro.bias[0])); s.print(' ');
+          s.print(Utils::toDeg(_model.state.gyro.bias[1])); s.print(' ');
+          s.print(Utils::toDeg(_model.state.gyro.bias[2])); s.println(F("]"));
 
           s.print(F("accel offset: "));
           s.print(_model.config.accel.bias[0]); s.print(' ');
@@ -1200,7 +1200,7 @@ class Cli
           float v = _model.state.input.ch[c];
           float min = _model.config.scaler[i].minScale * 0.01f;
           float max = _model.config.scaler[i].maxScale * 0.01f;
-          float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max);
+          float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max);
           s.print(F("scaler: "));
           s.print(i);
           s.print(' ');
@@ -1445,7 +1445,7 @@ class Cli
           {
             size = String(cmd.args[3]).toInt();
           }
-          size = Math::clamp(size, 8u, 128 * 1024u);
+          size = Utils::clamp(size, 8u, 128 * 1024u);
           size_t chunk_size = 256;
 
           uint8_t* data = new uint8_t[chunk_size];

lib/Espfc/src/Connect/MspProcessor.h

@@ -679,9 +679,9 @@ class MspProcessor
           break;
 
         case MSP_ATTITUDE:
-          r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll  [decidegrees]
-          r.writeU16(lrintf(Math::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees]
-          r.writeU16(lrintf(Math::toDeg(-_model.state.attitude.euler.z)));       // yaw   [degrees]
+          r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.x) * 10.f)); // roll  [decidegrees]
+          r.writeU16(lrintf(Utils::toDeg(_model.state.attitude.euler.y) * 10.f)); // pitch [decidegrees]
+          r.writeU16(lrintf(Utils::toDeg(-_model.state.attitude.euler.z)));       // yaw   [degrees]
           break;
 
         case MSP_ALTITUDE:
@@ -951,10 +951,10 @@ class MspProcessor
             {
               _model.config.input.superRate[i] = m.readU8();
             }
-            _model.config.controller.tpaScale = Math::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid
+            _model.config.controller.tpaScale = Utils::clamp(m.readU8(), (uint8_t)0, (uint8_t)90); // dyn thr pid
             m.readU8(); // thrMid8
             m.readU8();  // thr expo
-            _model.config.controller.tpaBreakpoint = Math::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint
+            _model.config.controller.tpaBreakpoint = Utils::clamp(m.readU16(), (uint16_t)1000, (uint16_t)2000); // tpa breakpoint
             if(m.remain() >= 1)
             {
               _model.config.input.expo[AXIS_YAW] = m.readU8(); // yaw expo
@@ -1283,7 +1283,7 @@ class MspProcessor
           }
           for (int i = 0; i < AXIS_COUNT_RPY; i++)
           {
-            r.writeU16(lrintf(Math::toDeg(_model.state.gyro.adc[i])));
+            r.writeU16(lrintf(Utils::toDeg(_model.state.gyro.adc[i])));
           }
           for (int i = 0; i < AXIS_COUNT_RPY; i++)
           {

lib/Espfc/src/Control/Actuator.cpp

@@ -1,5 +1,5 @@
 #include "Control/Actuator.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 
 namespace Espfc {
 
@@ -59,7 +59,7 @@ void Actuator::updateScaler()
     float v = _model.state.input.ch[c];
     float min = _model.config.scaler[i].minScale * 0.01f;
     float max = _model.config.scaler[i].maxScale * 0.01f;
-    float scale = Math::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max);
+    float scale = Utils::map3(v, -1.f, 0.f, 1.f, min, min < 0 ? 0.f : 1.f, max);
     for(size_t x = 0; x < AXIS_COUNT_RPYT; x++)
     {
       if(
@@ -212,15 +212,15 @@ void Actuator::updateBuzzer()
 void Actuator::updateDynLpf()
 {
   return; // temporary disable
-  int scale = Math::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000);
+  int scale = Utils::clamp((int)_model.state.input.us[AXIS_THRUST], 1000, 2000);
   if(_model.config.gyro.dynLpfFilter.cutoff > 0) {
-    int gyroFreq = Math::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq);
+    int gyroFreq = Utils::map(scale, 1000, 2000, _model.config.gyro.dynLpfFilter.cutoff, _model.config.gyro.dynLpfFilter.freq);
     for(size_t i = 0; i < AXIS_COUNT_RPY; i++) {
       _model.state.gyro.filter[i].reconfigure(gyroFreq);
     }
   }
   if(_model.config.dterm.dynLpfFilter.cutoff > 0) {
-    int dtermFreq = Math::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq);
+    int dtermFreq = Utils::map(scale, 1000, 2000, _model.config.dterm.dynLpfFilter.cutoff, _model.config.dterm.dynLpfFilter.freq);
     for(size_t i = 0; i < AXIS_COUNT_RPY; i++) {
       _model.state.innerPid[i].dtermFilter.reconfigure(dtermFreq);
     }

lib/Espfc/src/Control/Controller.cpp

@@ -1,5 +1,5 @@
 #include "Control/Controller.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 
 namespace Espfc {
 
@@ -65,19 +65,19 @@ void Controller::outerLoopRobot()
 
   if(true || _model.isModeActive(MODE_ANGLE))
   {
-    angle = _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit);
+    angle = _model.state.input.ch[AXIS_PITCH] * Utils::toRad(_model.config.level.angleLimit);
   }
   else
   {
-    angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Math::toRad(_model.config.level.rateLimit);
+    angle = _model.state.outerPid[AXIS_PITCH].update(_model.state.input.ch[AXIS_PITCH], speed) * Utils::toRad(_model.config.level.rateLimit);
   }
   _model.state.setpoint.angle.set(AXIS_PITCH, angle);
-  _model.state.setpoint.rate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Math::toRad(_model.config.level.rateLimit);
+  _model.state.setpoint.rate[AXIS_YAW] = _model.state.input.ch[AXIS_YAW] * Utils::toRad(_model.config.level.rateLimit);
 
   if(_model.config.debug.mode == DEBUG_ANGLERATE)
   {
     _model.state.debug[0] = speed * 1000;
-    _model.state.debug[1] = lrintf(Math::toDeg(angle) * 10);
+    _model.state.debug[1] = lrintf(Utils::toDeg(angle) * 10);
   }
 }
 
@@ -88,7 +88,7 @@ void Controller::innerLoopRobot()
   //const float angle = acos(v.z);
   const float angle = std::max(abs(_model.state.attitude.euler[AXIS_PITCH]), abs(_model.state.attitude.euler[AXIS_ROLL]));
 
-  const bool stabilize = angle < Math::toRad(_model.config.level.angleLimit);
+  const bool stabilize = angle < Utils::toRad(_model.config.level.angleLimit);
   if(stabilize)
   {
     _model.state.output.ch[AXIS_PITCH] = _model.state.innerPid[AXIS_PITCH].update(_model.state.setpoint.angle[AXIS_PITCH], _model.state.attitude.euler[AXIS_PITCH]);
@@ -103,7 +103,7 @@ void Controller::innerLoopRobot()
 
   if(_model.config.debug.mode == DEBUG_ANGLERATE)
   {
-    _model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[AXIS_PITCH]) * 10);
+    _model.state.debug[2] = lrintf(Utils::toDeg(_model.state.attitude.euler[AXIS_PITCH]) * 10);
     _model.state.debug[3] = lrintf(_model.state.output.ch[AXIS_PITCH] * 1000);
   }
 }
@@ -113,8 +113,8 @@ void FAST_CODE_ATTR Controller::outerLoop()
   if(_model.isModeActive(MODE_ANGLE))
   {
     _model.state.setpoint.angle = VectorFloat(
-      _model.state.input.ch[AXIS_ROLL] * Math::toRad(_model.config.level.angleLimit),
-      _model.state.input.ch[AXIS_PITCH] * Math::toRad(_model.config.level.angleLimit),
+      _model.state.input.ch[AXIS_ROLL] * Utils::toRad(_model.config.level.angleLimit),
+      _model.state.input.ch[AXIS_PITCH] * Utils::toRad(_model.config.level.angleLimit),
       _model.state.attitude.euler[AXIS_YAW]
     );
     _model.state.setpoint.rate[AXIS_ROLL]  = _model.state.outerPid[AXIS_ROLL].update(_model.state.setpoint.angle[AXIS_ROLL], _model.state.attitude.euler[AXIS_ROLL]);
@@ -135,7 +135,7 @@ void FAST_CODE_ATTR Controller::outerLoop()
   {
     for(size_t i = 0; i < AXIS_COUNT_RPY; ++i)
     {
-      _model.state.debug[i] = lrintf(Math::toDeg(_model.state.setpoint.rate[i]));
+      _model.state.debug[i] = lrintf(Utils::toDeg(_model.state.setpoint.rate[i]));
     }
   }
 }
@@ -152,18 +152,18 @@ void FAST_CODE_ATTR Controller::innerLoop()
 
   if(_model.config.debug.mode == DEBUG_ITERM_RELAX)
   {
-    _model.state.debug[0] = lrintf(Math::toDeg(_model.state.innerPid[0].itermRelaxBase));
+    _model.state.debug[0] = lrintf(Utils::toDeg(_model.state.innerPid[0].itermRelaxBase));
     _model.state.debug[1] = lrintf(_model.state.innerPid[0].itermRelaxFactor * 100.0f);
-    _model.state.debug[2] = lrintf(Math::toDeg(_model.state.innerPid[0].iTermError));
+    _model.state.debug[2] = lrintf(Utils::toDeg(_model.state.innerPid[0].iTermError));
     _model.state.debug[3] = lrintf(_model.state.innerPid[0].iTerm * 1000.0f);
   }
 }
 
 float Controller::getTpaFactor() const
 {
   if(_model.config.controller.tpaScale == 0) return 1.f;
-  float t = Math::clamp(_model.state.input.us[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f);
-  return Math::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f));
+  float t = Utils::clamp(_model.state.input.us[AXIS_THRUST], (float)_model.config.controller.tpaBreakpoint, 2000.f);
+  return Utils::map(t, (float)_model.config.controller.tpaBreakpoint, 2000.f, 1.f, 1.f - ((float)_model.config.controller.tpaScale * 0.01f));
 }
 
 void Controller::resetIterm()

lib/Espfc/src/Control/Fusion.cpp

@@ -74,9 +74,9 @@ int FAST_CODE_ATTR Fusion::update()
 
     if(_model.config.debug.mode == DEBUG_ALTITUDE)
     {
-      _model.state.debug[0] = lrintf(Math::toDeg(_model.state.attitude.euler[0]) * 10);
-      _model.state.debug[1] = lrintf(Math::toDeg(_model.state.attitude.euler[1]) * 10);
-      _model.state.debug[2] = lrintf(Math::toDeg(_model.state.attitude.euler[2]) * 10);
+      _model.state.debug[0] = lrintf(Utils::toDeg(_model.state.attitude.euler[0]) * 10);
+      _model.state.debug[1] = lrintf(Utils::toDeg(_model.state.attitude.euler[1]) * 10);
+      _model.state.debug[2] = lrintf(Utils::toDeg(_model.state.attitude.euler[2]) * 10);
     }
     return 1;
 }

lib/Espfc/src/Control/Pid.cpp

@@ -1,5 +1,5 @@
 #include "Pid.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 #include "Utils/MemoryHelper.h"
 
 namespace Espfc {
@@ -41,11 +41,11 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement)
         const bool increasing = (iTerm > 0 && iTermError > 0) || (iTerm < 0 && iTermError < 0);
         const bool incrementOnly = itermRelax == ITERM_RELAX_RP_INC || itermRelax == ITERM_RELAX_RPY_INC;
         itermRelaxBase = setpoint - itermRelaxFilter.update(setpoint);
-        itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Math::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40)
+        itermRelaxFactor = std::max(0.0f, 1.0f - std::abs(Utils::toDeg(itermRelaxBase)) * 0.025f); // (itermRelaxBase / 40)
         if(!incrementOnly || increasing) iTermError *= itermRelaxFactor;
       }
       iTerm += Ki * iScale * iTermError * dt;
-      iTerm = Math::clamp(iTerm, -iLimit, iLimit);
+      iTerm = Utils::clamp(iTerm, -iLimit, iLimit);
     }
   }
   else
@@ -82,7 +82,7 @@ float FAST_CODE_ATTR Pid::update(float setpoint, float measurement)
   prevError = error;
   prevSetpoint = setpoint;
 
-  return Math::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit);
+  return Utils::clamp(pTerm + iTerm + dTerm + fTerm, -oLimit, oLimit);
 }
 
 }

lib/Espfc/src/Control/Pid.h

@@ -2,7 +2,7 @@
 
 #include <cstdint>
 #include "Utils/Filter.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 
 namespace Espfc {
 
@@ -12,10 +12,10 @@ constexpr float ITERM_SCALE_BETAFLIGHT = 0.244381f;
 constexpr float DTERM_SCALE_BETAFLIGHT = 0.000529f;
 constexpr float FTERM_SCALE_BETAFLIGHT = 0.00013754f;
 
-constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000
-constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.014f
-constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; // ~ 0.0000303f
-constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Math::toDeg(1.0f) * 0.001f; //
+constexpr float PTERM_SCALE = PTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.00183 = 0.032029f * 57.29 / 1000
+constexpr float ITERM_SCALE = ITERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.014f
+constexpr float DTERM_SCALE = DTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; // ~ 0.0000303f
+constexpr float FTERM_SCALE = FTERM_SCALE_BETAFLIGHT * Utils::toDeg(1.0f) * 0.001f; //
 
 constexpr float LEVEL_PTERM_SCALE = 0.1f;    // 1/10
 constexpr float LEVEL_ITERM_SCALE = 0.1f;    // 1/10

lib/Espfc/src/Control/Rates.cpp

@@ -22,7 +22,7 @@ void Rates::begin(const InputConfig& config)
 
 float FAST_CODE_ATTR Rates::getSetpoint(const int axis, float input) const
 {
-  input = Math::clamp(input, -0.995f, 0.995f); // limit input
+  input = Utils::clamp(input, -0.995f, 0.995f); // limit input
   const float inputAbs = fabsf(input);
   float result = 0;
   switch(rateType)
@@ -38,7 +38,7 @@ float FAST_CODE_ATTR Rates::getSetpoint(const int axis, float input) const
     case RATES_TYPE_QUICK:
       result = quick(axis, input, inputAbs);
   }
-  return Math::toRad(Math::clamp(result, -(float)rateLimit[axis], (float)rateLimit[axis]));
+  return Utils::toRad(Utils::clamp(result, -(float)rateLimit[axis], (float)rateLimit[axis]));
 }
 
 float FAST_CODE_ATTR Rates::betaflight(const int axis, float rcCommandf, const float rcCommandfAbs) const

lib/Espfc/src/Control/Rates.h

@@ -1,6 +1,6 @@
 #pragma once
 
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 #include "ModelConfig.h"
 
 namespace Espfc
@@ -41,7 +41,7 @@ class Rates
 
     inline float constrainf(float x, float l, float h) const
     {
-      return Math::clamp(x, l, h);
+      return Utils::clamp(x, l, h);
     }
 
   private:

lib/Espfc/src/Device/InputSBUS.cpp

@@ -1,5 +1,5 @@
 #include "InputSBUS.h"
-#include "Math/Utils.h"
+#include "Utils/Math.hpp"
 #include "Utils/MemoryHelper.h"
 
 namespace Espfc {
@@ -140,7 +140,7 @@ void FAST_CODE_ATTR InputSBUS::apply()
 
 uint16_t FAST_CODE_ATTR InputSBUS::convert(int v)
 {
-  return Math::clamp(((v * 5) / 8) + 880, 800, 2200);
+  return Utils::clamp(((v * 5) / 8) + 880, 800, 2200);
 }
 
 }