[BUG] Issue with motor directions

[FrankvVeelen]

It seems there is an issue with motor directions in the control/PID.

I have a custom position sensor (based on an IMU) which I've validated to output values around pi. I've implemented this using GenericSensor. When I have a motor that is CW (sensor_direction == 1) everything works fine, the direction is found by the initFOC function and the control is smooth. When I take the same setup and I flip the direction of the position sensor (still centered around pi), the initFOC correctly detects movement to be CCW, but then the shaft_velocity_sp is always at it's maximum (2pi). It seems to me something does not take sensor_direction into account.

I use a rp2040, with BMI088 as position sensor. No current sense is used. I use a BLDC motor in angle control, with SpaceVectorPWM modulation.