SmoothingSensor: Experimental sensor angle extrapoltion

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
3 Likes

I know I said I was going to stop working on this thing, but insomnia is the mother of invention, so it’s refactoring time already.

While trying unsuccessfully to sleep, I made several realizations:

  • Having SmoothingHallSensor inherit from SmoothingSensor is dumb. The wrapper approach was Runger’s idea to make it generic, but if I’m making a specialized version anyway it would be better off as its own class. This also saves a few CPU cycles because projectedAngle() no longer needs to be virtual.

  • The enable_smoothing variable is a waste of CPU cycles because if you want to disable smoothing you can just call motor.linkSensor with the wrapped sensor. But because SmoothingHallSensor will no longer be a wrapper, it will still need it.

  • Instead of a simple voltage cutoff for the phase correction, I can use the kv and applied voltage to calculate the no load speed and apply it proportionally. Also it needs to be multiplied by sensor direction.

  • The velocity threshold is still useful because I can really improve low speed motion by switching to a pseudo-open-loop mode when it gets below the speed where it transitions from continuous motion to moving in cogging steps.

So that will be my task for today. Or perhaps tomorrow because there’s a pretty good chance I’ll end up spending all my time helping my dad sort out a perplexing car problem and then fall asleep.

Can also spiral out of hand. Be good to yourself. I was able to quit my sleeping-pharma when I re-discovered Chamomile tè. Especially the pure flower (organic), bag it yourself kind.

Well, I think it’s time to admit defeat for the time being. There are no improvements in functionality of this code, only elegance.

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(); }
  float getMechanicalAngle() { return _normalizeAngle(wrappedSensor->getMechanicalAngle() + projectedAngle()); }
  float getAngle() { wrappedSensor->getAngle() + projectedAngle(); }
  double 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();
  }

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

protected:
  float projectedAngle() { return motor->shaft_velocity * (_micros() - wrappedSensor->angle_prev_ts) * 1e-6f; }
  float getSensorAngle() { return wrappedSensor->getSensorAngle(); }
  void init() { wrappedSensor->init(); }
  
  Sensor *wrappedSensor;
  const FOCMotor *motor;
};

class SmoothingHallSensor : public HallSensor
{
public:
  SmoothingHallSensor(int encA, int encB, int encC, int pp, const FOCMotor *m):
    HallSensor(encA, encB, encC, pp), motor(m) {}

  float getMechanicalAngle() { return _normalizeAngle(HallSensor::getMechanicalAngle() + projectedAngle()); }
  float getAngle() { return  HallSensor::getAngle() + projectedAngle(); }
  double getPreciseAngle() { return HallSensor::getPreciseAngle() + projectedAngle(); }
  int32_t getFullRotations() { float angle = getAngle(); return lroundf((angle - _normalizeAngle(angle)) / _2PI); }

  bool enable_smoothing = true;
  
  // 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() {
    if (enable_smoothing == false) return 0;
    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() - pulse_timestamp) * 1e-6f;
  }
  const FOCMotor *motor;
};

At least with hall sensors, the only thing it’s really good for is continuous running velocity mode. If I slow the rotor down with my fingers it becomes twitchy, whereas with un-smoothed hall sensors it continues running smoothly even down to near-zero speed. And in angle mode it twitches regularly due to its own inertia. I haven’t narrowed down exactly what is the sequence of feedbacks that causes it, so it’s possible it could be eliminated with more effort.

Using open loop for precise angle control is promising, but the twitching issue applies to it too, as it twitches when transitioning back to closed loop control. The current implementation is rather hacky, having the sensor monkey with the motor’s variables. And as-is it only works in velocity or angle mode with no hardware current sense. This code also includes my idea of applying phase correction proportional to shaft speed relative to no-load speed, but it didn’t make any noticeable difference compared to the simple velocity threshold.

class SmoothingHallSensor : public HallSensor
{
public:
  SmoothingHallSensor(int encA, int encB, int encC, int pp, FOCMotor *m):
    HallSensor(encA, encB, encC, pp), motor(m) {}

  float getMechanicalAngle() {
    return open_loop_active ? _normalizeAngle(motor->shaft_angle) :
      _normalizeAngle(HallSensor::getMechanicalAngle() + projectedAngle());
  }
  float getAngle() {
    return open_loop_active ? motor->shaft_angle :
      HallSensor::getAngle() + projectedAngle();
  }
  double getPreciseAngle() {
    return open_loop_active ? motor->shaft_angle :
      HallSensor::getPreciseAngle() + projectedAngle();
  }
  int32_t getFullRotations() {
    float angle = getAngle();
    return lroundf((angle - _normalizeAngle(angle)) / _2PI);
  }

  void update() {
    // At low speed if motor is not lagging significantly behind sensor angle, use pseudo-open-loop mode
    if (open_loop_active)
    {
      if (!enable_smoothing ||
          fabs(motor->shaft_velocity) > velocity_threshold * 1.1 ||
          fabs(motor->shaft_velocity_sp) > velocity_threshold * 1.1 ||
          fabs(motor->shaft_angle - HallSensor::getAngle()) > _PI / motor->pole_pairs)
        open_loop_active = false;
    }
    else if (enable_smoothing && fabs(motor->shaft_velocity) < velocity_threshold && fabs(motor->shaft_velocity_sp) < velocity_threshold)
      open_loop_active = true;
    
    if (open_loop_active)
    {
      // get current timestamp
      unsigned long now_us = _micros();
      // calculate the sample time from last call
      float Ts = (now_us - open_loop_timestamp) * 1e-6f;
      // quick fix for strange cases (micros overflow + timestamp not defined)
      if(Ts <= 0 || Ts > 0.5f) Ts = 1e-3f;
      motor->shaft_angle += motor->shaft_velocity_sp * Ts;
      open_loop_timestamp = now_us;
      // Drive motor at half of voltage/current limit, to reduce heating
      motor->voltage.q = !_isset(motor->phase_resistance) ? motor->voltage_limit / 2 :
        _constrain(motor->current_limit*motor->phase_resistance/2,-motor->voltage_limit, motor->voltage_limit);
    }
  }

  bool enable_smoothing = true;
  
  // 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.
  // If the motor has its KV_rating set, it will be applied proportionally to the shaft velocity relative
  // to no-load speed for the applied voltage, improving efficiency during acceleration.
  float phase_correction = -_PI_6;

  // Shaft speed below which the sensor will switch to operating in a pseudo-open-loop mode, enabling 
  // precise angle control despite being unable to actually sense it. This threshold will likely need to be tuned.
  float velocity_threshold = 10;
  bool open_loop_active = false;
  long open_loop_timestamp;
  
protected:
  float projectedAngle() {
    if (enable_smoothing == false) return 0;

    float offset = phase_correction / motor->pole_pairs;
    if (_isset(motor->KV_rating)) {
      float no_load_speed = motor->KV_rating * fabs(motor->voltage.q) * _RPM_TO_RADS;
      if (fabs(motor->shaft_velocity) < no_load_speed)
        return (offset / no_load_speed + 1) * motor->shaft_velocity * (_micros() - pulse_timestamp) * 1e-6f;
    }
    if (motor->shaft_velocity < 0) offset = -offset;
    return offset + motor->shaft_velocity * (_micros() - pulse_timestamp) * 1e-6f;
  }
  FOCMotor *motor;
};
1 Like

Can you give us some example code that uses this? There’s a couple outstanding questions that would be answered by this. I’m trying to make a fan, so this should be good for me, otherwise I can hack up the library to do this projected sensor thing maybe or something. It’s important for a pwm based sensor, which I am ending up using because you don’t get that many updates per second with one.

Create the wrapped sensor like normal, pass it to the SmoothingSensor, and link the SmoothingSensor to the motor. Also set sensor_downsample to something other than 0 or it will sample the sensor every update like usual.

MagneticSensorPWM pwmSensor(your parameters here);
SmoothingSensor(&pwmSensor, &motor);

void setup() {
...
  motor.linkSensor(&smoothingSensor);
  smoothingSensor.sensor_downsample = something;

Now that I think about it, I wonder if it would be better to sample the sensor on a set time interval rather than a set fraction of loop cycles. Something like

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(); }
  float getMechanicalAngle() { return _normalizeAngle(wrappedSensor->getMechanicalAngle() + projectedAngle()); }
  float getAngle() { wrappedSensor->getAngle() + projectedAngle(); }
  double getPreciseAngle() { wrappedSensor->getPreciseAngle() + projectedAngle(); }
  int32_t getFullRotations() { float angle = getAngle(); return lroundf((angle - _normalizeAngle(angle)) / _2PI); }
  
  void update() {
    unsigned long now_us = _micros();
    if (now_us - sample_timestamp >= sample_interval || 
      now_us < sample_timestamp) { // _micros() overflow
    sample_timestamp = now_us;
    wrappedSensor->update();
  }

  unsigned long sample_interval = 0; // how often to sample the sensor, in microseconds

protected:
  float projectedAngle() { return motor->shaft_velocity * (_micros() - wrappedSensor->angle_prev_ts) * 1e-6f; }
  float getSensorAngle() { return wrappedSensor->getSensorAngle(); }
  void init() { wrappedSensor->init(); }
  
  Sensor *wrappedSensor;
  const FOCMotor *motor;
  unsigned long sample_timestamp; // Value of _micros() when sensor was last read
};
1 Like

Why this? It seems to me this would introduce some additional latency, and make the wrapped updates less “regular”, but I guess since the expected frequency of calls to the wrapping sensor is much larger than the update frequency the difference would not be big. If there are a lot of comms going on (making the loop cycles irregular) then it could improve things, of course. Hard to call, somehow.

If you like I can try to integrate this into the drivers repository? Or would you prefer to send a pull request on GitHub?

1 Like

I was imagining with a fast CPU and slow sensor, you might complete several loop iterations in less time that it takes to read the sensor, so even with downsampling you’re still spending more time on sensor reading than you are running the motor. But with interval setting, you could measure the sensor time and then decide what percentage of your total time you want to spend on it.

But it’s probably 6 of one, half dozen of the other… if the sensor is that slow, the motor won’t run smoothly nomatter what you do, due to the stator field being stalled during the sensor wait.

Let me do a pull request… that will give me a kick in the rear to finally learn how to use github :slight_smile: And there are a few tweaks to other sensor classes to get the best quality timestamps for prediction.

Awesome, if you have any questions let me know…

Please make your changes against a fresh pull of the dev branch of our drivers repository - in that way it should be easy to just merge it in.

In terms of how it works, you need access to the angle_prev_ts field, so I think we should probably just make the field public in Sensor. Using the friend class would mean we’d have to put it in the main library, which I would avoid for the moment.

In terms of the HallSensor, it might be better to make a separate subclass SmoothingHallSensor to deal with the differences in timestamp handling?

For the encoder, I don’t see much benefit to the smoothing - except possibly at low speeds?

1 Like

I could go either way on the friend class. It’s done by forward declaration, so the actual SmoothingSensor class doesn’t have to be included in the main branch. And the init function would also need to be made public without it.

Where should I put SmoothingSensor in the drivers repository? Utilities?

I’ve done a little more studying of its behavior, and understand it better now. The major twitchiness when slowing the motor down by force is due to the predicted angle getting far enough ahead of reality that the electrical angle wraps around and starts pulling the rotor in the opposite direction (similar to missing steps in open loop). I think that can only happen during deceleration, so second-order prediction may fix it. But a simpler method is to constrain the projection to ±_PI_6/pole_pairs, i.e. don’t project more than one hall sensor step ahead from the last reading. I think that’s a reasonable restriction for any kind of sensor in any situation.

Another problem is due to interrupts. See if you can spot it :slight_smile:

  float getAngle() { return wrappedSensor->getAngle() + projectedAngle(); }

If a sensor update interrupt happens in the middle of those two calls, the projection is done relative to the new angle (near-zero delta time), but added to the old angle (which should have had large delta), so you end up pulling the motor backward until the next update. I can change it to

  float getAngle() {
    noInterupts();
    float angle = wrappedSensor->getAngle() + projectedAngle();
    interrupts();
    return angle;
  }

But then it will be important that it’s never called from inside an interrupt, else you’ll have interrupts enabled inside an interrupt, and nested interrupts are serious business. I’ve done it before, but it requires a very carefully written interrupt handler, and I doubt the ones for all the CPUs supported by Arduino are made for it. An interrupt disable function is supposed to return the previous state, so you can restore to that after your critical section rather than unconditionally enabling interrupts. But apparently whoever designed the Arduino library didn’t know what they were doing because noInterrupts() doesn’t return anything, and there doesn’t appear to be a function to check the current state either…

And in fact this is an issue for HallSensor itself,

float HallSensor::getAngle() {
  return ((float)(electric_rotations * 6 + electric_sector) / (float)cpr) * _2PI ;
}

That may get electric_rotations and electric_sector from different updates. But if I add interrupt disable/enable around that, then interrupts will be enabled in the middle of the SmoothingSensor::getAngle’s critical section. This is terrible. I think HallSensor is going to need some major rework even without SmoothingSensor.

MagneticSensorPWM updates via interrupts, but I think it’s ok because it only sets the pulse duration in the interrupt, and the actual angle is set from that in the base Sensor::update which won’t be happening in the middle of anything else.

I don’t think there are any other sensors that update via interrupts, so if I fix HallSensor then I think SmoothingSensor will be ok without disabling/enabling interrupts.