sensor->getVelocity() returns inf occasionally in FOCMotor::shaftVelocity()

developement
#1

Hi,

I am using a B-G431B-ESC1 board and a motor with hall sensor. Most of the time everything is ok, but occasionally everything breaks down (position control using current). After debugging a bit I found that sensor->getVelocity sometimes returns an inf which kills everything afterwards. I used the following modified code for this:

float Sensor::getVelocity() {
    // calculate sample time
    float Ts = (angle_prev_ts - vel_angle_prev_ts)*1e-6;
    // quick fix for strange cases (micros overflow)
    if(Ts <= 0) Ts = 1e-3f;
    // velocity calculation
    float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;  
    // save variables for future pass
    vel_angle_prev = angle_prev;
    vel_full_rotations = full_rotations;
    vel_angle_prev_ts = angle_prev_ts;
    if (isnan(vel) || isinf(vel))
    {
        Serial2.printf("Invalid speed in Sensor::getVelocity()!\n");
    }  
    return vel;
}

float FOCMotor::shaftVelocity() {
  // if no sensor linked return previous value ( for open loop )
  if(!sensor) return shaft_velocity;
  float V = sensor->getVelocity();
  if (isnan(V))
  {
      Serial2.printf("Invalid shaft_velocity nan!\n");
      _delay(500);
      HAL_NVIC_SystemReset();
  }
  if (isinf(V))
  {
      Serial2.printf("Invalid shaft_velocity inf!\n");
      _delay(500);
      HAL_NVIC_SystemReset();   //****** Code runs to this!
  }
  V = sensor_direction*LPF_velocity(V);
 return V;
}

As you can see, the error happens when the return value of getVelocity() is transfered. I fear this has something to do with floating point registers and compiler optimizations. I looked at the disassembly, but have far too little knowledge of the ARM architecture to really understand it.
To reproduce the behavior I have to frequently change the position (values between -100 and 100) and play with different P values of the angle PID. It happens on average after approximately 30 position changes and it is independent of the actual values. Any ideas?

In my code I am also using the modifications from this thread: B-G431B-ESC1: ADC reading of Temp, pot, VBUS while foc_current. Could it be related?

  • Chris
Problem with B-G431B-ESC1 and SimpleFOC V2.2.2
#2

Dear Grizzly,

I don’t read this as being compiler-related, although of course its always possible.

I think Ts is working out to be zero or NaN - we’ll have to look into why.

There are also some other issues with this code, I was planning to improve it later in the year, but at the moment I’m busy with other tasks…

Since you’re modifying the code anyway, I would try this:

...
float vel = ( (float)(full_rotations - vel_full_rotations)*_2PI + (angle_prev - vel_angle_prev) ) / Ts;  
if (isnan(vel) || isinf(vel))
  return lastVel;
lastVel = vel;
vel_angle_prev = angle_prev;
vel_full_rotations = full_rotations;
vel_angle_prev_ts = angle_prev_ts;
...

and introduce a new field in Sensor.h, lastVel.

Basically, if the result is NaN, use the previous result, and hope the calculation works out better next time. You could also add a check to make sure this isn’t happening too often in a row.

#3

Thanks for the proposal, I will try something along these lines. The real cause however, must be something else. If you look at my version of he code, you will see that right before the return statement in sensor::getVelocity(), I am checking for inf/nan and the code never fails here. The first thing after the return is again a check for inf/nan and this one fails. I only know such behaviour when local variables were stored in e.g. FPU registers with N+x bits and converted to N bits when they are transferred to the calling function (e.g. on the stack).

  • Chris
#4

Huh, you’re right I misunderstood you there!

In this case you are correct, there is something going on here on a deeper level.
STM32G431 has a math co-processor, there could be an issue here regarding the stack frame unwinding, in which case it could help to move the value to a temporary variable in the called function before returning it. You’d have to mark it up somehow (or use it) to make sure the compiler doesn’t optimize it away again.
Or it could be to do with a interrupt that happens and isn’t handling the stack and registers right. That would be more of a framework issue, and hard to test for, but consistent with the fact that it happens sporadically…

I wonder what’s up. This particular issue hasn’t been reported before by G431 users as far as I am aware.

#5

Well, I am always the one stumbling across such things. Anyway, I modified shaftVelocity() as below and it helps to avoid the negative effects, but the problem is of course still there. I also tried to fix it by changing the comparison in sensor::getVelocity() to if(Ts <= 1e-3) Ts = 1e-3f; but it didn’t help either (but still I think it is a good idea to compare to something slightly larger than 0.0…).

I had tried the thing with assigning vel already yesterday, actually, I made it a class member, but it did not help either. Fooling compilers is difficult today though.

// shaft velocity calculation
float FOCMotor::shaftVelocity() {
  // if no sensor linked return previous value ( for open loop )
  if(!sensor) return shaft_velocity;
  float V = sensor->getVelocity();
  if (isinf(V))
  {
      V = lastVelocity;
      Serial2.printf("Inf detected in FOCMotor::shaftVelocity(), using last velocity=%.1f instead\n", V);
  }
  lastVelocity = V;
  V = sensor_direction*LPF_velocity(V);
 return V;
}