Added set and external inputs to observer and controllers

include/controllers/pi_controller.hpp

@@ -2,19 +2,19 @@
 
 namespace controllers
 {
-struct pi_config
+struct PIConfig
 {
   float Kp, Ki, Ts, upper_limit, lower_limit;
 };
 
-class pi_controller
+class PIController
 {
   float error_sum, output, error_prev;
   float reset(float input, float output, float upper_limit, float lower_limit);
   float clamp(float input, float upper_limit, float lower_limit);
 
 public:
-  pi_controller();
-  float loop(float error, const pi_config& config);
+  PIController();
+  float loop(float error, const PIConfig& config, float external_integral = 0, bool reset_integral = false);
 };
 }  // namespace controllers

include/controllers/pid_controller.hpp

@@ -16,6 +16,6 @@ class pid_controller
 
 public:
   pid_controller();
-  float loop(float error, const pid_config& config);
+  float loop(float error, const pid_config& config, float external_integral = 0, bool reset_integral = false);
 };
 }  // namespace controllers

include/math/functions.hpp

@@ -6,7 +6,7 @@ namespace math
 {
 float abs(float input);
 float pow(float input, int power);
-float wrap_angle(float input);
+float wrapAngle(float input);
 float sin(float angle);
 float cos(float angle);
 float atan(float input);

include/observers/bemf_observer.hpp

@@ -12,7 +12,7 @@ class bemf_observer
     position_tracker tracker;
     float speed, angle, Ts;
   public:
-    bemf_observer(const bemf_gains& gains, const controllers::pi_config& update_config, const controllers::pi_config& tracker_config, float Ts);
+    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);
 };
 } // namespace observers

include/observers/dq_update.hpp

@@ -5,10 +5,31 @@
 
 namespace observers
 {
-struct bemf_gains
+
+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(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)
+  {
+  }
+};
+
+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(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)
+  {
+  }
+};
+
+struct BemfGains
 {
   float VOLTAGE_GAIN, CURRENT_GAIN, EMF_GAIN, SPEED_CURRENT_GAIN;
-  bemf_gains(float Ld, float Lq, float Rs, float Ts, int axis)
+  BemfGains(float Ld, float Lq, float Rs, float Ts, int axis)
   {
     if(axis == 0) {
       VOLTAGE_GAIN = Ts/(2*Ld + Rs*Ts);
@@ -29,12 +50,12 @@ class bemf_solver
 {
   float angular_velocity, rotor_angle, Ts;
   math::frame_dq I, V, X, E, I_prev, X_prev;
-  const bemf_gains& gains;
-  controllers::pi_config update_config;
-  controllers::pi_controller d_axis, q_axis;
+  const BemfGains& gains;
+  controllers::PIConfig update_config;
+  controllers::PIController d_axis, q_axis;
 
 public:
-  bemf_solver(const controllers::pi_config& config, const bemf_gains& gains, float Ts);
-  float loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity, float rotor_angle);
+  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());
 };
 }  // namespace observers

include/observers/tracker.hpp

@@ -6,16 +6,30 @@
 
 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) {}
+};
+
+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) {}
+};
+
 class position_tracker
 {
   float speed_est, angle_est, Ts;
-  controllers::pi_controller angle_controller;
-  controllers::pi_config tracker_config;
+  controllers::PIController angle_controller;
+  controllers::PIConfig tracker_config;
   math::integrator angle_integrator;
 
 public:
-  position_tracker(controllers::pi_config config, float Ts);
-  float loop(float phase_error);
+  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);
 };
 }  // namespace observers

src/controllers/pi_controller.cpp

@@ -1,10 +1,10 @@
 #include <controllers/pi_controller.hpp>
 
-controllers::pi_controller::pi_controller() : error_sum(0), output(0), error_prev(0)
+controllers::PIController::PIController() : error_sum(0), output(0), error_prev(0)
 {
 }
 
-float controllers::pi_controller::clamp(float input, float upper_limit, float lower_limit)
+float controllers::PIController::clamp(float input, float upper_limit, float lower_limit)
 {
   if (input < lower_limit)
     return lower_limit;
@@ -14,7 +14,7 @@ float controllers::pi_controller::clamp(float input, float upper_limit, float lo
     return input;
 }
 
-float controllers::pi_controller::reset(float input, float output, float upper_limit, float lower_limit)
+float controllers::PIController::reset(float input, float output, float upper_limit, float lower_limit)
 {
   if (input > 0 && output >= upper_limit)
     return 0;
@@ -24,14 +24,17 @@ float controllers::pi_controller::reset(float input, float output, float upper_l
     return input;
 }
 
-float controllers::pi_controller::loop(float error, const pi_config& config)
+float controllers::PIController::loop(float error, const PIConfig& config, float external_integral, bool reset_integral)
 {
   if (error > 0 && output >= config.upper_limit)
     error = 0;
   else if (error < 0 && output <= config.lower_limit)
     error = 0;
 
-  error_sum += clamp(config.Ki * config.Ts * (error + error_prev) / 2, config.upper_limit, config.lower_limit);
+  error_sum += config.Ki * config.Ts * (error + error_prev) / 2;
+  if (reset_integral)
+    error_sum = external_integral;
+  error_sum = clamp(error_sum, config.upper_limit, config.lower_limit);
   output = clamp(config.Kp * error + config.Ki * error_sum, config.upper_limit, config.lower_limit);
 
   error_prev = error;

src/controllers/pid_controller.cpp

@@ -24,14 +24,17 @@ float controllers::pid_controller::reset(float input, float output, float upper_
     return input;
 }
 
-float controllers::pid_controller::loop(float error, const pid_config& config)
+float controllers::pid_controller::loop(float error, const pid_config& config, float external_integral, bool reset_integral)
 {
   if (error > 0 && output >= config.upper_limit)
     error = 0;
   else if (error < 0 && output <= config.lower_limit)
     error = 0;
 
-  error_sum += clamp(config.Ki * config.Ts * (error + error_prev) / 2, config.upper_limit, config.lower_limit);
+  error_sum += config.Ki * config.Ts * (error + error_prev) / 2;
+  if (reset_integral)
+    error_sum = external_integral;
+  error_sum = clamp(error_sum, config.upper_limit, config.lower_limit);
   float derror = (error - error_prev) / config.Ts;
   output =
       clamp(config.Kp * error + config.Ki * error_sum + config.Kd * derror, config.upper_limit, config.lower_limit);

src/math/functions.cpp

@@ -16,7 +16,7 @@ float math::pow(float input, int power)
   return output;
 }
 
-float math::wrap_angle(float input)
+float math::wrapAngle(float input)
 {
   if (input < -PI)
     return input + PI;

src/observers/bemf_observer.cpp

@@ -1,6 +1,6 @@
 #include <observers/bemf_observer.hpp>
 
-observers::bemf_observer::bemf_observer(const bemf_gains& gains, const controllers::pi_config& update_config, const controllers::pi_config& tracker_config, float Ts)
+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)
 {
 }

src/observers/dq_update.cpp

@@ -1,7 +1,7 @@
 #include <observers/dq_update.hpp>
 #include <math/functions.hpp>
 
-observers::bemf_solver::bemf_solver(const controllers::pi_config& config, const bemf_gains& gains, float Ts)
+observers::bemf_solver::bemf_solver(const controllers::PIConfig& config, const BemfGains& gains, float Ts)
   : angular_velocity(0)
   , rotor_angle(0)
   , Ts(Ts)
@@ -15,22 +15,38 @@ observers::bemf_solver::bemf_solver(const controllers::pi_config& config, const
 }
 
 float observers::bemf_solver::loop(math::frame_abc line_currents, math::frame_abc line_voltages, float angular_velocity,
-                                   float rotor_angle)
+                                   float rotor_angle, const SetBemfParams& set_params, const ExtBemfParams& ext_params)
 {
+  math::frame_dq I_est;
+  math::frame_dq error;
+
   I = math::park_transform(math::clarke_transform(line_currents), rotor_angle);
   V = math::park_transform(math::clarke_transform(line_voltages), rotor_angle);
 
-  E.d = q_axis.loop(I.d - I_prev.d, update_config);
-  E.q = d_axis.loop(I.q - I_prev.q, update_config);
-
   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;
 
-  I.d = X.d + X_prev.d + gains.CURRENT_GAIN * I_prev.d;
-  I.q = X.q + X_prev.q + gains.CURRENT_GAIN * I_prev.q;
+  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)
+    I_est.d = ext_params.id_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)
+    error.d = ext_params.error_d;
+  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);
 
   X_prev = X;
-  I_prev = I;
+  I_prev = I_est;
 
-  return math::atan2(E.q, E.d);
+  return math::atan2(E.d, E.q);
 }

src/observers/tracker.cpp

@@ -1,14 +1,19 @@
 #include <observers/tracker.hpp>
 
-observers::position_tracker::position_tracker(controllers::pi_config config, float Ts)
+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)
+float observers::position_tracker::loop(float phase_error, const SetTrackerParams& set_params, const ExtTrackerParams& ext_params)
 {
-  angle_est += Ts * angle_controller.loop(phase_error, tracker_config);
-  return math::wrap_angle(angle_est);
+  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);
+  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)

tests/bemf_observer_test.cpp

@@ -8,9 +8,9 @@
 
 int main() 
 {
-  observers::bemf_gains gains(1, 1, 1, Ts, 0);
-  controllers::pi_config update_config = {1, 1, Ts, 180, -180};
-  controllers::pi_config tracker_config  = {1, 1, Ts, 180,-180};
+  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);
 
   math::frame_abc line_currents;

tests/pi_test.cpp

@@ -8,8 +8,8 @@ using namespace controllers;
 int main()
 {
   float x_meas = 0, x_ref = 10;
-  pi_config config = { 2, 1, Ts, 10, -10 };
-  pi_controller controller;
+  PIConfig config = { 2, 1, Ts, 10, -10 };
+  PIController controller;
   std::cout << " Starting loop" << std::endl;
   for (float t = 0; t < 10; t += Ts)
   {