timur/ASIBFM700
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

LibSidServo: fix velocity calculation errors in real mode

Browse Source
This commit is contained in:
Timur A. Fatkhullin
2026-03-11 14:06:58 +03:00
parent b44e695e1c
commit a35b62d21d
4 changed files with 151 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
LibSidServo/ssii.h
View File

@@ -209,12 +209,14 @@ extern double X_MOT_STEPSPERREV, Y_MOT_STEPSPERREV, X_ENC_STEPSPERREV, Y_ENC_STE
// convert angle in radians to +-pi
static inline __attribute__((always_inline)) double ang2half(double ang){
ang = fmod(ang, 2.*M_PI);
if(ang < -M_PI) ang += 2.*M_PI;
else if(ang > M_PI) ang -= 2.*M_PI;
return ang;
}
// convert to only positive: 0..2pi
static inline __attribute__((always_inline)) double ang2full(double ang){
ang = fmod(ang, 2.*M_PI);
if(ang < 0.) ang += 2.*M_PI;
else if(ang > 2.*M_PI) ang -= 2.*M_PI;
return ang;