Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-24 01:23:52 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

fix rounding mode, use vcvtr

Browse Source
This commit is contained in:
Oskar Weigl
2020-09-04 14:27:22 -07:00
parent 54636c764f
commit 805b308a86
2 changed files with 20 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/.vscode/c_cpp_properties.json
Vendored
+4 -2
View File
@@ -6,12 +6,12 @@
"${workspaceFolder}/**"
],
"defines": [
"__arm__",
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=6",
"HW_VERSION_VOLTAGE=56",
"USB_PROTOCOL_NATIVE",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
@@ -27,6 +27,7 @@
"${workspaceFolder}/**"
],
"defines": [
"__arm__",
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
@@ -47,11 +48,12 @@
"${workspaceFolder}/**"
],
"defines": [
"__arm__",
"STM32F405xx",
"USE_HAL_DRIVER",
"HW_VERSION_MAJOR=3",
"HW_VERSION_MINOR=4",
"HW_VERSION_VOLTAGE=24",
"HW_VERSION_VOLTAGE=56",
"__weak=\"__attribute__((weak))\"",
"__packed=\"__attribute__((__packed__))\"",
"__GNUC__"
Firmware/MotorControl/utils.hpp
+16 -4
View File
@@ -84,12 +84,24 @@ static const float one_by_sqrt3 = 0.57735026919f;
static const float two_by_sqrt3 = 1.15470053838f;
static const float sqrt3_by_2 = 0.86602540378f;
// Result depends on rounding mode
// With default rounding (round to nearest),
// Round to integer
// Default rounding mode: round to nearest
static inline int round_int(float x) {
#ifdef __arm__
int res;
asm("vcvtr.s32.f32 %[res], %[x]" : [res] "=X" (res) : [x] "w" (x));
return res;
#else
return (int)rint(x);
#endif
}
// Wrap value to range.
// With default rounding mode (round to nearest),
// the result will be in range -y/2 to y/2
static inline float wrap_pm(float x, float y) {
int q = (int)(x/y);
return x - (float)q * y;
int intval = round_int(x/y);
return x - (float)intval * y;
}
// Same as fmodf but result is positive and y must be positive