Improve _normalizeAngle speed.#530
Conversation
Previously _normalizeAngle used fmodf, which is expensive on Cortex-M series chips (40-100 cycles). For normalizing the angle, we can use a trick of just subtracking 2 pi times the number of full rotations from the angle. This method does lose precision much faster than the fmodf approach. There will be significant error if the total number of radians passed in is greater than 10^5. However, we can counteract this by not passing in such a large number of radians -- basically making sure that we are not using an accumulating radian count in any location where the angle will eventually need to be normalized. This should typically be the case since accumulating radian use cases are definitionally not in need of normalization. We should consider adding an assert here to ensure that we catch any existing cases where we should be passing the mechanical or electrical angle and are instead passing an accumulator. Performance test (on STM32G474, with certain other optimizations): Before: foc_nanos:13737 After: foc_nanos:12550 (approx 9% reduction in loopFOC time)
| } | ||
| // Normalize the shaft angle after each iteration to prevent it growing indefinitely | ||
| // and eventually losing precision. | ||
| shaft_angle = _normalizeAngle(shaft_angle); |
There was a problem hiding this comment.
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?
There was a problem hiding this comment.
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?
|
Okay, here is my suggestion. Let's drop all the parts of this commit that call _normalizeAngle in more places. Currently the only real place where _normalizeAngle is called on an accumulator is the shaft_angle of angle_openloop. I don't think it's very important to get high precision for very high angles here. If someone is willing to set a target angle above 50k, They will potentially lose some precision, but I don't think that is a typical use case, especially not in openloop mode. My upcoming changes to _sincos will require normalizing some things that aren't currently normalized, but we will do that closer to the callsite. wdyt? |
2 participants
Description
Previously _normalizeAngle used fmodf, which is expensive on Cortex-M series chips (40-100 cycles). For normalizing the angle, we can use a trick of just subtracking 2 pi times the number of full rotations from the angle.
This method does lose precision much faster than the fmodf approach. There will be significant error if the total number of radians passed in is greater than 10^5. However, we can counteract this by not passing in such a large number of radians -- basically making sure that we are not using an accumulating radian count in any location where the angle will eventually need to be normalized. This should typically be the case since accumulating radian use cases are definitionally not in need of normalization. We should consider adding an assert here to ensure that we catch any existing cases where we should be passing the mechanical or electrical angle and are instead passing an accumulator.
Performance test (on STM32G474, with certain other optimizations):
Before: foc_nanos:13737
After: foc_nanos:12550 (approx 9% reduction in loopFOC time)
Type of change
How Has This Been Tested?
I built an arduino-foc-based test program based on this commit and observed that the motor still worked fine. I also benchmarked the code and observed it to be faster. I have not tested all possible control modes so it is not impossible that this approach is unacceptable in some configurations. Please review carefully and suggest anything further that I should check.
Test Configuration/Setup: