Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-08-18 18:20:15 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

Convert SVM to not use out parameters

Browse Source
This commit is contained in:
Unknown
2020-09-05 00:56:59 -04:00
parent ea08c1fe77
commit 29332281be
2 changed files with 27 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/motor.cpp
+2 -2
View File
@@ -262,8 +262,8 @@ bool Motor::run_calibration() {
bool Motor::enqueue_modulation_timings(float mod_alpha, float mod_beta) {
if (is_nan(mod_alpha) || is_nan(mod_beta))
return set_error(ERROR_MODULATION_IS_NAN), false;
float tA, tB, tC;
if (!SVM(mod_alpha, mod_beta, &tA, &tB, &tC))
auto [tA, tB, tC, success] = SVM(mod_alpha, mod_beta);
if(!success)
return set_error(ERROR_MODULATION_MAGNITUDE), false;
next_timings_[0] = (uint16_t)(tA * (float)TIM_1_8_PERIOD_CLOCKS);
next_timings_[1] = (uint16_t)(tB * (float)TIM_1_8_PERIOD_CLOCKS);
Firmware/MotorControl/utils.hpp
+25 -23
View File
@@ -4,6 +4,7 @@
#include <limits>
#include <algorithm>
#include <array>
#include <tuple>
/**
* @brief Flash size register address
@@ -124,7 +125,8 @@ inline float wrap_pm_pi(float x) {
// as per the magnitude invariant clarke transform
// The magnitude of the alpha-beta vector may not be larger than sqrt(3)/2
// Returns true on success, and false if the input was out of range
inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
inline auto SVM(float alpha, float beta) {
float tA, tB, tC;
int Sextant;
if (beta >= 0.0f) {
@@ -166,9 +168,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
float t2 = two_by_sqrt3 * beta;
// PWM timings
*tA = (1.0f - t1 - t2) * 0.5f;
*tB = *tA + t1;
*tC = *tB + t2;
tA = (1.0f - t1 - t2) * 0.5f;
tB = tA + t1;
tC = tB + t2;
} break;
// sextant v2-v3
@@ -178,9 +180,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
float t3 = -alpha + one_by_sqrt3 * beta;
// PWM timings
*tB = (1.0f - t2 - t3) * 0.5f;
*tA = *tB + t3;
*tC = *tA + t2;
tB = (1.0f - t2 - t3) * 0.5f;
tA = tB + t3;
tC = tA + t2;
} break;
// sextant v3-v4
@@ -190,9 +192,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
float t4 = -alpha - one_by_sqrt3 * beta;
// PWM timings
*tB = (1.0f - t3 - t4) * 0.5f;
*tC = *tB + t3;
*tA = *tC + t4;
tB = (1.0f - t3 - t4) * 0.5f;
tC = tB + t3;
tA = tC + t4;
} break;
// sextant v4-v5
@@ -202,9 +204,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
float t5 = -two_by_sqrt3 * beta;
// PWM timings
*tC = (1.0f - t4 - t5) * 0.5f;
*tB = *tC + t5;
*tA = *tB + t4;
tC = (1.0f - t4 - t5) * 0.5f;
tB = tC + t5;
tA = tB + t4;
} break;
// sextant v5-v6
@@ -214,9 +216,9 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
float t6 = alpha - one_by_sqrt3 * beta;
// PWM timings
*tC = (1.0f - t5 - t6) * 0.5f;
*tA = *tC + t5;
*tB = *tA + t6;
tC = (1.0f - t5 - t6) * 0.5f;
tA = tC + t5;
tB = tA + t6;
} break;
// sextant v6-v1
@@ -226,17 +228,17 @@ inline bool SVM(float alpha, float beta, float* tA, float* tB, float* tC) {
float t1 = alpha + one_by_sqrt3 * beta;
// PWM timings
*tA = (1.0f - t6 - t1) * 0.5f;
*tC = *tA + t1;
*tB = *tC + t6;
tA = (1.0f - t6 - t1) * 0.5f;
tC = tA + t1;
tB = tC + t6;
} break;
}
int result_valid =
*tA >= 0.0f && *tA <= 1.0f
&& *tB >= 0.0f && *tB <= 1.0f
&& *tC >= 0.0f && *tC <= 1.0f;
return result_valid;
tA >= 0.0f && tA <= 1.0f
&& tB >= 0.0f && tB <= 1.0f
&& tC >= 0.0f && tC <= 1.0f;
return std::make_tuple(tA, tB, tC, result_valid);
}