SmoothingSensor: Experimental sensor angle extrapoltion

developement
11

SmoothingSensor works quite well now after fixing the pervasive interrupt problems (see Refactoring interrupt-based sensor code · Issue #270 · simplefoc/Arduino-FOC · GitHub) and a bad condition in HallSensor::getVelocity, if(_micros() - pulse_timestamp > last_pulse_diff) which would return zero velocity any time the motor decelerates at all. I changed it to pulse_diff*2 so it usually won’t trigger unless the motor has actually been stopped for a short while.

In line with the new approach of copying the volatile interrupt variables into the base Sensor state variables during update(), I’ve changed SmoothingSensor to perform its prediction once during update() and use that for the duration of the frame.

Hall sensors do still need the phase correction (-pi/6). With it, I get more or less the same top speed as non-smoothed. Without it, top speed is 15% higher due to field weakening. I’m going to put it in the one class instead of having a separate SmoothingHallSensor. It seems to work fine with the full offset applied even at near-zero speed, so I removed the velocity threshold.

Low speed behavior is similar with or without smoothing. Below about 15rad/s (150rpm) is stop-and-go rather than continuous turning. And there’s no more twitching when slowing the rotor down with my fingers or running in angle mode (at least, no more than there is without smoothing). In all cases, there’s less vibration and noise than HallSensor alone.

I’d still like to do the pseudo-open-loop for precise positioning between hall steps, but I think that will need some elaborate logic to gradually transition into and out of open loop mode to prevent twitching. I’ll call it done for now, and write up some documentation and an example project and see if we can get it integrated into the dev branch.

class SmoothingSensor : public Sensor
{
public:
  SmoothingSensor(Sensor *s, const FOCMotor *m):wrappedSensor(s),motor(m){}

  float getVelocity() { return wrappedSensor->getVelocity(); }
  int needsSearch() { return wrappedSensor->needsSearch(); }

  void update() {
    if(sensor_cnt++ >= sensor_downsample) {
      sensor_cnt = 0;
      wrappedSensor->update();
    }
    
    // Copy state variables from the sensor
    angle_prev = wrappedSensor->angle_prev;
    angle_prev_ts = wrappedSensor->angle_prev_ts;
    full_rotations = wrappedSensor->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 angle reading.
    float dt = (_micros() - angle_prev_ts) * 1e-6f;
    angle_prev += _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 -= phase_correction / motor->pole_pairs;
      else if (motor->shaft_velocity > 0) angle_prev += 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;
  }

  unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update
  unsigned int sensor_cnt = 0; // counting variable for downsampling

  // For hall sensors, 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 (set to -PI_6) to get the average in phase with the rotor
  float phase_correction = 0;//-_PI_6;

protected:
  float getSensorAngle() { return wrappedSensor->getSensorAngle(); }
  void init() { wrappedSensor->init(); }
  
  Sensor *wrappedSensor;
  const FOCMotor *motor;
};
3 Likes