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;
}
}
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 )