From e315496a0d7df3c9dbafcd6c2886f266437a4593 Mon Sep 17 00:00:00 2001 From: Gabriel Zerbib Date: Mon, 1 Jun 2026 14:55:56 +0200 Subject: [PATCH] Switch angle sensors to use integer instead of floating point --- src/common/base_classes/FOCMotor.cpp | 18 ++++++-- src/common/base_classes/Sensor.cpp | 68 +++++++++++++++++++++++++--- src/common/base_classes/Sensor.h | 37 +++++++++++++-- src/sensors/Encoder.cpp | 14 +++++- src/sensors/Encoder.h | 2 +- src/sensors/GenericSensor.cpp | 4 +- src/sensors/GenericSensor.h | 6 +-- src/sensors/HallSensor.cpp | 12 +++-- src/sensors/HallSensor.h | 2 +- src/sensors/MagneticSensorAnalog.cpp | 11 ++++- src/sensors/MagneticSensorAnalog.h | 2 +- src/sensors/MagneticSensorI2C.cpp | 9 +++- src/sensors/MagneticSensorI2C.h | 2 +- src/sensors/MagneticSensorPWM.cpp | 12 ++++- src/sensors/MagneticSensorPWM.h | 2 +- src/sensors/MagneticSensorSPI.cpp | 9 +++- src/sensors/MagneticSensorSPI.h | 2 +- 17 files changed, 177 insertions(+), 35 deletions(-) diff --git a/src/common/base_classes/FOCMotor.cpp b/src/common/base_classes/FOCMotor.cpp index febf91cd..6aab3b47 100644 --- a/src/common/base_classes/FOCMotor.cpp +++ b/src/common/base_classes/FOCMotor.cpp @@ -77,7 +77,11 @@ float FOCMotor::shaftVelocity() { float FOCMotor::electricalAngle(){ // if no sensor linked return previous value ( for open loop ) if(!sensor) return electrical_angle; - return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - zero_electric_angle ); + return _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() + #ifdef INTEGER_ANGLE + * _2PI / sensor->getStepsPerRevolution() + #endif + - zero_electric_angle ); } /** @@ -289,8 +293,16 @@ int FOCMotor::characteriseMotor(float voltage, float correction_factor=1.0f){ * We then report the one closest to the actual value. This could be useful if the zero search method is not reliable enough (eg. high pole count). */ - float estimated_zero_electric_angle_A = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle); - float estimated_zero_electric_angle_B = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() - d_electrical_angle + _PI); + float estimated_zero_electric_angle_A = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() + #ifdef INTEGER_ANGLE + * _2PI / sensor->getStepsPerRevolution() + #endif + - d_electrical_angle); + float estimated_zero_electric_angle_B = _normalizeAngle( (float)(sensor_direction * pole_pairs) * sensor->getMechanicalAngle() + #ifdef INTEGER_ANGLE + * _2PI / sensor->getStepsPerRevolution() + #endif + - d_electrical_angle + _PI); float estimated_zero_electric_angle = 0.0f; if (fabsf(estimated_zero_electric_angle_A - zero_electric_angle) < fabsf(estimated_zero_electric_angle_B - zero_electric_angle)) { diff --git a/src/common/base_classes/Sensor.cpp b/src/common/base_classes/Sensor.cpp index e58eeb05..271f4c3c 100644 --- a/src/common/base_classes/Sensor.cpp +++ b/src/common/base_classes/Sensor.cpp @@ -5,14 +5,11 @@ void Sensor::update() { - float val = getSensorAngle(); + angle_type val = getSensorAngle(); if (val<0) // sensor angles are strictly non-negative. Negative values are used to signal errors. return; // TODO signal error, e.g. via a flag and counter angle_prev_ts = _micros(); - float d_angle = val - angle_prev; - // if overflow happened track it as full rotation - if(abs(d_angle) > (0.8f*_2PI) ) full_rotations += ( d_angle > 0 ) ? -1 : 1; - angle_prev = val; + setAngleContinuous(val); } @@ -20,33 +17,57 @@ void Sensor::update() { float Sensor::getVelocity() { // calculate sample time // if timestamps were unsigned, we could get rid of this section, unsigned overflow handles it correctly + #ifdef INTEGER_ANGLE + angle_type Ts = angle_prev_ts - vel_angle_prev_ts; + #else float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6f; + #endif + #if 0 if (Ts < 0.0f) { // handle micros() overflow - we need to reset vel_angle_prev_ts vel_angle_prev = angle_prev; + #ifndef INTEGER_ANGLE vel_full_rotations = full_rotations; + #endif vel_angle_prev_ts = angle_prev_ts; return velocity; } + #endif if (Ts < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small // Calculate change in angle. Handles `full_rotations` integer wrap-arounds, // and avoids float precision loss issues by keeping numbers small. - float delta_angle = angle_prev - vel_angle_prev; + angle_type delta_angle = angle_prev - vel_angle_prev; + #ifndef INTEGER_ANGLE const int32_t delta_full_rotations = full_rotations - vel_full_rotations; if (delta_full_rotations) { delta_angle += delta_full_rotations * _2PI; } + #endif + #ifdef INTEGER_ANGLE + if (abs(delta_angle) > 0) + { + velocity = delta_angle * 1000000 * _2PI / (Ts * steps_per_revolution); + } + else + { + velocity = 0.0f; + } + + #else // floating point equality checks are bad, so instead we check that the angle change is very small if (fabsf(delta_angle) > 1e-8f) { velocity = delta_angle / Ts; } else { velocity = 0.0f; } + #endif // Always advance the velocity reference sample to avoid stale deltas/time windows. vel_angle_prev = angle_prev; + #ifndef INTEGER_ANGLE vel_full_rotations = full_rotations; + #endif vel_angle_prev_ts = angle_prev_ts; return velocity; @@ -68,26 +89,38 @@ void Sensor::init() { } -float Sensor::getMechanicalAngle() { +angle_type Sensor::getMechanicalAngle() { return angle_prev; } float Sensor::getAngle(){ + #ifdef INTEGER_ANGLE + return angle_prev / (float)steps_per_revolution * _2PI; + #else return (float)full_rotations * _2PI + angle_prev; + #endif } double Sensor::getPreciseAngle() { + #ifdef INTEGER_ANGLE + return angle_prev / (double)steps_per_revolution * _2PI; + #else return (double)full_rotations * (double)_2PI + (double)angle_prev; + #endif } int32_t Sensor::getFullRotations() { + #ifdef INTEGER_ANGLE + return angle_prev/steps_per_revolution; + #else return full_rotations; + #endif } @@ -95,3 +128,24 @@ int32_t Sensor::getFullRotations() { int Sensor::needsSearch() { return 0; // default false } + +void Sensor::setAngleContinuous(angle_type sensor_angle) +{ + #ifdef INTEGER_ANGLE + angle_type d_angle = sensor_angle - sensor_angle_prev; + if(abs(d_angle) > (steps_per_revolution/2) ) + { + d_angle += d_angle > 0 ? -steps_per_revolution : steps_per_revolution; + } + angle_prev += d_angle; + sensor_angle_prev = sensor_angle; + #else + angle_type d_angle = sensor_angle - angle_prev; + if(abs(d_angle) > (0.8f*_2PI) ) + { + full_rotations += ( d_angle > 0 ) ? -1 : 1; + } + angle_prev = sensor_angle; + #endif + +} \ No newline at end of file diff --git a/src/common/base_classes/Sensor.h b/src/common/base_classes/Sensor.h index adfdc986..aa59a2ee 100644 --- a/src/common/base_classes/Sensor.h +++ b/src/common/base_classes/Sensor.h @@ -21,6 +21,14 @@ enum Pullup : uint8_t { USE_EXTERN = 0x01 //!< Use external pullups }; +#define INTEGER_ANGLE + +#ifdef INTEGER_ANGLE +using angle_type = int; +#else +using angle_type = float; +#endif + /** * Sensor abstract class defintion * @@ -43,13 +51,15 @@ enum Pullup : uint8_t { */ class Sensor{ friend class SmoothingSensor; + friend class CalibratedSensor; + friend class HysteresisSensor; public: /** * Get mechanical shaft angle in the range 0 to 2PI. This value will be as precise as possible with * the hardware. Base implementation uses the values returned by update() so that * the same values are returned until update() is called again. */ - virtual float getMechanicalAngle(); + virtual angle_type getMechanicalAngle(); /** * Get current position (in rad) including full rotations and shaft angle. @@ -108,6 +118,10 @@ class Sensor{ */ float min_elapsed_time = 0.000100f; // default is 100 microseconds, or 10kHz + #ifdef INTEGER_ANGLE + int getStepsPerRevolution() const {return steps_per_revolution;}; + #endif + protected: /** * Get current shaft angle from the sensor hardware, and @@ -117,7 +131,8 @@ class Sensor{ * Calling this method directly does not update the base-class internal fields. * Use update() when calling from outside code. */ - virtual float getSensorAngle()=0; + virtual angle_type getSensorAngle()=0; + /** * Call Sensor::init() from your sensor subclass's init method if you want smoother startup * The base class init() method calls getSensorAngle() several times to initialize the internal fields @@ -126,14 +141,26 @@ class Sensor{ */ virtual void init(); + /** + * Set angle_prev and full_rotations from a sensor angle, handling sensor overflows as revolutions + */ + void setAngleContinuous(angle_type sensor_angle); + // velocity calculation variables float velocity=0.0f; - float angle_prev=0.0f; // result of last call to getSensorAngle(), used for full rotations and velocity - long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity + angle_type angle_prev=0.0f; // result of last call to getSensorAngle(), used for full rotations and velocity + unsigned long angle_prev_ts=0; // timestamp of last call to getAngle, used for velocity float vel_angle_prev=0.0f; // angle at last call to getVelocity, used for velocity - long vel_angle_prev_ts=0; // last velocity calculation timestamp + unsigned long vel_angle_prev_ts=0; // last velocity calculation timestamp + #ifdef INTEGER_ANGLE + private: + angle_type sensor_angle_prev = 0; + protected: + angle_type steps_per_revolution = 2; + #else int32_t full_rotations=0; // full rotation tracking int32_t vel_full_rotations=0; // previous full rotation value for velocity calculation + #endif }; #endif diff --git a/src/sensors/Encoder.cpp b/src/sensors/Encoder.cpp index fa4b8e73..7c1a75b0 100644 --- a/src/sensors/Encoder.cpp +++ b/src/sensors/Encoder.cpp @@ -19,6 +19,9 @@ Encoder::Encoder(int _encA, int _encB , float _ppr, int _index){ pulse_timestamp = 0; cpr = _ppr; + #ifdef INTEGER_ANGLE + steps_per_revolution = cpr; + #endif A_active = 0; B_active = 0; I_active = 0; @@ -106,16 +109,25 @@ void Encoder::update() { angle_prev_ts = pulse_timestamp; long copy_pulse_counter = pulse_counter; interrupts(); + #ifdef INTEGER_ANGLE + setAngleContinuous(copy_pulse_counter%steps_per_revolution); + #else // TODO: numerical precision issue here if the pulse_counter overflows the angle will be lost full_rotations = copy_pulse_counter / (int)cpr; angle_prev = _2PI * ((copy_pulse_counter) % ((int)cpr)) / ((float)cpr); + #endif + } /* Shaft angle calculation */ -float Encoder::getSensorAngle(){ +angle_type Encoder::getSensorAngle(){ + #ifdef INTEGER_ANGLE + return angle_prev % steps_per_revolution; + #else return _2PI * (pulse_counter) / ((float)cpr); + #endif } diff --git a/src/sensors/Encoder.h b/src/sensors/Encoder.h index ab494d13..ecaa14f6 100644 --- a/src/sensors/Encoder.h +++ b/src/sensors/Encoder.h @@ -60,7 +60,7 @@ class Encoder: public Sensor{ // Abstract functions of the Sensor class implementation /** get current angle (rad) */ - float getSensorAngle() override; + angle_type getSensorAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; virtual void update() override; diff --git a/src/sensors/GenericSensor.cpp b/src/sensors/GenericSensor.cpp index 3d4025f3..c70bda9d 100644 --- a/src/sensors/GenericSensor.cpp +++ b/src/sensors/GenericSensor.cpp @@ -6,7 +6,7 @@ - readCallback - pointer to the function which reads the sensor angle. */ -GenericSensor::GenericSensor(float (*readCallback)(), void (*initCallback)()){ +GenericSensor::GenericSensor(angle_type (*readCallback)(), void (*initCallback)()){ // if function provided add it to the if(readCallback != nullptr) this->readCallback = readCallback; if(initCallback != nullptr) this->initCallback = initCallback; @@ -21,6 +21,6 @@ void GenericSensor::init(){ /* Shaft angle calculation */ -float GenericSensor::getSensorAngle(){ +angle_type GenericSensor::getSensorAngle(){ return this->readCallback(); } \ No newline at end of file diff --git a/src/sensors/GenericSensor.h b/src/sensors/GenericSensor.h index ba0dfe5c..246d7308 100644 --- a/src/sensors/GenericSensor.h +++ b/src/sensors/GenericSensor.h @@ -14,16 +14,16 @@ class GenericSensor: public Sensor{ * @param readCallback pointer to the function reading the sensor angle * @param initCallback pointer to the function initialising the sensor */ - GenericSensor(float (*readCallback)() = nullptr, void (*initCallback)() = nullptr); + GenericSensor(angle_type (*readCallback)() = nullptr, void (*initCallback)() = nullptr); - float (*readCallback)() = nullptr; //!< function pointer to sensor reading + angle_type (*readCallback)() = nullptr; //!< function pointer to sensor reading void (*initCallback)() = nullptr; //!< function pointer to sensor initialisation void init() override; // Abstract functions of the Sensor class implementation /** get current angle (rad) */ - float getSensorAngle() override; + angle_type getSensorAngle() override; }; diff --git a/src/sensors/HallSensor.cpp b/src/sensors/HallSensor.cpp index 64c5e48c..7d6007ef 100644 --- a/src/sensors/HallSensor.cpp +++ b/src/sensors/HallSensor.cpp @@ -15,6 +15,9 @@ HallSensor::HallSensor(int _hallA, int _hallB, int _hallC, int _pp){ // hall has 6 segments per electrical revolution cpr = _pp * 6; + #ifdef INTEGER_ANGLE + steps_per_revolution = cpr; + #endif // extern pullup as default pullup = Pullup::USE_EXTERN; @@ -108,8 +111,7 @@ void HallSensor::update() { long last_electric_rotations = electric_rotations; int8_t last_electric_sector = electric_sector; if (use_interrupt) interrupts(); - angle_prev = ((float)((last_electric_rotations * 6 + last_electric_sector) % cpr) / (float)cpr) * _2PI ; - full_rotations = (int32_t)((last_electric_rotations * 6 + last_electric_sector) / cpr); + setAngleContinuous((last_electric_rotations * 6 + last_electric_sector) % cpr); } @@ -118,8 +120,12 @@ void HallSensor::update() { Shaft angle calculation TODO: numerical precision issue here if the electrical rotation overflows the angle will be lost */ -float HallSensor::getSensorAngle() { +angle_type HallSensor::getSensorAngle() { + #ifdef INTEGER_ANGLE + return electric_rotations * 6 + electric_sector; + #else return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ; + #endif } /* diff --git a/src/sensors/HallSensor.h b/src/sensors/HallSensor.h index 94053e0f..d5314bec 100644 --- a/src/sensors/HallSensor.h +++ b/src/sensors/HallSensor.h @@ -57,7 +57,7 @@ class HallSensor: public Sensor{ /** Interrupt-safe update */ void update() override; /** get current angle (rad) */ - float getSensorAngle() override; + angle_type getSensorAngle() override; /** get current angular velocity (rad/s) */ float getVelocity() override; diff --git a/src/sensors/MagneticSensorAnalog.cpp b/src/sensors/MagneticSensorAnalog.cpp index a388f0c1..cedbbb45 100644 --- a/src/sensors/MagneticSensorAnalog.cpp +++ b/src/sensors/MagneticSensorAnalog.cpp @@ -10,6 +10,9 @@ MagneticSensorAnalog::MagneticSensorAnalog(uint8_t _pinAnalog, int _min_raw_coun pinAnalog = _pinAnalog; cpr = _max_raw_count - _min_raw_count + 1; + #ifdef INTEGER_ANGLE + steps_per_revolution = cpr; + #endif min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; @@ -30,10 +33,14 @@ void MagneticSensorAnalog::init(){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorAnalog::getSensorAngle(){ +angle_type MagneticSensorAnalog::getSensorAngle(){ // raw data from the sensor - raw_count = getRawCount(); + raw_count = getRawCount(); + #ifdef INTEGER_ANGLE + return raw_count - min_raw_count; + #else return ( (float) (raw_count - min_raw_count) / (float)cpr) * _2PI; + #endif } // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorAnalog.h b/src/sensors/MagneticSensorAnalog.h index 6f787b95..c36ffafb 100644 --- a/src/sensors/MagneticSensorAnalog.h +++ b/src/sensors/MagneticSensorAnalog.h @@ -29,7 +29,7 @@ class MagneticSensorAnalog: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getSensorAngle() override; + angle_type getSensorAngle() override; /** raw count (typically in range of 0-1023), useful for debugging resolution issues */ int raw_count; diff --git a/src/sensors/MagneticSensorI2C.cpp b/src/sensors/MagneticSensorI2C.cpp index 9298413a..c8e14d57 100644 --- a/src/sensors/MagneticSensorI2C.cpp +++ b/src/sensors/MagneticSensorI2C.cpp @@ -53,6 +53,9 @@ MagneticSensorI2C::MagneticSensorI2C(uint8_t _chip_address, int _bit_resolution, _conf.lsb_shift = 0; _conf.msb_shift = lsb_used; + #ifdef INTEGER_ANGLE + steps_per_revolution = _powtwo(_bit_resolution); + #endif cpr = _powtwo(_bit_resolution); wire = &Wire; @@ -84,9 +87,13 @@ void MagneticSensorI2C::init(TwoWire* _wire){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorI2C::getSensorAngle(){ +angle_type MagneticSensorI2C::getSensorAngle(){ // (number of full rotations)*2PI + current sensor angle + #ifdef INTEGER_ANGLE + return getRawCount(); + #else return ( getRawCount() / (float)cpr) * _2PI ; + #endif } diff --git a/src/sensors/MagneticSensorI2C.h b/src/sensors/MagneticSensorI2C.h index 381a950a..8f0aff25 100644 --- a/src/sensors/MagneticSensorI2C.h +++ b/src/sensors/MagneticSensorI2C.h @@ -50,7 +50,7 @@ class MagneticSensorI2C: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getSensorAngle() override; + angle_type getSensorAngle() override; /** experimental function to check and fix SDA locked LOW issues */ int checkBus(byte sda_pin , byte scl_pin ); diff --git a/src/sensors/MagneticSensorPWM.cpp b/src/sensors/MagneticSensorPWM.cpp index 04b7cc75..19d475a5 100644 --- a/src/sensors/MagneticSensorPWM.cpp +++ b/src/sensors/MagneticSensorPWM.cpp @@ -11,6 +11,9 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int _min_raw_count, int _m pinPWM = _pinPWM; cpr = _max_raw_count - _min_raw_count + 1; + #ifdef INTEGER_ANGLE + steps_per_revolution = cpr; + #endif min_raw_count = _min_raw_count; max_raw_count = _max_raw_count; @@ -39,6 +42,9 @@ MagneticSensorPWM::MagneticSensorPWM(uint8_t _pinPWM, int freqHz, int _total_pwm min_raw_count = lroundf(1000000.0f/freqHz/_total_pwm_clocks*_min_pwm_clocks); max_raw_count = lroundf(1000000.0f/freqHz/_total_pwm_clocks*_max_pwm_clocks); cpr = max_raw_count - min_raw_count + 1; + #ifdef INTEGER_ANGLE + steps_per_revolution = cpr; + #endif // define if the sensor uses interrupts is_interrupt_based = false; @@ -72,12 +78,16 @@ void MagneticSensorPWM::update() { } // get current angle (rad) -float MagneticSensorPWM::getSensorAngle(){ +angle_type MagneticSensorPWM::getSensorAngle(){ // raw data from sensor raw_count = getRawCount(); if (raw_count > max_raw_count) raw_count = max_raw_count; if (raw_count < min_raw_count) raw_count = min_raw_count; + #ifdef INTEGER_ANGLE + return raw_count-min_raw_count; + #else return( (float) (raw_count - min_raw_count) / (float)cpr) * _2PI; + #endif } diff --git a/src/sensors/MagneticSensorPWM.h b/src/sensors/MagneticSensorPWM.h index a5fd7e6e..1095467f 100644 --- a/src/sensors/MagneticSensorPWM.h +++ b/src/sensors/MagneticSensorPWM.h @@ -37,7 +37,7 @@ class MagneticSensorPWM: public Sensor{ void update() override; // get current angle (rad) - float getSensorAngle() override; + angle_type getSensorAngle() override; // pwm handler void handlePWM(); diff --git a/src/sensors/MagneticSensorSPI.cpp b/src/sensors/MagneticSensorSPI.cpp index baaab2de..94f991e2 100644 --- a/src/sensors/MagneticSensorSPI.cpp +++ b/src/sensors/MagneticSensorSPI.cpp @@ -56,6 +56,9 @@ MagneticSensorSPI::MagneticSensorSPI(int cs, int _bit_resolution, int _angle_reg // angle read register of the magnetic sensor angle_register = _angle_register ? _angle_register : DEF_ANGLE_REGISTER; // register maximum value (counts per revolution) + #ifdef INTEGER_ANGLE + steps_per_revolution = _powtwo(_bit_resolution); + #endif cpr = _powtwo(_bit_resolution); spi_mode = SPI_MODE1; clock_speed = _isset(_clock_speed) ? _clock_speed : 1000000; @@ -100,8 +103,12 @@ void MagneticSensorSPI::init(SPIClass* _spi){ // Shaft angle calculation // angle is in radians [rad] -float MagneticSensorSPI::getSensorAngle(){ +angle_type MagneticSensorSPI::getSensorAngle(){ + #ifdef INTEGER_ANGLE + return getRawCount(); + #else return (getRawCount() / (float)cpr) * _2PI; + #endif } // function reading the raw counter of the magnetic sensor diff --git a/src/sensors/MagneticSensorSPI.h b/src/sensors/MagneticSensorSPI.h index 6c07573c..3868facb 100644 --- a/src/sensors/MagneticSensorSPI.h +++ b/src/sensors/MagneticSensorSPI.h @@ -48,7 +48,7 @@ class MagneticSensorSPI: public Sensor{ // implementation of abstract functions of the Sensor class /** get current angle (rad) */ - float getSensorAngle() override; + angle_type getSensorAngle() override; // returns the spi mode (phase/polarity of read/writes) i.e one of SPI_MODE0 | SPI_MODE1 | SPI_MODE2 | SPI_MODE3 int spi_mode;