Branching off from discussion in this thread Programming the Lepton from start to finish - #100 by dekutree64
I’ve been working on a method to get better performance from low resolution or slow updating sensors, by using the low-pass filtered shaft velocity to predict the true angle from the time elapsed since the last update.
So far the results are somewhat promising, however it needs more testing to really verify its usefulness. I only have digital hall sensors to work with, and they are a fair bit different from other sensors.
Here is the present state of the code:
class SmoothingSensor : public Sensor
{
public:
SmoothingSensor(Sensor *s, const FOCMotor *m):wrappedSensor(s),motor(m){}
void init() { wrappedSensor->init(); }
float getVelocity() { return wrappedSensor->getVelocity(); }
int needsSearch() { return wrappedSensor->needsSearch(); }
float getMechanicalAngle() {
return enable_smoothing == false ? wrappedSensor->getMechanicalAngle() :
_normalizeAngle(wrappedSensor->getMechanicalAngle() + projectedAngle());
}
float getAngle() {
return enable_smoothing == false ? wrappedSensor->getAngle() :
wrappedSensor->getAngle() + projectedAngle();
}
double getPreciseAngle() {
return enable_smoothing == false ? wrappedSensor->getPreciseAngle() :
wrappedSensor->getPreciseAngle() + projectedAngle();
}
int32_t getFullRotations() {
float angle = getAngle();
return lroundf((angle - _normalizeAngle(angle)) / _2PI);
}
void update() {
if(sensor_cnt++ < sensor_downsample) return;
sensor_cnt = 0;
wrappedSensor->update();
}
bool enable_smoothing = true;
unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update
unsigned int sensor_cnt = 0; // counting variable for downsampling
protected:
virtual float projectedAngle() { return motor->shaft_velocity * (_micros() - wrappedSensor->angle_prev_ts) / 1000000; }
float getSensorAngle() { return wrappedSensor->getSensorAngle(); }
Sensor *wrappedSensor;
const FOCMotor *motor;
};
And to deal with a couple of problems specific to hall sensors, I’ve also written this class:
class SmoothingHallSensor : public SmoothingSensor
{
public:
SmoothingHallSensor(int encA, int encB, int encC, int pp, const FOCMotor *m):
SmoothingSensor(&hallSensor, m), hallSensor(encA, encB, encC, pp) {}
// Pass-through functions to the hallSensor. You can use these or call the hallSensor's functions directly
void handleA() { hallSensor.handleA(); }
void handleB() { hallSensor.handleB(); }
void handleC() { hallSensor.handleC(); }
void enableInterrupts(void (*doA)() = nullptr, void(*doB)() = nullptr, void(*doC)() = nullptr)
{ hallSensor.enableInterrupts(doA, doB, doC); }
void attachSectorCallback(void (*onSectorChange)(int a) = nullptr)
{ hallSensor.attachSectorCallback(onSectorChange); }
HallSensor hallSensor;
// The predicted angle is always 0 to 60 electrical degrees ahead of where it would be without
// smoothing, so offset it backward by 30 degrees to get back approximately in phase with the rotor
float phase_correction = -_PI_6;
float velocity_threshold = 5; // Shaft speed below which phase_correction will not be applied
protected:
float projectedAngle() {
float offset = 0;
if (motor->shaft_velocity < -velocity_threshold) offset = -phase_correction / motor->pole_pairs;
else if (motor->shaft_velocity > velocity_threshold) offset = phase_correction / motor->pole_pairs;
return offset + motor->shaft_velocity * (_micros() - hallSensor.pulse_timestamp) / 1000000;
}
};
It uses hallSensor.pulse_timestamp to make the prediction, rather than angle_prev_ts which is not used by the HallSensor class. And it attempts to correct for the fact that extrapolating from hall state changes results in a field weakening effect (higher top speed but lower torque) because the predicted angle is always 0 to 60 electrical degrees ahead of where it would be without smoothing.
To use these requires 3 other changes:
- the Sensor base class must declare SmoothingSensor a friend class so it can access the protected functions and angle_prev_ts
- HallSensor must declare SmoothingHallSensor a friend
- change HallSensor’s pulse_timestamp to protected rather than private
And there are a few tweaks to other sensor classes to get the best quality timestamps for prediction.
