[FEATURE] Calculate dt for Loopfoc and move only once

[Candas1]

While working on some code I had an idea for performance improvement.

Problem:

Now Loopfoc and move are running in the main loop and timestamps are used to calculate dt:
float dt = (timestamp_now - timestamp_prev) * 1e-6f;

If I take the worst example being foc_current, dt is being calculated separately for:

• SmoothingSensor::update()
• LPF_current_q()
• LPF_current_d()
• PID_current_q()
• PID_current_d()
• getVelocity()
• LPF_velocity()

This can cost a lot of resources on chips without FPU.

Proposed solution:

Add a dt parameter to those functions, make it optional (=0) not to impact the user code.
If dt = 0, calculate Ts as usual.- Check for issues (e.g. dt too small).
dt can now be calculated only once for loopfoc, and saved as a member of BLDCmotor.

This could also be useful if someone runs Loopfoc from a timer interrupt, then the timer period could be used.
This can be used for measuring the frequency of loopfoc without additional code.

Doubt:

This is with the assumption that an interrupt running in the middle will impact all those different dt in the same way.
But now, if a hall sensor interrupt was running between LPF_current_q() and LPF_current_d(), both functions would run with the same data but with a different dt ?

[EDIT] Replaced Ts by dt

[Candas1]

Here is an example after changing the current LPF and PID only.

I tested this on a STM32F1 in foc_current mode.

Before:
RAM: [= ] 9.0% (used 4432 bytes from 49152 bytes)
Flash: [=== ] 26.5% (used 69560 bytes from 262144 bytes)
loopfoc = 292us

After:
RAM: [= ] 9.0% (used 4440 bytes from 49152 bytes)
Flash: [=== ] 26.6% (used 69656 bytes from 262144 bytes)
loopfoc = 284us

Not a very big improvement but it can be useful.

A small class could also be created for:

• measuring current time with _micros()
• Calculating Ts
• Checking if it's too small and assigning a default value
• saving the previous timestamp

This could be reused in the different functions to reduce the duplication.

[dekutree64]

I think it should be named dt, not Ts. Timestamp usually refers to an absolute time value when an event occurred, whereas this is the change in time since last frame.

motor.move will need a different dt when downsampling is used.

I do agree factoring it into a reusable class would be nice. Then you could cleanly use function overloading rather than a default argument, like float operator() (float error) { (*this)(error, dt_calculator.update()); } so the decision on how to acquire dt is made at compile time rather than having to check for 0 every frame.

[Candas1]

I think it should be named dt, not Ts. Timestamp usually refers to an absolute time value when an event occurred, whereas this is the change in time since last frame.

True, my mistake.

I do agree factoring it into a reusable class would be nice. Then you could cleanly use function overloading rather than a default argument, like float operator() (float error) { (*this)(error, dt_calculator.update()); } so the decision on how to acquire dt is made at compile time rather than having to check for 0 every frame.

Thank you, I am new to C++/Object oriented programming so I appreciate the ideas.

[runger1101001]

I would propose another way of doing it:

At the moment we have a "sensor" abstraction for the motor, which allows to sample the rotor angle. The idea behind it is that you choose a moment to sample it, and it then holds the sampled value for anyone who needs it during the subsequent computations.

Can we do the same thing for the dt value? We choose a point to sample the clock, and then keep that sample for use in subsequent computations?

This seems easier to me than passing the value around all the functions, instead of calling _micros() we instead use something like: motor.iteration_dt. That value gets set at the beginning of loopFOC(), for example.

Would there be a disadvantage to using the same timestamp for all the calculations? The many values used right now can't be considered accurate, since the us accuracy and indeed the MCU clock itself aren't that accurate, and the dt value assigned to the sample during computation is close, but not exactly matched to the sensor or current sampling moment.
In fact, its kind of odd that the LPF keeps its own timestamp to begin with, since the value we want is the one associated with the sample time of the value being smoothed, and not the point in time at which we do the smoothing operation...

What do you think of motor.iteration_dt?

[dekutree64]

That sounds the same as what we're talking about. The end users of the dt value are the lowpass filters and PIDs, which don't have access to the motor's member variables, so there's no getting around passing it as an argument all over the place. But it would first be calculated in loopFOC and cached in a motor member variable.

Factoring the dt calculation out into a separate class would allow the PID and lowpass classes to have a member dt_calculator which is used if no argument is given, and left to waste a bit of RAM when the argument is used. So they would retain their current functionality and add the new option for a user-specified dt, rather than making them require the dt argument. Though if all the SimpleFOC code uses the argument, then maybe it would be better to remove the old functionality so it's more obvious how they're intended to be used.

But as I said before, downsampling of motor.move will require a separate dt. Creating a second instance of dt_calculator would take care of that, whereas keeping it as inline code would likely result in one duplication even if PID and lowpass do require the argument.

EDIT: Or maybe in motor.move, do like move_dt += loopfoc_dt; before the downsampling check, and move_dt = 0; right at the end. So if no downsampling is used, move_dt is always equal to loopfoc_dt, and if downsampling is used then it will accumulate the deltas during the skipped frames.

[Candas1]

One idea I have is that clarke/park/filters/pids could also be in the move function, so that could also be downsampled on slow hardware.

So I think most of the need for dt would be in the move function:
LPF_current_q()
LPF_current_d()
PID_current_q()
PID_current_d()
getVelocity()
LPF_velocity()

Only the smoothing would be in loopfoc because it's needed for the position.

[Copper280z]

Here are api compatible implementations for the PID controller and Low Pass filters. I break the calculation into a separate function, then use operator overloading on the call operator to handle calculating dt or using the passed in value.
https://github.com/Copper280z/Arduino-FOC/blob/dual_motor/src/common/pid.cpp
https://github.com/Copper280z/Arduino-FOC/blob/dual_motor/src/common/lowpass_filter.cpp