Skip to content

Improve _normalizeAngle speed. #530

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Draft
jaguilar wants to merge 1 commit into
base: dev
Choose a base branch
from
+11 −6
Draft

Improve _normalizeAngle speed. #530

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
10 changes: 7 additions & 3 deletions src/common/base_classes/FOCMotor.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,5 +1,6 @@
#include "FOCMotor.h"
#include "../../communication/SimpleFOCDebug.h"
#include "common/foc_utils.h"

/**
* Default constructor - setting all variabels to default values
Expand Down Expand Up @@ -431,6 +432,9 @@ float FOCMotor::angleOpenloop(float target_angle){
shaft_angle = target_angle;
shaft_velocity = 0;
}
// Normalize the shaft angle after each iteration to prevent it growing indefinitely
// and eventually losing precision.
shaft_angle = _normalizeAngle(shaft_angle);
Copy link
Copy Markdown
Member

@runger1101001 runger1101001 Apr 30, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Did you test this? It seems to me we’ll never reach the target as shaft-angle is used to track the current position in open loop angle mode?

Copy link
Copy Markdown
Author

@jaguilar jaguilar Apr 30, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This part was not tested with angles greater than 2PI. I agree that if the user sets angles outside the range, it wouldn't work. We should probably also normalize the target angle beforehand. WDYT?


// save timestamp for next call
open_loop_timestamp = now_us;
Expand Down Expand Up @@ -864,7 +868,7 @@ int FOCMotor::alignSensor() {
// move one electrical revolution forward
for (int i = 0; i <=500; i++ ) {
float angle = _3PI_2 + _2PI * i / 500.0f;
setPhaseVoltage(voltage_align, 0, angle);
setPhaseVoltage(voltage_align, 0, _normalizeAngle(angle));
sensor->update();
_delay(2);
}
Expand All @@ -874,7 +878,7 @@ int FOCMotor::alignSensor() {
// move one electrical revolution backwards
for (int i = 500; i >=0; i-- ) {
float angle = _3PI_2 + _2PI * i / 500.0f ;
setPhaseVoltage(voltage_align, 0, angle);
setPhaseVoltage(voltage_align, 0, _normalizeAngle(angle));
sensor->update();
_delay(2);
}
Expand Down Expand Up @@ -914,7 +918,7 @@ int FOCMotor::alignSensor() {
sensor->update();
// get the current zero electric angle
zero_electric_angle = 0;
zero_electric_angle = electricalAngle();
zero_electric_angle = _normalizeAngle(electricalAngle());
_delay(20);
SIMPLEFOC_MOTOR_DEBUG("Zero elec. angle: ", zero_electric_angle);
// stop everything
Expand Down
7 changes: 4 additions & 3 deletions src/common/foc_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -75,13 +75,14 @@ __attribute__((weak)) float _atan2(float y, float x) {

// normalizing radian angle to [0,2PI]
__attribute__((weak)) float _normalizeAngle(float angle){
float a = fmod(angle, _2PI);
return a >= 0 ? a : (a + _2PI);
constexpr float INV_2PI = 1.f / _2PI;
float rotations = floorf(angle * INV_2PI);
return angle - (rotations * _2PI);
}

// Electrical angle calculation
float _electricalAngle(float shaft_angle, int pole_pairs) {
return (shaft_angle * pole_pairs);
return _normalizeAngle(shaft_angle * pole_pairs);
}

// square root approximation function using
Expand Down
Loading