diff --git a/.github/workflows/cmake-single-platform.yml b/.github/workflows/cmake-single-platform.yml index 28c6f78..481635f 100644 --- a/.github/workflows/cmake-single-platform.yml +++ b/.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}} - diff --git a/include/observers/bemf_observer.hpp b/include/observers/bemf_observer.hpp index 3a3c47b..77a8c75 100644 --- a/include/observers/bemf_observer.hpp +++ b/include/observers/bemf_observer.hpp @@ -1,4 +1,4 @@ -# pragma once +#pragma once #include #include @@ -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 diff --git a/include/observers/dq_update.hpp b/include/observers/dq_update.hpp index 4e71970..d0df65f 100644 --- a/include/observers/dq_update.hpp +++ b/include/observers/dq_update.hpp @@ -9,9 +9,16 @@ 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) { } }; @@ -19,9 +26,16 @@ struct SetBemfParams 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 diff --git a/include/observers/tracker.hpp b/include/observers/tracker.hpp index 2dbbe05..f2c5672 100644 --- a/include/observers/tracker.hpp +++ b/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 diff --git a/src/observers/bemf_observer.cpp b/src/observers/bemf_observer.cpp index f3a4b30..ac22867 100644 --- a/src/observers/bemf_observer.cpp +++ b/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; diff --git a/src/observers/dq_update.cpp b/src/observers/dq_update.cpp index abfbe67..48404c3 100644 --- a/src/observers/dq_update.cpp +++ b/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); diff --git a/src/observers/tracker.cpp b/src/observers/tracker.cpp index 24a0e3b..e9f5da9 100644 --- a/src/observers/tracker.cpp +++ b/src/observers/tracker.cpp @@ -1,11 +1,12 @@ #include -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; diff --git a/tests/bemf_observer_test.cpp b/tests/bemf_observer_test.cpp index 7143a00..130b88f 100644 --- a/tests/bemf_observer_test.cpp +++ b/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; } diff --git a/tests/pi_test.cpp b/tests/pi_test.cpp index c75bae6..ae438e6 100644 --- a/tests/pi_test.cpp +++ b/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;