58 lines
1.6 KiB
C++
58 lines
1.6 KiB
C++
#include "SmoothingSensor.h"
|
|
#include "common/foc_utils.h"
|
|
#include "common/time_utils.h"
|
|
|
|
|
|
SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m)
|
|
{
|
|
}
|
|
|
|
|
|
void SmoothingSensor::update() {
|
|
// Update sensor, with optional downsampling of update rate
|
|
if(sensor_cnt++ >= sensor_downsample) {
|
|
sensor_cnt = 0;
|
|
_wrapped.update();
|
|
}
|
|
|
|
// Copy state variables from the sensor
|
|
angle_prev = _wrapped.angle_prev;
|
|
angle_prev_ts = _wrapped.angle_prev_ts;
|
|
full_rotations = _wrapped.full_rotations;
|
|
|
|
// Perform angle prediction, using low-pass filtered velocity. But don't advance more than
|
|
// pi/3 (equivalent to one step of block commutation) from the last true angle reading.
|
|
float dt = (_micros() - angle_prev_ts) * 1e-6f;
|
|
angle_prev += _motor.sensor_direction * _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs);
|
|
|
|
// Apply phase correction if needed
|
|
if (phase_correction != 0) {
|
|
if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs;
|
|
else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs;
|
|
}
|
|
|
|
// Handle wraparound of the projected angle
|
|
if (angle_prev < 0) full_rotations -= 1, angle_prev += _2PI;
|
|
else if (angle_prev >= _2PI) full_rotations += 1, angle_prev -= _2PI;
|
|
}
|
|
|
|
|
|
float SmoothingSensor::getVelocity() {
|
|
return _wrapped.getVelocity();
|
|
}
|
|
|
|
|
|
int SmoothingSensor::needsSearch() {
|
|
return _wrapped.needsSearch();
|
|
}
|
|
|
|
|
|
float SmoothingSensor::getSensorAngle() {
|
|
return _wrapped.getSensorAngle();
|
|
}
|
|
|
|
|
|
void SmoothingSensor::init() {
|
|
_wrapped.init();
|
|
}
|