diff --git a/include/math/foc.hpp b/include/math/foc.hpp index dd01eeb..b750793 100644 --- a/include/math/foc.hpp +++ b/include/math/foc.hpp @@ -2,39 +2,39 @@ namespace math { -struct frame_abc +struct FrameABC { float a, b, c; - frame_abc() = default; - frame_abc(float a, float b, float c) : a(a), b(b), c(c) + FrameABC() = default; + FrameABC(float a, float b, float c) : a(a), b(b), c(c) { } }; -struct frame_alpha_beta +struct FrameAlphaBeta { float alpha, beta; - frame_alpha_beta() = default; - frame_alpha_beta(float alpha, float beta) : alpha(alpha), beta(beta) + FrameAlphaBeta() = default; + FrameAlphaBeta(float alpha, float beta) : alpha(alpha), beta(beta) { } }; -struct frame_dq +struct FrameDQ { float d, q; - frame_dq() = default; - frame_dq(float d, float q) : d(d), q(q) + FrameDQ() = default; + FrameDQ(float d, float q) : d(d), q(q) { } }; -frame_alpha_beta clarke_transform(frame_abc X); +FrameAlphaBeta clarke_transform(FrameABC X); -frame_dq park_transform(frame_alpha_beta X, float angle); +FrameDQ park_transform(FrameAlphaBeta X, float angle); -frame_abc inverse_clarke_transform(frame_alpha_beta X); +FrameABC inverse_clarke_transform(FrameAlphaBeta X); -frame_alpha_beta inverse_park_transform(frame_dq X, float angle); +FrameAlphaBeta inverse_park_transform(FrameDQ X, float angle); } // namespace math diff --git a/include/observers/bemf_observer.hpp b/include/observers/bemf_observer.hpp index ad3dff3..3a3c47b 100644 --- a/include/observers/bemf_observer.hpp +++ b/include/observers/bemf_observer.hpp @@ -20,9 +20,9 @@ class BemfObserver float speed_prev, angle_prev, Vbus_prev; public: 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, + 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 diff --git a/include/observers/dq_update.hpp b/include/observers/dq_update.hpp index cb89932..4e71970 100644 --- a/include/observers/dq_update.hpp +++ b/include/observers/dq_update.hpp @@ -48,14 +48,14 @@ struct BemfGains class BemfSolver { - math::frame_dq I_prev, X_prev, E; + math::FrameDQ I_prev, X_prev, E; controllers::PIController d_axis, q_axis; public: BemfSolver(); - float loop(math::frame_alpha_beta currents, math::frame_alpha_beta voltages, const controllers::PIConfig& config, + 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()); - math::frame_dq get_emfs() const; + math::FrameDQ get_emfs() const; }; } // namespace observers diff --git a/src/math/foc.cpp b/src/math/foc.cpp index c8343f1..efed937 100644 --- a/src/math/foc.cpp +++ b/src/math/foc.cpp @@ -1,35 +1,35 @@ #include #include -math::frame_alpha_beta math::clarke_transform(math::frame_abc X) +math::FrameAlphaBeta math::clarke_transform(math::FrameABC X) { - math::frame_alpha_beta Y; + math::FrameAlphaBeta Y; Y.alpha = X.a; Y.beta = (X.b - X.c) * 0.5773; return Y; } -math::frame_dq math::park_transform(math::frame_alpha_beta X, float angle) +math::FrameDQ math::park_transform(math::FrameAlphaBeta X, float angle) { - math::frame_dq Y; + math::FrameDQ Y; float cosine = math::cos(angle), sine = math::sin(angle); Y.d = X.alpha * cosine + X.beta * sine; Y.q = -X.alpha * sine + X.beta * cosine; return Y; } -math::frame_abc math::inverse_clarke_transform(math::frame_alpha_beta X) +math::FrameABC math::inverse_clarke_transform(math::FrameAlphaBeta X) { - math::frame_abc Y; + math::FrameABC Y; Y.a = X.alpha; Y.b = -0.5 * X.alpha + 0.866 * X.beta; Y.c = -0.5 * X.alpha - 0.866 * X.beta; return Y; } -math::frame_alpha_beta math::inverse_park_transform(math::frame_dq X, float angle) +math::FrameAlphaBeta math::inverse_park_transform(math::FrameDQ X, float angle) { - math::frame_alpha_beta Y; + math::FrameAlphaBeta Y; float cosine = math::cos(angle), sine = math::sin(angle); Y.alpha = X.d * cosine - X.q * sine; Y.beta = X.d * sine + X.q * cosine; diff --git a/src/observers/bemf_observer.cpp b/src/observers/bemf_observer.cpp index f69a328..f3a4b30 100644 --- a/src/observers/bemf_observer.cpp +++ b/src/observers/bemf_observer.cpp @@ -4,15 +4,15 @@ observers::BemfObserver::BemfObserver() : speed_prev(0), angle_prev(0) { } -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, +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::frame_alpha_beta currents = math::clarke_transform(line_currents); - math::frame_alpha_beta voltages; + math::FrameAlphaBeta currents = math::clarke_transform(line_currents); + math::FrameAlphaBeta voltages; if(idle_mode) voltages = math::clarke_transform(line_voltages); else { @@ -21,7 +21,7 @@ observers::BemfOutput observers::BemfObserver::loop(math::frame_abc line_current Vbus_prev = Vbus; // Duty Correction - math::frame_abc duty_corrected; + math::FrameABC 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; @@ -94,7 +94,7 @@ observers::BemfOutput observers::BemfObserver::loop(math::frame_abc line_current return angle_deg; }(); - //Bemf Stablility SMD + //Bemf Stability SMD if(pos_obs_mode == 2) { } @@ -109,4 +109,4 @@ observers::BemfOutput observers::BemfObserver::loop(math::frame_abc line_current 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 e7ce479..abfbe67 100644 --- a/src/observers/dq_update.cpp +++ b/src/observers/dq_update.cpp @@ -5,14 +5,14 @@ observers::BemfSolver::BemfSolver() : I_prev(0, 0), X_prev(0, 0), E(0, 0) { } -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, +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::frame_dq I_est, error, X; + math::FrameDQ I_est, error, X; - math::frame_dq I = math::park_transform(currents, rotor_angle); - math::frame_dq V = math::park_transform(voltages, rotor_angle); + math::FrameDQ I = math::park_transform(currents, rotor_angle); + math::FrameDQ 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,7 +42,7 @@ float observers::BemfSolver::loop(math::frame_alpha_beta currents, math::frame_a return math::atan2(E.d, E.q); } -math::frame_dq observers::BemfSolver::get_emfs() const +math::FrameDQ observers::BemfSolver::get_emfs() const { return E; } diff --git a/tests/bemf_observer_test.cpp b/tests/bemf_observer_test.cpp index 26f45de..7143a00 100644 --- a/tests/bemf_observer_test.cpp +++ b/tests/bemf_observer_test.cpp @@ -6,14 +6,13 @@ #include #define Ts 1e-4 -int main() +int main() { observers::BemfGains gains(1, 1, 1, Ts, 0); observers::BemfObserver bemf_observer; - math::frame_abc line_currents; - math::frame_abc line_voltages; - math::frame_dq U; + math::FrameABC line_currents, line_voltages; + math::FrameDQ U; float angle = 0, Vbus = 12; observers::SetBemfParams set_bemf_params; observers::SetTrackerParams set_tracker_params; @@ -24,9 +23,9 @@ int main() for(float t = 0; t < 10; t += Ts) { - math::frame_abc duties = math::inverse_clarke_transform(math::inverse_park_transform(U, angle)); + 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); angle = output.e_theta_deg; std::cout << "Duties: " << duties.a << " " << duties.b << " " << duties.c << std::endl; } -} \ No newline at end of file +}