Sensor update taking too long with AS5600 - causing vibration

I’m trying to use a B-G431B-ESC1 board to drive a fan motor quietly. I have been able to ascertain, by modifying the code and listening to the frequency, that there is a noise caused by the sensor update over I2C using an AS5600 sensor… I thought it was working with a different motor but not so much, probably the noise was being masked or something.

Looking at the code, it appears that the sensor.update() sits there and waits for the info to come back. This causes a pause in the rotation of the magnetic field and this leads to the noise. Surely this isn’t strictly necessary? If I could just sprinkle a couple motor.move( command in there to update the commutation while it was waiting for the reply, I think that would work, but I don’t know how to call the motor.move function from within there… looked into using interrupts to run the motor.move() function at high frequency with good reliability, but that’s not promising for several reasons, there is probably stuff going on in there that you are not allowed to do inside an interrupt, and I am aware that’s generally not advisable to call a function like that inside an interrupt of course… I just need a smooth waveform, that’s totally paramount. The other code seems to run ok, fast enough it doesn’t cause any noticeable noise, as far as I can tell thus far. Just this one issue.

You will laugh at my code but it works better than torque mode, and basically does something similar. I do all this with great reluctance and as a last resort after trying and finding non-workable many other options for drivers. Even 2 pre-made chips couldn’t hack it.

I have some SPI sensors but would have to get the driver working for them. Think SPI might solve the problem, if this isn’t practical? The sensor has to be updated twice per rotation at least or the velocity calculation breaks down, found that out the hard way.

#include <SimpleFOC.h>

BLDCMotor motor = BLDCMotor(7); 
MagneticSensorI2C sensor = MagneticSensorI2C(0x36, 12, 0X0C, 4);
#define number_of_samples 10

float e=0;
float s=0;
float target_speed = 2;//rads per second
float accumulator =0;
float average =0;
int loop_counter = 0;
float phase_diff = 0.2;// just suppose it's 0.2 radians to start with, that seems to usually happens with 3 volts and 2 rads per second
int samples_taken = 0;
float no_load_phase_diff = 0;
float tim_advance =0;
// pid stuff
float p_gain = 1;
float i_gain = 0.00;
float d_gain = 0;
float control_signal_limit = 10;
float i_windup_limit = 2;
float setpoint = 2.3 ; //This determines the motor "timing" . Theoretically it should be 90 degrees but that doesn't work at all.
float control_signal = 0; // the control signal will be the *change* in velocity, it will be added to the current velocity and fed to the open loop commutation. in rads per second.
float i_term = 0;
float p_term = 0;
float d_term = 0;
float error = 0;
float pid_input=0;
unsigned long loop_time_last_clock_in = 0;
unsigned long loop_period = 0;
float samples[number_of_samples] = {0,0,0,0,0,0,0,0,0,0};
#define number_of_v_samples 10
float average_shaft_velocity = 0;
float radspersec = 0;
float shaft_v_samples[number_of_v_samples] = {0,0,0,0,0,0,0,0,0,0};// if you ever change the sample number this might break, it's a define way up at the top.
float shaft_velocity_accumulator = 0;
float shaft_velocity_f_func = 0; 
void SerialComm()
  case 'T': = Serial.parseFloat(); Serial.print("Speed target set");
  case 't': Serial.print("Angular speed"); Serial.println(; break;

  case 'V': motor.voltage_limit = Serial.parseFloat(); Serial.print("Voltage limit set");
  case 'v': Serial.print("Voltage limit is:"); Serial.println(motor.voltage_limit); break;
  case 'P': p_gain = Serial.parseFloat(); Serial.print("p_gain set");
  case 'p': Serial.print("p_gain is:"); Serial.println(p_gain); break;
  case 'I': i_gain = Serial.parseFloat(); Serial.print("i_gain set");
  case 'i': Serial.print("i_gain is:"); Serial.println(i_gain); break;
  case 'D': d_gain = Serial.parseFloat(); Serial.print("d_gain set");
  case 'd': Serial.print("d_gain is:"); Serial.println(d_gain); break;
  case 'O': i_windup_limit = Serial.parseFloat(); Serial.print("i_windup_limit set");
  case 'o': Serial.print("windup limit is:"); Serial.println(i_windup_limit); break;
  case 'U': setpoint = Serial.parseFloat(); Serial.print("setpoint changed:");
  case 'u': Serial.print("setpoint is:"); Serial.println(setpoint); break;
float angle_diff(float a1,float a2){ //a2 should be the shaft, a1 is electrical so it'a like a1-a2.  Thus if the angle is negative rotation is negative i.e. counterclockwise.
        if (abs(a1-a2) > _PI){ //  if it's more than 180 degrees difference and shaft is greater than electrical, the rotor will get pulled clockwise still
        if ((a1-a2)<0){
          return (_2PI-abs((a1-a2))); //then rotation is clockwise, so positive
      if (abs(a1-a2) > _PI){ // ok so when greater than 180 degrees but shaft  is less than electical then it will get pulled backwards, counterclockwise 
        if ((a1-a2)>0){
          return (-1*(_2PI-abs((a1-a2))));// so it's negative
      if (abs(a1-a2) < _PI){ // neither has rolled around yet if they are both less than 180 degrees. If shaft is less, rotation is clockwise so positive.  If shaft is more, rotation is counterclockwise so negative
        return (a1-a2);

unsigned long ticks_diff(unsigned long long time2, unsigned long time1){ // millis and micros() are both unsigned long integers.  time 2 has to be later than time 1. time 2 would be millis() for instance. This is for use when you save a millis or micros time and then compare it later to detrmine elapsed time.
   unsigned long ticks_diffed = 0;
   if (time2 > time1){
     ticks_diffed =  time2-time1; // nothing rolled over recently so it's simple
     return ticks_diffed; 
   if (time2 < time1){//micros or millis rolled over after time1 was saved
    ticks_diffed = (4294964296-time1)+(time2);  //current time plus the difference till the saved time would have rolled over too.
    return ticks_diffed;

void pid_controller(){// the input should be the present filtered phase angle difference, negative is counterclockwise positive is clockwise rotation, the shaft will lag the electrical

 // static float last_input = 0;
 // static long int last_clock_in = 0; 
 // long int time_elapsed = 0 ;
  //we are using the global setpoint so it's easy to change without calling the function. idk that's the way it seems to be done usually.
  error =  setpoint-pid_input; // when input is more than the setpoint, the shaft during clockwise (positive) rotation lagging too much, with this equation the error will be negative so if gain is positive and control signal is added to the electrical rpm, the rpm of the electrical will go down, and the shaft can catch up. To reverse rotation I think both gain and setpoint need to be reversed.
  p_term = error*p_gain;
 // i_term = i_term + (error*i_gain);// we actually have no use for an i term due to how we are using the output, as an acceleration, I think.  But whatever.
  //if (i_term > i_windup_limit){
 //   i_term = i_windup_limit;
 // }
   // if (i_term < (-1*i_windup_limit)){
  //  i_term = -i_windup_limit;
 // }
  //time_elapsed = ticks_diff(millis(), last_clock_in);
  //last_clock_in = millis();
  //d_term = d_gain*((pid_input-last_input)*1000/float(time_elapsed)); // this might not work right if the time elapsed is very large because you are trying to stuff a long unsigned int into a float. Multiply by 1000 so the time base is in seconds, not milliseconds.
 // last_input = pid_input;
  control_signal = p_term;// + i_term + d_term;
/*float shaft_angle_diff(float before, float now){ // assume difference is relatively small, less than pi radians, just factor in the modulo.  It might be continous, not wrap around, idk.
  static float diff = 0;
  if (now > before){
    diff = now - before;   
  if (now < before){// now must have wrapped around
    diff =  // assume before is within pi raidans of the wraparound point

void print_pid_stuff(){
 // Serial.print("Pterm:");
 // Serial.print(",");
 // Serial.print("Iterm:");
 // Serial.print(i_term);
 // Serial.print(",");
//  Serial.print("Dterm");
//  Serial.println(d_term);
 // Serial.print("Control:");
 // Serial.print(control_signal);
 // Serial.print(",");
 // Serial.print(pid_input); 
//  Serial.print(",");
 // Serial.print("e:");
 // Serial.print(e); 
 // Serial.print(",");
 // Serial.print("loop_period:");
 // Serial.print(average); 
 // Serial.print(",");

void print_other(){


float no_load_phase_diff_measurement(){
  long int start_time =0; = 0.1;
  loop_counter = 0;
  start_time = millis();
  while ((millis() - start_time)<6000){  
  for(int q = 0; q<40; q++){
      for(int i = 0; i<1000; i++){
      s = fmod(sensor.getAngle()*7,_2PI);//+(_2PI)
      e = fmod(motor.shaft_angle*7,_2PI);
      samples[loop_counter%number_of_samples] = phase_diff;//store the last ten samples and calculate the average every time.
      accumulator = 0;
      for (int i = 0;i<number_of_samples;i++){
        accumulator = accumulator+(samples[i]);
      average = accumulator/number_of_samples;
  no_load_phase_diff = average;
  Serial.println("average phase displacement under no load measured");
  return no_load_phase_diff;
void setup() {
  motor.voltage_sensor_align = 4;//I'm not even using  the align info any more get rid of all that stuff later. No, keep it so I can use torque mode for acceleration.
  Serial.println("test serial");
  // driver config
  // power supply voltage [V]
  driver.voltage_power_supply = 24;
  // link the motor and the driver
motor.foc_modulation = FOCModulationType::SpaceVectorPWM;
  // limiting motor movements
  motor.voltage_limit = 7;   // [V]
  motor.velocity_limit = 520; // [rad/s]
  // open loop control config
  motor.controller = MotionControlType::velocity_openloop;
 motor.LPF_velocity.Tf = 0.05f;
  // init motor hardware

  motor.initFOC(); = 2;
  motor.voltage_limit = 3;
 // shaft_velocity_check();
  //start_motor(); // that was more about the old filtering approach which I no longer need.

void loop() {
  for(int q =0; q<10;q++){
    for (int x =0; x<7; x++){
      for(int i = 0;i<200; i++){
      s = (fmod((sensor.getAngle()*7)+no_load_phase_diff,_2PI));//+(_2PI)
      e = fmod(motor.shaft_angle*7,_2PI);// motor.shaft_angle is actually the electricla angle, bad name

      samples[loop_counter%number_of_samples] = phase_diff;
      accumulator = 0;
      for (int i = 0;i<number_of_samples;i++){
        accumulator = accumulator+(samples[i]);
      average = accumulator/number_of_samples;
      pid_input = average;

      target_speed = motor.shaftVelocity() + control_signal; // should normalize for the amount of time elapsed or things will change if you change loop time the effective gain will change. too complicated for now.
    = target_speed;
      if (loop_counter >(number_of_samples*10)){ //just so it never overflows, probably don't need this as it makes no diff if it overflows and wraps around.
      loop_counter = 0;

The MagneticSensorI2C class uses the Arduino Wire library, and I don’t know if there’s any way to make it operate via interrupts (i.e. return immediately after sending a request to the sensor, and trigger an interrupt when the sensor responds with the angle). I wonder why it’s so slow anyway. Might be worthwhile to put some profiling code in MagneticSensorI2C::read, using _micros() to see how long each of the wire functions take. From the datasheet it looks like the angle can have up to 2.2ms settling time, but surely it wouldn’t just stall the communication waiting for that, would it? You could try configuring it for the shortest/least accurate settling time and see if it makes any difference.

Wire.write(0x07); // CONF register
Wire.write(00000011b); // SF = 11, 0.286ms settling time

Another option would be to try using MagneticSensorAnalog to read the AS5600’s OUT pin instead of communicating via I2C.

I tried to get the sensor to give an analog output, it won’t do it. It’s supposed to do it as the default from the factory, but the pin never changes. There is an arduino library to supposedly program the AS5600 chip to configure it to give analog or PWM output etc. but I tried it and it doens’t work. It says it programmed the chip but the behaviour of the output pin doesn’t change. I got the right pin etc. I could try a different as5600 based board.

The problem with analog read is that the board doesn’t have an ADC pin broken out, so I can’t really do that. Otherwise yeah it would be a good option. I could use PWM but I can’t get the sensor to cooperate and give me pwm.

The thing is, when I look at the library documentation, I follow the program flow of what happens when I call sensor.update(). IT’s hard to follow because the search function in the documentation doesn’t seem to work right, I often search for stuff that is definitely in the code and it says nothing found… anyway. I eventually get to the part where it’s reading the angle from the sensor,
line 128
It transmits.

Then it tries to read.

However, I think that the I2C peripheral will have an empty buffer until the reply comes back. I think the read from the peripheral will be blocking until there is something in the buffer? I mean, it can’t return a value until it has something, so either the function waits or it does not return anything.

I think there are several ways to get traction on this. One could be to split the sensor.update() call into two peices. You call the half that transmits, then you can do something else for a bit while the info comes in. Then you call the half that reads the I2C peripheral to get the angle. Obviously you can’t wait long or the angle reading won’t be accurate any more.

(indeed, the time delay causing wrong angle readings appears to be a problem already and I suspect this is partly why torque mode with the stock FOC stuff does not work right. My hacky plan was to apply a compensation based on the mechanical rotational speed, with a single factor measured empirically to detrmine the time delay between when I say to get the angle and when the angle was actually sampled/what it’s timestamp would be if there was a timestamp for it. alternatively, as you indicate yeah an interrupt could be used to nab that value as soon as it’s ready to be read from the I2C peripheral, if that’s supported by the wire library.)

Maybe if I just excise the actual reading part from the read() function mentioned above, then I just read the peripheral in my own program… then I have to chop it up and actually get the angular value out of the reply. Sigh. I guess I could figure it out. It’s just that my primitive ideas often don’t work out so I am hesitant to undertake anything that implies work to try it. Shit.

Is there no way I can just sprinkle some motor.move( commands in there somehow? I just need to make it keep rotating the magnetic field smoothly while it waits for the reply from the sensor. Any ideas how I could do that?

Maybe splitting that read() function in two is the best way. Just make it return without actually reading anything then have an actual_read() function that goes back and picks up where it left off.

No wait, I should change the name of the original read function to prep_to_read() or something. Then keep read() the same so everything still works. the other things that call the read function should still work, they will get their return value, just without the request part. I have to remember to call the prep_to_read first in my main program. Then I loop for a short time running motor.move( while I wait for the reply. Yeah that should work?

DMA? Peripheral to Memory

Almost no clue how to do that :frowning:

It’s usually MCU specific stuff. Which one are you working with ? Look into the datasheet.

If it has the functionality, you will then set it up to read the data and raise a flag when new data is available

I don’t think you can use DMA on a device that only communicates by I2C.

Try pasting this over MagneticSensorI2C::read() and see what you get

int MagneticSensorI2C::read(uint8_t angle_reg_msb) {
  unsigned long time[8];
  static unsigned long printTimestamp = 0;

   // read the angle register first MSB then LSB
  byte readArray[2];
  uint16_t readValue = 0;
   // notify the device that is aboout to be read
  time[0] = _micros();
  time[1] = _micros();
  time[2] = _micros();
  time[3] = _micros();
   // read the data msb and lsb
  time[4] = _micros();
  wire->requestFrom(chip_address, (uint8_t)2);
  for (byte i=0; i < 2; i++) {
   time[5 + i] = _micros();
   readArray[i] = wire->read();
  time[7] = _micros();
  if (time[7] > printTimestamp + 1000000)
    printTimestamp = time[7];
    for (int i = 1; i < 8; i++)
       Serial.print(time[i] - time[i - 1]);
       Serial.print(", ");
   // depending on the sensor architecture there are different combinations of 
   // LSB and MSB register used bits
   // AS5600 uses 0..7 LSB and 8..11 MSB
   // AS5048 uses 0..5 LSB and 6..13 MSB
   readValue = ( readArray[1] &  lsb_mask );
  readValue += ( ( readArray[0] & msb_mask ) << lsb_used );
  return readValue;

The Arduino Wire library is synchronous (I.e. blocking) on the controller side. I2C device code can use interrupts (onReceive, onRequest).

That’s just how Arduino works, it doesn’t have any notion of multithreaded code.

However all MCU types I’m aware of implement I2C in hardware and are actually asynchronous.

So if you wanted to you could write your own driver, bypassing Arduino’s wire library and using the MCU registers directly, or using a deeper abstraction layer than Arduino, like STM32’s HAL library.

And on many MCUs I imagine you could use DMA as well, based on either an event or an interrupt from the I2C peripheral. That would be even more complex, and really not worth it IMHO since you’ll be spending many microseconds reading from I2C anyways, why bother optimizing the copy operation for 2 bytes?

I would not really recommend any of this though - it will make the code much more complex, and the core problem will still remain - I2C is too slow…

Found out the hard way about I2C being blocking. I got the delay without calling motor.move( down to 300 microseconds but it’s not enough. It’s quieter but not enough. My next hope is to use the PWM mode of the sensor. I got the sensor outputting PWM, but I don’t know if the code SimpleFOC uses uses interrupts etc. to read the data quickly… guess I will find out in the hours ahead. So over budget and over time… however this all proves open source is the way to go. Nothing works, but at least with open source you can eventually get it working. At this point I’m only using a few pieces of the original simpleFOC code, however those pieces are good to have.

The PWM sensor can work with interrupts, so it might help in your specific case.

But it is also the worst sensor mode by far, in terms of accuracy (its based on timings, which can be quite imprecise) and in terms of speed. The time to read one value is always 1ms, because that’s the frequency of the PWM signal. So its even slower than I2C :frowning:

Before investing time in this I would work on the SPI sensor. It is working for me, so there is no reason it should not work for you.
Of course, connecting it to the B-G431-ESC1 is quite difficult. But connecting it to the Lepton is easy :slight_smile:
For me, I lowered the SPI speed somewhat, and I switched it into a different SPI mode (Mode 3, I think), and then it worked. I think the problems may be caused by the long cables (due to the JST-SH connector on my sensor and the Microblade connectors on the Lepton I have 2 cables with a little adapter in-between, for total length of about 25cm… I’m guessing its too long.
I have to say, the SC60228 has not been the easiest SPI sensor to use. Depending on the MCU and connection I’ve had to play with the settings to get it to work. Other SPI sensors have been less problematic for me (but they all cost more).


Yoics, I’m not so sure you are encouraging me to go back to that sensor. Yes it would be impossible to connect to the ESC board, that is why I did not try it again unfortunately, the lepton was too small to run my code and the ESC doesn’t have an SPI port… My current latest approach is to get rid of the sensor entirely and replace it with a current sensor right before the lepton. A separate MCU will read the current sensor (can’t use the lepton cuz i2c is blocking) and adjust the RPM till it is optimal. It sounds clownerific, and it is a little sketch as there is only a small margin of possible overspeed past the optimal point until the motor stalls, but I think it’s ok for me, because it’s just a fan so it could almost just work in open loop mode with a little help to decide ahead of time which voltage to use at what rpm. All it need is a little help making sure things don’t stop working if the bearings start to wear out and get a little stiff or the wind blows real hard or something. I am concerned the angle sensor will get wet and stop working, also the wires may need to be as long as 60 cm.

My main problem is carefully finding the optimal point without going past it too far. I am hoping if the motor does loose it’s lock to the RPM of the electrical angle I can actually measure a small current ripple on the input of the lepton and actually determine the shaft rpm and try to get the lock back, but hopefully I won’t need that. That should only happen if some bug flies in the fan or something. In which case it looses the lock, notices, stops and restarts if it can. If not, well no other fan would work either so I think that’s not a big deal you just have to clear the blockage.