Updated with pre-commit

.github/workflows/cmake-single-platform.yml

@@ -36,4 +36,3 @@ jobs:
       # Execute tests defined by the CMake configuration.
       # See https://cmake.org/cmake/help/latest/manual/ctest.1.html for more detail
       run: ctest -C ${{env.BUILD_TYPE}}
-

include/observers/bemf_observer.hpp

@@ -1,4 +1,4 @@
-# pragma once
+#pragma once
 
 #include <observers/dq_update.hpp>
 #include <observers/tracker.hpp>
@@ -15,14 +15,16 @@ struct BemfOutput
 
 class BemfObserver
 {
-    BemfSolver dq_update;
-    Tracker tracker;
-    float speed_prev, angle_prev, Vbus_prev;
-  public:
-    BemfObserver();
-    BemfOutput loop(math::FrameABC line_currents, math::FrameABC line_voltages, math::FrameABC duties,
-                float Vbus, const float Ts, const SetBemfParams& set_bemf_params, const SetTrackerParams& set_tracker_params,
-                const ExtBemfParams& ext_bemf_params, const ExtTrackerParams& ext_tracker_params, uint8_t pos_obs_mode, uint8_t idle_mode,
-                uint8_t opmode, const uint8_t num_rotor_poles, const uint8_t freq_mode, bool force_bemf, bool en_dis_6_step_comm);
+  BemfSolver dq_update;
+  Tracker tracker;
+  float speed_prev, angle_prev, Vbus_prev;
+
+public:
+  BemfObserver();
+  BemfOutput loop(math::FrameABC line_currents, math::FrameABC line_voltages, math::FrameABC duties, float Vbus,
+                  const float Ts, const SetBemfParams& set_bemf_params, const SetTrackerParams& set_tracker_params,
+                  const ExtBemfParams& ext_bemf_params, const ExtTrackerParams& ext_tracker_params,
+                  uint8_t pos_obs_mode, uint8_t idle_mode, uint8_t opmode, const uint8_t num_rotor_poles,
+                  const uint8_t freq_mode, bool force_bemf, bool en_dis_6_step_comm);
 };
-} // namespace observers
+}  // namespace observers

include/observers/dq_update.hpp

@@ -9,19 +9,33 @@ namespace observers
 struct SetBemfParams
 {
   bool error_d, error_sum_d, id_est, error_q, error_sum_q, iq_est;
-  SetBemfParams() : error_d(false), error_sum_d(false), id_est(false), error_q(false), error_sum_q(false), iq_est(false) {}
+  SetBemfParams() : error_d(false), error_sum_d(false), id_est(false), error_q(false), error_sum_q(false), iq_est(false)
+  {
+  }
   SetBemfParams(bool error_d, bool error_sum_d, bool id_est, bool error_q, bool error_sum_q, bool iq_est)
-    : error_d(error_d), error_sum_d(error_sum_d), id_est(id_est), error_q(error_q), error_sum_q(error_sum_q), iq_est(iq_est)
+    : error_d(error_d)
+    , error_sum_d(error_sum_d)
+    , id_est(id_est)
+    , error_q(error_q)
+    , error_sum_q(error_sum_q)
+    , iq_est(iq_est)
   {
   }
 };
 
 struct ExtBemfParams
 {
   float error_d, error_sum_d, id_est, error_q, error_sum_q, iq_est;
-  ExtBemfParams() : error_d(0), error_sum_d(0), id_est(0), error_q(0), error_sum_q(0), iq_est(0) {}
+  ExtBemfParams() : error_d(0), error_sum_d(0), id_est(0), error_q(0), error_sum_q(0), iq_est(0)
+  {
+  }
   ExtBemfParams(float error_d, float error_sum_d, float id_est, float error_q, float error_sum_q, float iq_est)
-    : error_d(error_d), error_sum_d(error_sum_d), id_est(id_est), error_q(error_q), error_sum_q(error_sum_q), iq_est(iq_est)
+    : error_d(error_d)
+    , error_sum_d(error_sum_d)
+    , id_est(id_est)
+    , error_q(error_q)
+    , error_sum_q(error_sum_q)
+    , iq_est(iq_est)
   {
   }
 };
@@ -31,17 +45,19 @@ struct BemfGains
   float VOLTAGE_GAIN, CURRENT_GAIN, EMF_GAIN, SPEED_CURRENT_GAIN;
   BemfGains(float Ld, float Lq, float Rs, float Ts, int axis)
   {
-    if(axis == 0) {
-      VOLTAGE_GAIN = Ts/(2*Ld + Rs*Ts);
-      CURRENT_GAIN = Lq*Ts/(2*Ld + Rs*Ts);
-      EMF_GAIN = Ts/(2*Ld + Rs*Ts);
-      SPEED_CURRENT_GAIN = (2*Ld - Rs*Ts)/(2*Ld + Rs*Ts);
+    if (axis == 0)
+    {
+      VOLTAGE_GAIN = Ts / (2 * Ld + Rs * Ts);
+      CURRENT_GAIN = Lq * Ts / (2 * Ld + Rs * Ts);
+      EMF_GAIN = Ts / (2 * Ld + Rs * Ts);
+      SPEED_CURRENT_GAIN = (2 * Ld - Rs * Ts) / (2 * Ld + Rs * Ts);
     }
-    if(axis == 1) {
-      VOLTAGE_GAIN = Ts/(2*Lq + Rs*Ts);
-      CURRENT_GAIN = -Ld*Ts/(2*Lq + Rs*Ts);
-      EMF_GAIN = Ts/(2*Lq + Rs*Ts);
-      SPEED_CURRENT_GAIN = (2*Lq - Rs*Ts)/(2*Lq + Rs*Ts);
+    if (axis == 1)
+    {
+      VOLTAGE_GAIN = Ts / (2 * Lq + Rs * Ts);
+      CURRENT_GAIN = -Ld * Ts / (2 * Lq + Rs * Ts);
+      EMF_GAIN = Ts / (2 * Lq + Rs * Ts);
+      SPEED_CURRENT_GAIN = (2 * Lq - Rs * Ts) / (2 * Lq + Rs * Ts);
     }
   }
 };
@@ -54,8 +70,8 @@ class BemfSolver
 public:
   BemfSolver();
   float loop(math::FrameAlphaBeta currents, math::FrameAlphaBeta voltages, const controllers::PIConfig& config,
-              const BemfGains& gains, float angular_velocity, float rotor_angle,
-              const SetBemfParams& set_params = SetBemfParams(), const ExtBemfParams& ext_params = ExtBemfParams());
+             const BemfGains& gains, float angular_velocity, float rotor_angle,
+             const SetBemfParams& set_params = SetBemfParams(), const ExtBemfParams& ext_params = ExtBemfParams());
   math::FrameDQ get_emfs() const;
 };
 }  // namespace observers

include/observers/tracker.hpp

@@ -9,15 +9,23 @@ namespace observers
 struct SetTrackerParams
 {
   bool error, speed, etheta;
-  SetTrackerParams() : error(false), speed(false), etheta(false) {}
-  SetTrackerParams(bool error, bool speed, bool etheta) : error(error), speed(speed), etheta(etheta) {}
+  SetTrackerParams() : error(false), speed(false), etheta(false)
+  {
+  }
+  SetTrackerParams(bool error, bool speed, bool etheta) : error(error), speed(speed), etheta(etheta)
+  {
+  }
 };
 
 struct ExtTrackerParams
 {
   float error, speed, etheta;
-  ExtTrackerParams() : error(0), speed(0), etheta(0) {}
-  ExtTrackerParams(float error, float speed, float etheta) : error(error), speed(speed), etheta(etheta) {}
+  ExtTrackerParams() : error(0), speed(0), etheta(0)
+  {
+  }
+  ExtTrackerParams(float error, float speed, float etheta) : error(error), speed(speed), etheta(etheta)
+  {
+  }
 };
 
 class Tracker
@@ -28,7 +36,8 @@ class Tracker
 
 public:
   Tracker() = default;
-  float loop(float phase_error, controllers::PIConfig config, const SetTrackerParams& params = SetTrackerParams(), const ExtTrackerParams& ext_params = ExtTrackerParams());
+  float loop(float phase_error, controllers::PIConfig config, const SetTrackerParams& params = SetTrackerParams(),
+             const ExtTrackerParams& ext_params = ExtTrackerParams());
   float speed_tracker(float angle_est, float Ts);
 };
 }  // namespace observers

src/observers/bemf_observer.cpp

@@ -4,20 +4,25 @@ observers::BemfObserver::BemfObserver() : speed_prev(0), angle_prev(0)
 {
 }
 
-observers::BemfOutput observers::BemfObserver::loop(math::FrameABC line_currents, math::FrameABC line_voltages, math::FrameABC duties,
-                float Vbus, const float Ts, const SetBemfParams& set_bemf_params, const SetTrackerParams& set_tracker_params,
-                const ExtBemfParams& ext_bemf_params, const ExtTrackerParams& ext_tracker_params, uint8_t pos_obs_mode, uint8_t idle_mode,
-                uint8_t opmode, const uint8_t num_rotor_poles, const uint8_t freq_mode, bool force_bemf, bool en_dis_6_step_comm)
+observers::BemfOutput observers::BemfObserver::loop(math::FrameABC line_currents, math::FrameABC line_voltages,
+                                                    math::FrameABC duties, float Vbus, const float Ts,
+                                                    const SetBemfParams& set_bemf_params,
+                                                    const SetTrackerParams& set_tracker_params,
+                                                    const ExtBemfParams& ext_bemf_params,
+                                                    const ExtTrackerParams& ext_tracker_params, uint8_t pos_obs_mode,
+                                                    uint8_t idle_mode, uint8_t opmode, const uint8_t num_rotor_poles,
+                                                    const uint8_t freq_mode, bool force_bemf, bool en_dis_6_step_comm)
 {
   observers::BemfOutput output;
 
   math::FrameAlphaBeta currents = math::clarke_transform(line_currents);
   math::FrameAlphaBeta voltages;
-  if(idle_mode)
+  if (idle_mode)
     voltages = math::clarke_transform(line_voltages);
-  else {
+  else
+  {
     // Vbus Filter
-    Vbus = 0.01* Vbus + 0.99*Vbus_prev;
+    Vbus = 0.01 * Vbus + 0.99 * Vbus_prev;
     Vbus_prev = Vbus;
 
     // Duty Correction
@@ -29,41 +34,41 @@ observers::BemfOutput observers::BemfObserver::loop(math::FrameABC line_currents
 
     // Duty to Voltage
     voltages = math::clarke_transform(duty_corrected);
-    voltages.alpha = voltages.alpha*Vbus;
-    voltages.beta = voltages.beta*Vbus;
-
+    voltages.alpha = voltages.alpha * Vbus;
+    voltages.beta = voltages.beta * Vbus;
   }
 
   const observers::BemfGains bemf_gains(0.0001, 0.0001, 0.1, Ts, 0);
-  const controllers::PIConfig update_config = [freq_mode, opmode, Ts](){
-    controllers::PIConfig config = {0.1, 0.1, Ts, -180, 180};
+  const controllers::PIConfig update_config = [freq_mode, opmode, Ts]() {
+    controllers::PIConfig config = { 0.1, 0.1, Ts, -180, 180 };
     return config;
   }();
-  const controllers::PIConfig track_config = [freq_mode, opmode, Ts](){
-    controllers::PIConfig config = {0.1, 0.1, Ts, -180, 180};
+  const controllers::PIConfig track_config = [freq_mode, opmode, Ts]() {
+    controllers::PIConfig config = { 0.1, 0.1, Ts, -180, 180 };
     return config;
   }();
 
-  float phase_error = dq_update.loop(currents, voltages, update_config, bemf_gains, speed_prev, angle_prev, set_bemf_params, ext_bemf_params);
+  float phase_error = dq_update.loop(currents, voltages, update_config, bemf_gains, speed_prev, angle_prev,
+                                     set_bemf_params, ext_bemf_params);
   float angle = tracker.loop(phase_error, track_config, set_tracker_params, ext_tracker_params);
   float speed = tracker.speed_tracker(angle, Ts);
 
   // Process Tracker Output
   output.phase_error_rad = phase_error;
-  output.m_speed_rpm_raw = speed * 30 / (PI * num_rotor_poles/2);
+  output.m_speed_rpm_raw = speed * 30 / (PI * num_rotor_poles / 2);
   float angular_velocity = [speed, set_tracker_params, ext_tracker_params, this]() -> float {
     constexpr float alpha = 0.01;
-    if(set_tracker_params.speed)
+    if (set_tracker_params.speed)
       return set_tracker_params.speed;
     else
-      return speed*alpha + (1-alpha)*speed_prev;
+      return speed * alpha + (1 - alpha) * speed_prev;
   }();
-  output.m_speed_rpm = angular_velocity * 30 / (PI * num_rotor_poles/2);
+  output.m_speed_rpm = angular_velocity * 30 / (PI * num_rotor_poles / 2);
   output.e_theta_deg = [angular_velocity, angle, Ts, this]() -> float {
-    float e_theta_deg = (angular_velocity * Ts + angle) * 180/PI;
-    if(e_theta_deg > 180)
+    float e_theta_deg = (angular_velocity * Ts + angle) * 180 / PI;
+    if (e_theta_deg > 180)
       e_theta_deg -= 360;
-    else if(e_theta_deg < -180)
+    else if (e_theta_deg < -180)
       e_theta_deg += 360;
     if (angular_velocity < 0 && e_theta_deg > 0)
       e_theta_deg -= 180;
@@ -73,19 +78,19 @@ observers::BemfOutput observers::BemfObserver::loop(math::FrameABC line_currents
   }();
   output.e_theta_deg_6_step = [angle, en_dis_6_step_comm]() -> float {
     float angle_deg = angle * 180 / PI;
-    if(en_dis_6_step_comm)
+    if (en_dis_6_step_comm)
     {
-      if(angle_deg >= -150 && angle_deg < -90)
+      if (angle_deg >= -150 && angle_deg < -90)
         return -120;
-      else if(angle_deg >= -90 && angle_deg < -30)
+      else if (angle_deg >= -90 && angle_deg < -30)
         return -60;
-      else if(angle_deg >= -30 && angle_deg < 30)
+      else if (angle_deg >= -30 && angle_deg < 30)
         return 0;
-      else if(angle_deg >= 30 && angle_deg < 90)
+      else if (angle_deg >= 30 && angle_deg < 90)
         return 60;
-      else if(angle_deg >= 90 && angle_deg < 150)
+      else if (angle_deg >= 90 && angle_deg < 150)
         return 120;
-      else if(angle_deg >= 150 || angle_deg < -150)
+      else if (angle_deg >= 150 || angle_deg < -150)
         return 180;
       else
         return 0;
@@ -94,16 +99,10 @@ observers::BemfOutput observers::BemfObserver::loop(math::FrameABC line_currents
       return angle_deg;
   }();
 
-  //Bemf Stability SMD
-  if(pos_obs_mode == 2) {
-
-  }
-  if(idle_mode) {
-
-  }
-  if(force_bemf) {
-
-  }
+  // Bemf Stability SMD
+  if (pos_obs_mode == 2) {}
+  if (idle_mode) {}
+  if (force_bemf) {}
 
   speed_prev = speed;
   angle_prev = angle;

src/observers/dq_update.cpp

@@ -5,9 +5,9 @@ observers::BemfSolver::BemfSolver() : I_prev(0, 0), X_prev(0, 0), E(0, 0)
 {
 }
 
-float observers::BemfSolver::loop(math::FrameAlphaBeta currents, math::FrameAlphaBeta voltages, const controllers::PIConfig& config,
-                                    const BemfGains& gains, float angular_velocity, float rotor_angle,
-                                    const SetBemfParams& set_params, const ExtBemfParams& ext_params)
+float observers::BemfSolver::loop(math::FrameAlphaBeta currents, math::FrameAlphaBeta voltages,
+                                  const controllers::PIConfig& config, const BemfGains& gains, float angular_velocity,
+                                  float rotor_angle, const SetBemfParams& set_params, const ExtBemfParams& ext_params)
 {
   math::FrameDQ I_est, error, X;
 
@@ -20,17 +20,17 @@ float observers::BemfSolver::loop(math::FrameAlphaBeta currents, math::FrameAlph
   I_est.d = X.d + X_prev.d + gains.CURRENT_GAIN * I_prev.d;
   I_est.q = X.q + X_prev.q + gains.CURRENT_GAIN * I_prev.q;
 
-  if(set_params.id_est)
+  if (set_params.id_est)
     I_est.d = ext_params.id_est;
-  if(set_params.iq_est)
+  if (set_params.iq_est)
     I_est.q = ext_params.iq_est;
 
   error.d = I_est.d - I.d;
   error.q = I_est.q - I.q;
 
-  if(set_params.error_d)
+  if (set_params.error_d)
     error.d = ext_params.error_d;
-  if(set_params.error_q)
+  if (set_params.error_q)
     error.q = ext_params.error_q;
 
   E.d = q_axis.loop(error.d, config, set_params.error_sum_d, ext_params.error_sum_d);

src/observers/tracker.cpp

@@ -1,11 +1,12 @@
 #include <observers/tracker.hpp>
 
-float observers::Tracker::loop(float phase_error, controllers::PIConfig config, const SetTrackerParams& set_params, const ExtTrackerParams& ext_params)
+float observers::Tracker::loop(float phase_error, controllers::PIConfig config, const SetTrackerParams& set_params,
+                               const ExtTrackerParams& ext_params)
 {
-  if(set_params.error)
+  if (set_params.error)
     phase_error = ext_params.error;
   angle_est += config.Ts * angle_controller.loop(phase_error, config, set_params.speed, ext_params.speed);
-  if(set_params.etheta)
+  if (set_params.etheta)
     angle_est = ext_params.etheta;
   angle_est = math::wrapAngle(angle_est);
   return angle_est;

tests/bemf_observer_test.cpp

@@ -21,10 +21,13 @@ int main()
   uint8_t pos_obs_mode = 2, idle_mode = 0, opmode = 3, num_rotor_poles = 8, freq_mode = 2;
   bool force_bemf = false, en_dis_6_step_comm = false;
 
-  for(float t = 0; t < 10; t += Ts)
+  for (float t = 0; t < 10; t += Ts)
   {
     math::FrameABC duties = math::inverse_clarke_transform(math::inverse_park_transform(U, angle));
-    observers::BemfOutput output = bemf_observer.loop(line_currents, line_voltages, duties, Vbus, Ts, set_bemf_params, set_tracker_params, ext_bemf_params, ext_tracker_params, pos_obs_mode, idle_mode, opmode, num_rotor_poles, freq_mode, force_bemf, en_dis_6_step_comm);
+    observers::BemfOutput output =
+        bemf_observer.loop(line_currents, line_voltages, duties, Vbus, Ts, set_bemf_params, set_tracker_params,
+                           ext_bemf_params, ext_tracker_params, pos_obs_mode, idle_mode, opmode, num_rotor_poles,
+                           freq_mode, force_bemf, en_dis_6_step_comm);
     angle = output.e_theta_deg;
     std::cout << "Duties: " << duties.a << " " << duties.b << " " << duties.c << std::endl;
   }

tests/pi_test.cpp

@@ -14,7 +14,7 @@ int main()
   for (float t = 0; t < 10; t += Ts)
   {
     float error = x_ref - x_meas;
-    if(math::abs(error) < 0.1 && t>5)
+    if (math::abs(error) < 0.1 && t > 5)
       break;
     x_meas += Ts * controller.loop(error, config);
     std::cout << "t = " << t << " x = " << x_meas << std::endl;