1. Updated observer_bemf for more runtime changing variables

2. Stability SMD needs to be added yet

include/observers/bemf_observer.hpp

@@ -3,16 +3,26 @@
 #include <observers/dq_update.hpp>
 #include <observers/tracker.hpp>
 #include <math/foc.hpp>
+#include <cstdint>
 
 namespace observers
 {
-class bemf_observer
+struct BemfOutput
 {
-    bemf_solver dq_update;
-    position_tracker tracker;
-    float speed, angle, Ts;
+  float m_speed_rpm, m_speed_rpm_raw, phase_error_rad, e_theta_deg, e_theta_deg_6_step;
+  uint8_t stability_smd_state, bemf_stable, bemf_stability_flag;
+};
+
+class BemfObserver
+{
+    BemfSolver dq_update;
+    Tracker tracker;
+    float speed_prev, angle_prev, Vbus_prev;
   public:
-    bemf_observer(const BemfGains& gains, const controllers::PIConfig& update_config, const controllers::PIConfig& tracker_config, float Ts);
-    float loop(math::frame_abc line_currents, math::frame_abc line_voltages);
+    BemfObserver();
+    BemfOutput loop(math::frame_abc line_currents, math::frame_abc line_voltages, math::frame_abc 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

include/observers/dq_update.hpp

@@ -46,16 +46,16 @@ struct BemfGains
   }
 };
 
-class bemf_solver
+class BemfSolver
 {
-  float angular_velocity, rotor_angle, Ts;
-  math::frame_dq I, V, X, E, I_prev, X_prev;
-  const BemfGains& gains;
-  controllers::PIConfig update_config;
+  math::frame_dq I_prev, X_prev, E;
   controllers::PIController d_axis, q_axis;
 
 public:
-  bemf_solver(const controllers::PIConfig& config, const BemfGains& gains, float Ts);
-  float loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity, float rotor_angle, const SetBemfParams& set_params = SetBemfParams(), const ExtBemfParams& ext_params = ExtBemfParams());
+  BemfSolver();
+  float loop(math::frame_alpha_beta currents, math::frame_alpha_beta voltages, const controllers::PIConfig& config, 
+              const BemfGains& gains, float angular_velocity, float rotor_angle,
+              const SetBemfParams& set_params = SetBemfParams(), const ExtBemfParams& ext_params = ExtBemfParams());
+  math::frame_dq get_emfs() const;
 };
 }  // namespace observers

include/observers/tracker.hpp

@@ -20,16 +20,15 @@ struct ExtTrackerParams
   ExtTrackerParams(float error, float speed, float etheta) : error(error), speed(speed), etheta(etheta) {}
 };
 
-class position_tracker
+class Tracker
 {
-  float speed_est, angle_est, Ts;
+  float speed_est, angle_est;
   controllers::PIController angle_controller;
-  controllers::PIConfig tracker_config;
   math::integrator angle_integrator;
 
 public:
-  position_tracker(controllers::PIConfig config, float Ts);
-  float loop(float phase_error, const SetTrackerParams& params = SetTrackerParams(), const ExtTrackerParams& ext_params = ExtTrackerParams());
-  float speed_tracker(float angle_est);
+  Tracker() = default;
+  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

@@ -1,15 +1,112 @@
 #include <observers/bemf_observer.hpp>
 
-observers::bemf_observer::bemf_observer(const BemfGains& gains, const controllers::PIConfig& update_config, const controllers::PIConfig& tracker_config, float Ts)
-  : dq_update(update_config, gains, Ts), tracker(tracker_config, Ts), Ts(Ts)
+observers::BemfObserver::BemfObserver() : speed_prev(0), angle_prev(0)
 {
 }
 
-float observers::bemf_observer::loop(math::frame_abc line_currents, math::frame_abc line_voltages)
+observers::BemfOutput observers::BemfObserver::loop(math::frame_abc line_currents, math::frame_abc line_voltages, math::frame_abc 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)
 {
-  float phase_error = dq_update.loop(line_currents, line_voltages, speed, angle);
-  float angle_est = tracker.loop(phase_error);
-  speed = tracker.speed_tracker(angle_est);
-  angle = angle_est;
-  return angle;
+  observers::BemfOutput output;
+
+  math::frame_alpha_beta currents = math::clarke_transform(line_currents);
+  math::frame_alpha_beta voltages;
+  if(idle_mode)
+    voltages = math::clarke_transform(line_voltages);
+  else {
+    // Vbus Filter
+    Vbus = 0.01* Vbus + 0.99*Vbus_prev;
+    Vbus_prev = Vbus;
+
+    // Duty Correction
+    math::frame_abc duty_corrected;
+    float duty_average = (duties.a + duties.b + duties.c) / 3;
+    duty_corrected.a = duties.a - duty_average;
+    duty_corrected.b = duties.b - duty_average;
+    duty_corrected.c = duties.c - duty_average;
+
+    // Duty to Voltage
+    voltages = math::clarke_transform(duty_corrected);
+    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};
+    return config;
+  }();
+  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 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);
+  float angular_velocity = [speed, set_tracker_params, ext_tracker_params, this]() -> float {
+    constexpr float alpha = 0.01;
+    if(set_tracker_params.speed)
+      return set_tracker_params.speed;
+    else
+      return speed*alpha + (1-alpha)*speed_prev;
+  }();
+  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)
+      e_theta_deg -= 360;
+    else if(e_theta_deg < -180)
+      e_theta_deg += 360;
+    if (angular_velocity < 0 && e_theta_deg > 0)
+      e_theta_deg -= 180;
+    else if (angular_velocity < 0 && e_theta_deg < 0)
+      e_theta_deg += 180;
+    return e_theta_deg;
+  }();
+  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(angle_deg >= -150 && angle_deg < -90)
+        return -120;
+      else if(angle_deg >= -90 && angle_deg < -30)
+        return -60;
+      else if(angle_deg >= -30 && angle_deg < 30)
+        return 0;
+      else if(angle_deg >= 30 && angle_deg < 90)
+        return 60;
+      else if(angle_deg >= 90 && angle_deg < 150)
+        return 120;
+      else if(angle_deg >= 150 || angle_deg < -150)
+        return 180;
+      else
+        return 0;
+    }
+    else
+      return angle_deg;
+  }();
+
+  //Bemf Stablility SMD
+  if(pos_obs_mode == 2) {
+
+  }
+  if(idle_mode) {
+
+  }
+  if(force_bemf) {
+
+  }
+
+  speed_prev = speed;
+  angle_prev = angle;
+
+  return output;
 }

src/observers/dq_update.cpp

@@ -1,27 +1,18 @@
 #include <observers/dq_update.hpp>
 #include <math/functions.hpp>
 
-observers::bemf_solver::bemf_solver(const controllers::PIConfig& config, const BemfGains& gains, float Ts)
-  : angular_velocity(0)
-  , rotor_angle(0)
-  , Ts(Ts)
-  , V(0, 0)
-  , E(0, 0)
-  , I_prev(0, 0)
-  , X_prev(0, 0)
-  , gains(gains)
-  , update_config(config)
+observers::BemfSolver::BemfSolver() : I_prev(0, 0), X_prev(0, 0), E(0, 0)
 {
 }
 
-float observers::bemf_solver::loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity,
-                                   float rotor_angle, const SetBemfParams& set_params, const ExtBemfParams& ext_params)
+float observers::BemfSolver::loop(math::frame_alpha_beta currents, math::frame_alpha_beta voltages, const controllers::PIConfig& config, 
+                                    const BemfGains& gains, float angular_velocity, float rotor_angle, 
+                                    const SetBemfParams& set_params, const ExtBemfParams& ext_params)
 {
-  math::frame_dq I_est;
-  math::frame_dq error;
+  math::frame_dq I_est, error, X;
 
-  I = math::park_transform(math::clarke_transform(line_currents), rotor_angle);
-  V = math::park_transform(math::clarke_transform(line_voltages), rotor_angle);
+  math::frame_dq I = math::park_transform(currents, rotor_angle);
+  math::frame_dq V = math::park_transform(voltages, rotor_angle);
 
   X.d = gains.VOLTAGE_GAIN * V.d + gains.SPEED_CURRENT_GAIN * I.q * angular_velocity + gains.EMF_GAIN * E.d;
   X.q = gains.VOLTAGE_GAIN * V.q + gains.SPEED_CURRENT_GAIN * I.d * angular_velocity + gains.EMF_GAIN * E.q;
@@ -42,11 +33,16 @@ float observers::bemf_solver::loop(math::frame_abc line_currents, math::frame_ab
   if(set_params.error_q)
     error.q = ext_params.error_q;
 
-  E.d = q_axis.loop(error.d, update_config, set_params.error_sum_d, ext_params.error_sum_d);
-  E.q = d_axis.loop(error.q, update_config, set_params.error_sum_q, ext_params.error_sum_q);
+  E.d = q_axis.loop(error.d, config, set_params.error_sum_d, ext_params.error_sum_d);
+  E.q = d_axis.loop(error.q, config, set_params.error_sum_q, ext_params.error_sum_q);
 
   X_prev = X;
   I_prev = I_est;
 
   return math::atan2(E.d, E.q);
 }
+
+math::frame_dq observers::BemfSolver::get_emfs() const
+{
+  return E;
+}

src/observers/tracker.cpp

@@ -1,23 +1,17 @@
 #include <observers/tracker.hpp>
 
-observers::position_tracker::position_tracker(controllers::PIConfig config, float Ts)
-  : speed_est(0), angle_est(0), Ts(Ts), tracker_config(config)
-{
-}
-
-float observers::position_tracker::loop(float phase_error, 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)
     phase_error = ext_params.error;
-  angle_est += Ts * angle_controller.loop(phase_error, tracker_config, set_params.speed, ext_params.speed);
+  angle_est += config.Ts * angle_controller.loop(phase_error, config, set_params.speed, ext_params.speed);
   if(set_params.etheta)
     angle_est = ext_params.etheta;
   angle_est = math::wrapAngle(angle_est);
   return angle_est;
 }
 
-float observers::position_tracker::speed_tracker(float angle_est)
+float observers::Tracker::speed_tracker(float angle_est, float Ts)
 {
-  speed_est = angle_integrator.loop(angle_est, Ts);
-  return speed_est;
+  return angle_integrator.loop(angle_est, Ts);
 }

tests/bemf_observer_test.cpp

@@ -9,18 +9,24 @@
 int main() 
 {
   observers::BemfGains gains(1, 1, 1, Ts, 0);
-  controllers::PIConfig update_config = {1, 1, Ts, 180, -180};
-  controllers::PIConfig tracker_config  = {1, 1, Ts, 180,-180};
-  observers::bemf_observer bemf_observer(gains, update_config, tracker_config, Ts);
+  observers::BemfObserver bemf_observer;
 
   math::frame_abc line_currents;
   math::frame_abc line_voltages;
   math::frame_dq U;
+  float angle = 0, Vbus = 12;
+  observers::SetBemfParams set_bemf_params;
+  observers::SetTrackerParams set_tracker_params;
+  observers::ExtBemfParams ext_bemf_params;
+  observers::ExtTrackerParams ext_tracker_params;
+  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)
   {
-    float angle = bemf_observer.loop(line_currents, line_voltages);
     math::frame_abc 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);
+    angle = output.e_theta_deg;
     std::cout << "Duties: " << duties.a << " " << duties.b << " " << duties.c << std::endl;
   }
 }