diff --git a/include/observers/bemf_observer.hpp b/include/observers/bemf_observer.hpp index 021d957..ad3dff3 100644 --- a/include/observers/bemf_observer.hpp +++ b/include/observers/bemf_observer.hpp @@ -3,16 +3,26 @@ #include #include #include +#include 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 diff --git a/include/observers/dq_update.hpp b/include/observers/dq_update.hpp index bf45f71..cb89932 100644 --- a/include/observers/dq_update.hpp +++ b/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 diff --git a/include/observers/tracker.hpp b/include/observers/tracker.hpp index 8ab7872..2dbbe05 100644 --- a/include/observers/tracker.hpp +++ b/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 diff --git a/src/observers/bemf_observer.cpp b/src/observers/bemf_observer.cpp index 46740dc..f69a328 100644 --- a/src/observers/bemf_observer.cpp +++ b/src/observers/bemf_observer.cpp @@ -1,15 +1,112 @@ #include -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; } \ No newline at end of file diff --git a/src/observers/dq_update.cpp b/src/observers/dq_update.cpp index dc2fcd1..e7ce479 100644 --- a/src/observers/dq_update.cpp +++ b/src/observers/dq_update.cpp @@ -1,27 +1,18 @@ #include #include -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; +} diff --git a/src/observers/tracker.cpp b/src/observers/tracker.cpp index 2cf9843..24a0e3b 100644 --- a/src/observers/tracker.cpp +++ b/src/observers/tracker.cpp @@ -1,23 +1,17 @@ #include -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); } diff --git a/tests/bemf_observer_test.cpp b/tests/bemf_observer_test.cpp index d52ca3b..26f45de 100644 --- a/tests/bemf_observer_test.cpp +++ b/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; } } \ No newline at end of file