Page 1 of 2
Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 10:59 am
by bexander
As the field weakening (FW) part of the SW limits the use of the Toyota MGR I have decided to test using the FW equations found here,
https://se.mathworks.com/help/mcb/ref/m ... rence.html, under Interior PMSM. The MTPA part is identical to what are used.
Starting from 5.13 SW.
I have created a spreadsheet from the equations to adjust them to make sense.
So the part of the code I've changed is found in foc.cpp,
Code: Select all
static const s32fp fluxLinkage = FP_FROMFLT(Param::GetFloat(Param::fluxlinkage));
static const s32fp fluxLinkage2 = FP_MUL(fluxLinkage, fluxLinkage);
static const s32fp ld = FP_FROMFLT(Param::GetFloat(Param::ld));
static const s32fp lq = FP_FROMFLT(Param::GetFloat(Param::lq));
static const s32fp rs = FP_FROMFLT(Param::GetFloat(Param::rs));
static const s32fp lqminusld = lq - ld;
static const s32fp lqminusldSquaredBs10 = (lqminusld * lqminusld) >> 5; //additional 10-bit left shift because otherwise it can't be represented
static const s32fp twopi = FP_FROMFLT(6.283185307)
void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref)
{
int32_t isSquared = is * is;
int32_t sign = is < 0 ? -1 : 1;
//factor of 8 has been incorporated into the right shift (7 instead of 10)
s32fp term1 = fpsqrt(fluxLinkage2 + ((lqminusldSquaredBs10 * isSquared) >> 7));
int32_t idrefmtpa = FP_TOINT(FP_DIV(fluxLinkage - term1, 4 * lqminusld));
int32_t iqrefmtpa = sign * (int32_t)sqrt(isSquared - idrefmtpa * idrefmtpa);
s32fp umax = udc / sqrt3 - rs * is;
s32fp denominator1 = fpsqrt(FP_MUL((lq * iqrefmtpa), (lq * iqrefmtpa)) + FP_MUL((ld * idrefmtpa + fluxLinkage), (ld * idrefmtpa + fluxLinkage)));
s32fp basespeed = FP_DIV(umax, denominator1);
s32fp espeed = frq * twopi;
if(espeed <= basespeed)
{
idref = idrefmtpa;
iqref = iqrefmtpa;
}
else
{
s32fp fluxLinkageLd = FP_MUL(fluxLinkage, ld);
s32fp ld2minuslq2 = FP_MUL(ld, ld) - FP_MUL(lq, lq);
s32fp term2 = FP_MUL(fluxLinkage, fluxLinkage) + FP_MUL(lq, lq) * isSquared - FP_DIV(FP_MUL(umax, umax), FP_MUL(basespeed, basespeed));
s32fp numerator1 = -fluxLinkageLd + fpsqrt(FP_MUL(fluxLinkageLd, fluxLinkageLd) - FP_MUL(ld2minuslq2, term2));
int32_t idfw = FP_TOINT(FP_DIV(numerator1, ld2minuslq2));
int32_t iqfw = sign * (int32_t)sqrt(isSquared - idfw * idfw);
int32_t idreffw = MAX(idfw, -ABS(is));
int32_t iqreffw = ABS(iqfw) < ABS(is) ? iqfw : is;
idref = idreffw;
iqref = iqreffw;
}
}
As I have only programmed Arduino or more spcifically Atmega328p using the Arduino IDE it is a bit of uphill at the moment understanding the SW and how to make it do what I want so a couple of questions remains.
How do I get hold of the values of frq and udc, used in other parts of the code?
Is there any simple way to input specific values to check if the code calculates correctly without actually programming the STM32 i.e. some kind of simulations?
EDIT:
I have also added four new parameters in the param_prj.h,
Code: Select all
PARAM_ENTRY(CAT_MOTOR, rs, "ohm", 0, 1, 0.075, 138 ) \
PARAM_ENTRY(CAT_MOTOR, ld, "H", 0, 1, 0.001, 139 ) \
PARAM_ENTRY(CAT_MOTOR, lq, "H", 0, 1, 0.002, 140 ) \
PARAM_ENTRY(CAT_MOTOR, fluxlinkage, "Weber", 0, 1, 0.108, 141 )
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 12:47 pm
by johu
That is a great effort!
bexander wrote: ↑Fri Oct 29, 2021 10:59 am
As I have only programmed Arduino or more spcifically Atmega328p using the Arduino IDE it is a bit of uphill at the moment understanding the SW and how to make it do what I want so a couple of questions remains.
How do I get hold of the values of frq and udc, used in other parts of the code?
You can obtain them from the parameter module with
Code: Select all
Param::Get(Param::frq) //returns fixed point with 5 decimal bits
Param::GetInt(Param::frq) //returns integer
Param::GetFloat(Param::frq) //returns float
I'm trying to keep the parameter module dependency out of many modules and rather maintain local copies. So like
Code: Select all
FOC::SetRotorFrequency(Param::Get(Param::frq))
which would then just set a private or static variable inside the FOC module.
In the 100 and 10ms tasks it is perfectly fine to work with floats. So the MTPA function can be implemented with floats as it is just called in the 10ms task. For the fast PWM task I strongly recommend using fixed point math as the CPU might run out of steam otherwise.
bexander wrote: ↑Fri Oct 29, 2021 10:59 am
Is there any simple way to input specific values to check if the code calculates correctly without actually programming the STM32 i.e. some kind of simulations?
I quite simply create a "test.cpp" and include the module to be tested there. Then I just write some test code in main. Here it pays off when modules only have little dependencies.
Compile all needed modules then and then compile and link and run your test program:
Code: Select all
g++ -c -Ilibopeninv/include libopeninv/src/foc.cpp -o foc.o
g++ -c -Ilibopeninv/include libopeninv/src/sine_core.cpp -o sine_core.o
g++ -Ilibopeninv/include test.cpp foc.o sine_core.o -o test
./test
You could also create a Makefile, but I couldn't be bothered. Or you can include you test with the other test routines and compile the "Test" target, i.e. "make Test".
Here is an example test.cpp:
Code: Select all
#include <stdio.h>
#include <stdint.h>
#include "libopeninv/include/sine_core.h"
#include "libopeninv/include/foc.h"
int main()
{
for (int angle = 0; angle < 65536; angle += 100)
{
FOC::SetAngle(angle);
FOC::InvParkClarke(-35000, 14311);
printf("%d;%d;%d;%d\n", angle, FOC::DutyCycles[0], FOC::DutyCycles[1], FOC::DutyCycles[2]);
}
return 0;
}
bexander wrote: ↑Fri Oct 29, 2021 10:59 amI have also added four new parameters in the param_prj.h,
Code: Select all
PARAM_ENTRY(CAT_MOTOR, rs, "ohm", 0, 1, 0.075, 138 ) \
PARAM_ENTRY(CAT_MOTOR, ld, "H", 0, 1, 0.001, 139 ) \
PARAM_ENTRY(CAT_MOTOR, lq, "H", 0, 1, 0.002, 140 ) \
PARAM_ENTRY(CAT_MOTOR, fluxlinkage, "Weber", 0, 1, 0.108, 141 )
You will hit the limitation of 5 decimal digits here. Rather use unit "mH" or "uH" right away. The smallest number if 1/32 and that is also the step size.
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 3:14 pm
by bexander
Thank you for some great input!
So one way of getting udc and frq would be doing this inside the Mtpa function then? Going for a way I can understand and make it work, I can always clean this up later...
Code: Select all
s32fp udc = FP_FROMINT(Param::GetInt(Param::udc));
s32fp frq = FP_FROMINT(Param::GetInt(Param::frq));
johu wrote: ↑Fri Oct 29, 2021 12:47 pm
In the 100 and 10ms tasks it is perfectly fine to work with floats. So the MTPA function can be implemented with floats as it is just called in the 10ms task. For the fast PWM task I strongly recommend using fixed point math as the CPU might run out of steam otherwise.
So all math inside the Mtpa function could be changed to float? Would make it a bit easier for me to follow. I just tried to be consistent with what was already in there...
johu wrote: ↑Fri Oct 29, 2021 12:47 pm
You will hit the limitation of 5 decimal digits here. Rather use unit "mH" or "uH" right away. The smallest number if 1/32 and that is also the step size.
Changed to milli on all four parameters and added a by 1000 division in the parameter read in foc.cpp, for example,
Code: Select all
static const s32fp ld = FP_FROMFLT(Param::GetFloat(Param::ld)) / 1000;
Regarding the testing of the calculations you make it sound so simple...
I was going for something more like this
https://www.onlinegdb.com/online_c_compiler, but for c++ I guess.
But of course, better to compile the actual code instead of just running parts of it.
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 3:48 pm
by johu
bexander wrote: ↑Fri Oct 29, 2021 3:14 pmSo one way of getting udc and frq would be doing this inside the Mtpa function then? Going for a way I can understand and make it work, I can always clean this up later...
Code: Select all
s32fp udc = FP_FROMINT(Param::GetInt(Param::udc));
s32fp frq = FP_FROMINT(Param::GetInt(Param::frq));
That's a bit redundant. s32fp udc = Param::Get(Param::udc); is better
bexander wrote: ↑Fri Oct 29, 2021 3:14 pmSo all math inside the Mtpa function could be changed to float? Would make it a bit easier for me to follow. I just tried to be consistent with what was already in there...
Yes
bexander wrote: ↑Fri Oct 29, 2021 3:14 pm
Changed to milli on all four parameters and added a by 1000 division in the parameter read in foc.cpp, for example,
Code: Select all
static const s32fp ld = FP_FROMFLT(Param::GetFloat(Param::ld)) / 1000;
don't think you can initialize a constant with a function call and I think you don't want to. Because then changing the parameter at runtime would have no effect.
bexander wrote: ↑Fri Oct 29, 2021 3:14 pm
Regarding the testing of the calculations you make it sound so simple...
I was going for something more like this
https://www.onlinegdb.com/online_c_compiler, but for c++ I guess.
But of course, better to compile the actual code instead of just running parts of it.
Whichever suits you.
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 4:45 pm
by bexander
johu wrote: ↑Fri Oct 29, 2021 3:48 pm
That's a bit redundant. s32fp udc = Param::Get(Param::udc); is better
Will this result udc as for example 400 or as multiplied by 2^15, 13107200?
johu wrote: ↑Fri Oct 29, 2021 3:48 pm
don't think you can initialize a constant with a function call and I think you don't want to. Because then changing the parameter at runtime would have no effect.
I agree, moved this inside the Mtpa function and changed to,
Code: Select all
s32fp ld = FP_FROMFLT(Param::GetFloat(Param::ld)) / 1000;
Is this also eqvivalent to,
Code: Select all
s32fp ld = (Param::Get(Param::ld)) / 1000;
or am I totally misunderstanding?
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 5:31 pm
by johu
bexander wrote: ↑Fri Oct 29, 2021 4:45 pm
Will this result udc as for example 400 or as multiplied by 2^15, 13107200?
Oh, you do have a point here. Inside FOC we use 15 decimal digits but Get() will return only 5 decimal digits. Well, I guess convert this whole mess to float instead of dwelling over digits and whether it might overflow.
bexander wrote: ↑Fri Oct 29, 2021 4:45 pm
I agree, moved this inside the Mtpa function and changed to,
Code: Select all
s32fp ld = FP_FROMFLT(Param::GetFloat(Param::ld)) / 1000;
Is this also eqvivalent to,
Code: Select all
s32fp ld = (Param::Get(Param::ld)) / 1000;
or am I totally misunderstanding?
Yes that would be correct, but see above...
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 6:37 pm
by bexander
Ok, will go float then.
How do I do square root on a float number like 0.001?
Do I just include "math.h" and use sqrt, and change name to the sqrt function already present in foc.cpp?
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 6:49 pm
by johu
bexander wrote: ↑Fri Oct 29, 2021 6:37 pm
How do I do square root on a float number like 0.001?
Do I just include "math.h" and use sqrt, and change name to the sqrt function already present in foc.cpp?
Oh, you'd need to implement that for float then. But I can do it if that's too daunting. That said it should be rather similar to the s32fp variant
Re: Changing the FW portion of the inverter SW
Posted: Fri Oct 29, 2021 7:43 pm
by bexander
There seem to be a standard cmath header file in c++ which includes a square root function that handles floats.
Is it possible to use this in the SW or is it required to create a function in the SW that handles this?
If a function is needed, I can look into it and let you know if I'm unsuccessful.
Re: Changing the FW portion of the inverter SW
Posted: Sat Oct 30, 2021 3:03 pm
by johu
Try it, but it will probably bloat binary size or result in link errors.
Re: Changing the FW portion of the inverter SW
Posted: Sun Oct 31, 2021 5:05 am
by bexander
I think I have a working code and even found a sqrt function I copied that seem to produce correct results.
Also got the testing of the code to works as per johu:s suggested method.
What remains is the new motor parameters, udc and frq.
johu wrote: ↑Fri Oct 29, 2021 12:47 pm
You can obtain them from the parameter module with
Code: Select all
Param::Get(Param::frq) //returns fixed point with 5 decimal bits
Param::GetInt(Param::frq) //returns integer
Param::GetFloat(Param::frq) //returns float
I'm trying to keep the parameter module dependency out of many modules and rather maintain local copies. So like
Code: Select all
FOC::SetRotorFrequency(Param::Get(Param::frq))
which would then just set a private or static variable inside the FOC module.
The foc.cpp does not use the parameter module so how do I create local copies?
Do I put this call in pwmgeneration-foc.cpp before I call FOC::Mtpa() and then create a function inside foc.cpp like this:
Code: Select all
void FOC::SetRotorFrequency(float input)
frq = input;
And then repat this for all six values needed? Or maybe better to do one function handling all six values?
Re: Changing the FW portion of the inverter SW
Posted: Sun Oct 31, 2021 10:01 am
by johu
Hmm, if they are just used by the MTPA function you could also just pass them all as parameters.
Re: Changing the FW portion of the inverter SW
Posted: Sun Oct 31, 2021 10:29 am
by bexander
Good point!
So this is what I ended up with:
foc.cpp
Code: Select all
void FOC::Mtpa(int32_t is, int32_t& idref, int32_t& iqref, float rs, float ld, float lq, float fluxLinkage, int32_t udc, int32_t frq)
{
float fluxLinkage2 = fluxLinkage * fluxLinkage;
float lqminusld = lq - ld;
float lqminusldSquared = lqminusld * lqminusld;
int32_t isSquared = is * is;
int32_t sign = is < 0 ? -1 : 1;
float term1 = sqrtf(fluxLinkage2 + (lqminusldSquared * isSquared * 8));
int32_t idrefmtpa = (float)(fluxLinkage - term1) / (float)(4 * lqminusld);
int32_t iqrefmtpa = sign * sqrtf(isSquared - idrefmtpa * idrefmtpa);
float umax = udc / sqrtthree - rs * is;
float denominator1 = sqrtf((lq * iqrefmtpa) * (lq * iqrefmtpa) + (ld * idrefmtpa + fluxLinkage) * (ld * idrefmtpa + fluxLinkage));
float basespeed = umax / denominator1;
float espeed = frq * twopi;
if(espeed <= basespeed)
{
idref = idrefmtpa;
iqref = iqrefmtpa;
}
else
{
float fluxLinkageLd = fluxLinkage * ld;
float ld2minuslq2 = ld * ld - lq * lq;
float term2 = fluxLinkage * fluxLinkage + lq * lq * isSquared - (umax * umax) / (espeed * espeed);
float numerator1 = -fluxLinkageLd + sqrtf(fluxLinkageLd * fluxLinkageLd - ld2minuslq2 * term2);
int32_t idfw = numerator1 / ld2minuslq2;
//int32_t iqfw = sign * (int32_t)sqrt(isSquared - idfw * idfw);
int32_t iqfw = isSquared > (idfw * idfw) ? sign * (int32_t)sqrtf(isSquared - idfw * idfw) : 0;
int32_t idreffw = MAX(idfw, -ABS(is));
int32_t iqreffw = ABS(iqfw) < ABS(is) ? iqfw : is;
idref = idreffw;
iqref = iqreffw;
}
}
pwmgeneration-foc.cpp
Code: Select all
float rs = Param::GetFloat(Param::rs) / 1000;
float ld = Param::GetFloat(Param::ld) / 1000;
float lq = Param::GetFloat(Param::lq) / 1000;
float fluxLinkage = Param::GetFloat(Param::fluxLinkage) / 1000;
int32_t udc = Param::GetInt(Param::udc);
FOC::Mtpa(is, id, iq, rs, ld, lq, fluxLinkage, udc, frq);
Regarding pwmgeneration-foc.cpp:
frq is already used as a variable so should be ok to use directly but in one place used as:
Param::SetFixed(Param::fstat, frq);
then later as:
float rotorfreq = FP_TOFLOAT(frq);
Is frq the stator or rotor frequency?
Do I need to convert it to int before sending it to mtpa (FP_TOINT)? Is it otherwise multiplied by 32?
Re: Changing the FW portion of the inverter SW
Posted: Sun Oct 31, 2021 4:18 pm
by bexander
When I run the "CONTROL=FOC make" command, I get the following warnings from my float sqrt:
Code: Select all
libopeninv/src/foc.cpp: In static member function 'static float FOC::sqrtf(float)':
libopeninv/src/foc.cpp:214:14: warning: dereferencing type-punned pointer will break strict-aliasing rules [-Wstrict-aliasing]
214 | int i = *(int*)&x;
| ^~~~~~~~
libopeninv/src/foc.cpp:216:10: warning: dereferencing type-punned pointer will break strict-aliasing rules [-Wstrict-aliasing]
216 | x = *(float*)&i;
| ^~~~~~~~~~
It still builds but should I be worried about this?
The function in question:
Code: Select all
float FOC::sqrtf(float x)
{
float xhalf = 0.5f*x;
int i = *(int*)&x;
i = 0x5f375a86 - (i>>1);
x = *(float*)&i;
x = x*(1.5f - xhalf*x*x);
x = x*(1.5f - xhalf*x*x);
x = x*(1.5f - xhalf*x*x);
x=1/x;
return x;
}
Re: Changing the FW portion of the inverter SW
Posted: Sun Oct 31, 2021 9:33 pm
by johu
Wow, that is super hacky. Maybe it works but I have no idea how.
Re: Changing the FW portion of the inverter SW
Posted: Sun Oct 31, 2021 9:37 pm
by johu
bexander wrote: ↑Sun Oct 31, 2021 10:29 am
Is frq the stator or rotor frequency?
Do I need to convert it to int before sending it to mtpa (FP_TOINT)? Is it otherwise multiplied by 32?
Stator and rotor frequency is the same - that's why some call it synchronous motor
Yes, you'd need a TOINT() so that it isn't multiplied by 32
Re: Changing the FW portion of the inverter SW
Posted: Mon Nov 01, 2021 5:48 am
by bexander
johu wrote: ↑Sun Oct 31, 2021 9:37 pm
Stator and rotor frequency is the same - that's why some call it synchronous motor
True, but to my knowledge, if you have a 4 pole motor the stator field need to "rotate" 4 cycles to complete on actual rotation on the rotor shaft i.e. 4Hz on the stator yields 1Hz or 1rps on the rotor shaft. So to me rotor frequency is the same as rotor speed and not the same as stator frequency but this is not the case in the SW then?
Re: Changing the FW portion of the inverter SW
Posted: Mon Nov 01, 2021 5:48 am
by bexander
Will try and find a better way to do sqrt(float) then.
Re: Changing the FW portion of the inverter SW
Posted: Mon Nov 01, 2021 6:35 pm
by bexander
Made some changes to the sqrt function, to build without warnings, but will need to find a better one later on as this loses some precision compared to the on used in math.h.
Code: Select all
float FOC::sqrtf(float x)
{
union
{
int i;
float x;
} u;
u.x = x;
float xhalf = 0.5f * u.x;
//int i = *(int*)&x;
u.i = 0x5f375a86 - (u.i >> 1);
//x = *(float*)&i;
u.x = u.x * (1.5f - xhalf * u.x * u.x);
u.x = u.x * (1.5f - xhalf * u.x * u.x);
u.x = u.x * (1.5f - xhalf * u.x * u.x);
u.x = 1 / u.x;
return u.x;
}
EDIT:
Will hopefully be able to test the code on wednesday.
Re: Changing the FW portion of the inverter SW
Posted: Wed Nov 03, 2021 10:30 am
by bexander
Tested the SW and that was a great disappointment. I couldn't even get it to take my gear selection, so clearly I have messed something up badly.
Re: Changing the FW portion of the inverter SW
Posted: Wed Nov 03, 2021 6:28 pm
by johu
Did you check that you can compile and run the "vanilla" software without your changes?
Re: Changing the FW portion of the inverter SW
Posted: Thu Nov 04, 2021 7:45 am
by bexander
No, but will do.
Re: Changing the FW portion of the inverter SW
Posted: Fri Nov 05, 2021 7:50 am
by bexander
If I make no chages it works well to use the compiled SW.
I suspect the param_prj.h file as I don't get the version to change and my added parameters don't show up in the web-interface.
The lines I've changed are,
Code: Select all
#define VER 5.13.R_math
//Next param id (increase when adding new parameter!): 142
PARAM_ENTRY(CAT_MOTOR, syncadv, "dig/Hz", 0, 65535, 10, 133 ) \
PARAM_ENTRY(CAT_MOTOR, rs, "mohm", 0, 1000, 75, 138 ) \
PARAM_ENTRY(CAT_MOTOR, ld, "mH", 0, 100, 1, 139 ) \
PARAM_ENTRY(CAT_MOTOR, lq, "mH", 0, 100, 2, 140 ) \
PARAM_ENTRY(CAT_MOTOR, fluxlinkage, "mWeber", 0, 1000, 108, 141 )
Attached complete param_prj.h file,
Do I need to do something extra to include this??
Have done,
git clone
https://github.com/jsphuebner/stm32-sine.git
Then rename the folder and run,
make get-deps
Then made changes to param_prj.h, foc.cpp, foc,h and pwmgeneration-foc.cpp then finally,
CONTROL=FOC make
Builds with no errors or warnings.
Re: Changing the FW portion of the inverter SW
Posted: Fri Nov 05, 2021 1:08 pm
by bexander
Found the issue! Need to use "make clean" if you already have built once before.
So now the testing can commence.
Re: Changing the FW portion of the inverter SW
Posted: Fri Nov 05, 2021 2:29 pm
by johu
Oh, yes. Somehow make doesn't detect changed header files.
FOC_Mathworks_Calculations_3.ods