Skip to content

Commit

Permalink
sine current control prototype
Browse files Browse the repository at this point in the history
  • Loading branch information
@catphish
catphish committed Jan 16, 2023
1 parent a3913ba commit 3573500
Show file tree
Hide file tree
Showing 5 changed files with 24 additions and 226 deletions.
32 changes: 3 additions & 29 deletions include/param_prj.h
View file
Expand Up @@ -37,20 +37,14 @@
PARAM_ENTRY(CAT_MOTOR, numimp, "ppr", 8, 8192, 60, 15 ) \
PARAM_ENTRY(CAT_MOTOR, dirchrpm, "rpm", 0, 20000, 100, 87 ) \
PARAM_ENTRY(CAT_MOTOR, dirmode, DIRMODES, 0, 4, 1, 95 ) \
PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 23, 12, 46 )
PARAM_ENTRY(CAT_MOTOR, throtcur, "A/%", 0, 10, 1, 105 ) \
PARAM_ENTRY(CAT_MOTOR, snsm, SNS_M, 12, 23, 12, 46 ) \
PARAM_ENTRY(CAT_MOTOR, curkp, "", 0, 20000, 32, 107 ) \

#define MOTOR_PARAMETERS_SINE \
PARAM_ENTRY(CAT_MOTOR, boost, "dig", 0, 37813, 1700, 1 ) \
PARAM_ENTRY(CAT_MOTOR, fweak, "Hz", 0, 1000, 90, 2 ) \
PARAM_ENTRY(CAT_MOTOR, fweakstrt, "Hz", 0, 1000, 400, 134 ) \
PARAM_ENTRY(CAT_MOTOR, fconst, "Hz", 0, 1000, 180, 99 ) \
PARAM_ENTRY(CAT_MOTOR, udcnom, "V", 0, 1000, 0, 78 ) \
PARAM_ENTRY(CAT_MOTOR, fslipmin, "Hz", 0.3, 10, 1, 37 ) \
PARAM_ENTRY(CAT_MOTOR, fslipmax, "Hz", 0.3, 10, 3, 33 ) \
PARAM_ENTRY(CAT_MOTOR, fslipconstmax,"Hz", 0, 10, 5, 100 )

#define MOTOR_PARAMETERS_FOC \
PARAM_ENTRY(CAT_MOTOR, curkp, "", 0, 20000, 32, 107 ) \
PARAM_ENTRY(CAT_MOTOR, curki, "", 0, 100000, 20000, 108 ) \
PARAM_ENTRY(CAT_MOTOR, vlimflt, "", 0, 16, 10, 145 ) \
PARAM_ENTRY(CAT_MOTOR, vlimmargin, "dig", 0, 10000, 2500, 141 ) \
Expand Down Expand Up @@ -89,11 +83,6 @@
PARAM_ENTRY(CAT_DERATE, throtmax, "%", 0, 100, 100, 97 ) \
PARAM_ENTRY(CAT_DERATE, throtmin, "%", -100, 0, -100, 119 )

#define DERATE_PARAMETERS_SINE \
PARAM_ENTRY(CAT_DERATE, iacmax, "A", 0, 5000, 5000, 89 ) \
PARAM_ENTRY(CAT_DERATE, ifltrise, "dig", 0, 32, 10, 91 ) \
PARAM_ENTRY(CAT_DERATE, ifltfall, "dig", 0, 32, 3, 92 ) \

#define CHARGER_PARAMETERS \
PARAM_ENTRY(CAT_CHARGER, chargemode, CHARGEMODS,0, 4, 0, 74 ) \
PARAM_ENTRY(CAT_CHARGER, chargecur, "A", 0, 50, 0, 71 ) \
Expand All @@ -112,14 +101,6 @@
PARAM_ENTRY(CAT_THROTTLE,throtramp, "%/10ms", 0.1, 100, 100, 81 ) \
PARAM_ENTRY(CAT_THROTTLE,throtramprpm,"rpm", 0, 20000, 20000, 85 )

#define THROTTLE_PARAMETERS_SINE \
PARAM_ENTRY(CAT_THROTTLE,ampmin, "%", 0, 100, 10, 4 ) \
PARAM_ENTRY(CAT_THROTTLE,slipstart, "%", 10, 100, 50, 90 ) \
PARAM_ENTRY(CAT_THROTTLE,throtfilter, "dig", 0, 10, 4, 147 )

#define THROTTLE_PARAMETERS_FOC \
PARAM_ENTRY(CAT_THROTTLE,throtcur, "A/%", 0, 10, 1, 105 )

#define REGEN_PARAMETERS \
PARAM_ENTRY(CAT_REGEN, brakeregen, "%", -100, 0, -50, 38 ) \
PARAM_ENTRY(CAT_REGEN, regenramp, "%/10ms", 0.1, 100, 100, 68 ) \
Expand Down Expand Up @@ -191,10 +172,6 @@
#define VALUES_SINE \
VALUE_ENTRY(ilmax, "A", 2005 ) \
VALUE_ENTRY(uac, "V", 2006 ) \
VALUE_ENTRY(il1rms, "A", 2007 ) \
VALUE_ENTRY(il2rms, "A", 2008 ) \
VALUE_ENTRY(boostcalc, "dig", 2009 ) \
VALUE_ENTRY(fweakcalc, "Hz", 2010 ) \

#define VALUES_FOC \
VALUE_ENTRY(id, "A", 2003 ) \
Expand All @@ -209,10 +186,8 @@
MOTOR_PARAMETERS_COMMON \
INVERTER_PARAMETERS_COMMON \
THROTTLE_PARAMETERS_COMMON \
THROTTLE_PARAMETERS_SINE \
REGEN_PARAMETERS \
DERATE_PARAMETERS_COMMON \
DERATE_PARAMETERS_SINE \
CHARGER_PARAMETERS \
AUTOMATION_CONTACT_PWM_COMM_PARAMETERS \
PARAM_ENTRY(CAT_TEST, fslipspnt, "Hz", -100, 1000, 0, 0 ) \
Expand Down Expand Up @@ -328,7 +303,6 @@ enum _modes
MOD_MANUAL,
MOD_BOOST,
MOD_BUCK,
MOD_SINE,
MOD_ACHEAT,
MOD_LAST
};
Expand Down
3 changes: 1 addition & 2 deletions include/pwmgeneration.h
View file
Expand Up @@ -52,8 +52,7 @@ class PwmGeneration
static s32fp ProcessCurrents();
static s32fp ProcessCurrents(s32fp& id, s32fp& iq);
static void CalcNextAngleSync();
static void CalcNextAngleAsync(int dir);
static void CalcNextAngleConstant(int dir);
static void CalcNextAngleAsync();
static void Charge();
static void AcHeat();
static s32fp GetIlMax(s32fp il1, s32fp il2);
Expand Down
166 changes: 17 additions & 149 deletions src/pwmgeneration-sine.cpp
View file
Expand Up @@ -37,19 +37,25 @@

void PwmGeneration::Run()
{
if (opmode == MOD_MANUAL || opmode == MOD_RUN || opmode == MOD_SINE)
if (opmode == MOD_MANUAL || opmode == MOD_RUN)
{
static uint32_t amp = 0;
int dir = Param::GetInt(Param::dir);

Encoder::UpdateRotorAngle(dir);
s32fp ampNomLimited = LimitCurrent();
ProcessCurrents();
CalcNextAngleAsync();

if (opmode == MOD_SINE)
CalcNextAngleConstant(dir);
else
CalcNextAngleAsync(dir);
// Adjust amplitude according to requested and measured current
uint32_t curkp = Param::GetInt(Param::curkp);
s32fp ilmax = Param::Get(Param::ilmax);
s32fp throtcur = Param::Get(Param::throtcur);
s32fp ilmaxtarget = FP_MUL(throtcur, ampnom);

uint32_t amp = MotorVoltage::GetAmpPerc(frq, ampNomLimited);
if (ilmax > ilmaxtarget && amp > 0)
amp -= curkp;
else if (ilmax < ilmaxtarget && amp < 37813)
amp += curkp;

SineCore::SetAmp(amp);
Param::SetInt(Param::amp, amp);
Expand Down Expand Up @@ -84,60 +90,11 @@ void PwmGeneration::Run()

void PwmGeneration::SetTorquePercent(float torque)
{
int filterConst = Param::GetInt(Param::throtfilter);
float roundingError = FP_TOFLOAT((float)((1 << filterConst) - 1));
float fslipmin = Param::GetFloat(Param::fslipmin);
float ampmin = Param::GetFloat(Param::ampmin);
float slipstart = Param::GetFloat(Param::slipstart);
float ampnomLocal;
float fslipspnt = 0;

if (torque >= 0)
{
/* In async mode first X% throttle commands amplitude, X-100% raises slip */
ampnomLocal = ampmin + (100.0f - ampmin) * torque / slipstart;
float fslipmax = Param::GetFloat(Param::fslipmax);

if (torque >= slipstart)
{
float fstat = Param::GetFloat(Param::fstat);
float fweak = Param::GetFloat(Param::fweakcalc);
float fslipmax = Param::GetFloat(Param::fslipmax);

if (fstat > fweak)
{
float fconst = Param::GetFloat(Param::fconst);
float fslipconstmax = Param::GetFloat(Param::fslipconstmax);
//Basically, for every Hz above fweak we get a fraction of
//the difference between fslipconstmax and fslipmax
//of additional slip
fslipmax += (fstat - fweak) / (fconst - fweak) * (fslipconstmax - fslipmax);
fslipmax = MIN(fslipmax, fslipconstmax); //never exceed fslipconstmax!
}
ampnom = FP_FROMFLT(ABS(torque));
fslip = FP_FROMFLT(torque / 100.f * fslipmax);

float fslipdiff = fslipmax - fslipmin;
fslipspnt = roundingError + fslipmin + (fslipdiff * (torque - slipstart)) / (100.0f - slipstart);
}
else
{
fslipspnt = fslipmin + roundingError;
}
}
else if (Encoder::GetRotorDirection() != Param::GetInt(Param::dir))
{
// Do not apply negative torque if we are already traveling backwards.
fslipspnt = 0;
ampnomLocal = 0;
}
else
{
ampnomLocal = -torque;
fslipspnt = -fslipmin;
}

ampnomLocal = MIN(ampnomLocal, 100.0f);
//anticipate sudden changes by filtering
ampnom = IIRFILTER(ampnom, FP_FROMFLT(ampnomLocal), filterConst);
fslip = IIRFILTER(fslip, FP_FROMFLT(fslipspnt), filterConst);
Param::Set(Param::ampnom, ampnom);
Param::Set(Param::fslipspnt, fslip);

Expand All @@ -155,36 +112,6 @@ void PwmGeneration::PwmInit()
AcHeatTimerSetup();
}

s32fp PwmGeneration::LimitCurrent()
{
static s32fp curLimSpntFiltered = 0, slipFiltered = 0;
s32fp slipmin = Param::Get(Param::fslipmin);
s32fp imax = Param::Get(Param::iacmax);
s32fp ilMax = ProcessCurrents();

//setting of 0 disables current limiting
if (imax == 0) return ampnom;

s32fp a = imax / 20; //Start acting at 80% of imax
s32fp imargin = imax - ilMax;
s32fp curLimSpnt = FP_DIV(100 * imargin, a);
s32fp slipSpnt = FP_DIV(FP_MUL(fslip, imargin), a);
slipSpnt = MAX(slipmin, slipSpnt);
curLimSpnt = MAX(FP_FROMINT(40), curLimSpnt); //Never go below 40%
int filter = Param::GetInt(curLimSpnt < curLimSpntFiltered ? Param::ifltfall : Param::ifltrise);
curLimSpntFiltered = IIRFILTER(curLimSpntFiltered, curLimSpnt, filter);
slipFiltered = IIRFILTER(slipFiltered, slipSpnt, 1);

s32fp ampNomLimited = MIN(ampnom, curLimSpntFiltered);
slipSpnt = MIN(fslip, slipFiltered);
slipIncr = FRQ_TO_ANGLE(slipSpnt);

if (curLimSpnt < ampnom)
ErrorMessage::Post(ERR_CURRENTLIMIT);

return ampNomLimited;
}

s32fp PwmGeneration::GetIlMax(s32fp il1, s32fp il2)
{
s32fp il3 = -il1 - il2;
Expand All @@ -195,71 +122,12 @@ s32fp PwmGeneration::GetIlMax(s32fp il1, s32fp il2)
return ilMax;
}

PwmGeneration::EdgeType PwmGeneration::CalcRms(s32fp il, EdgeType& lastEdge, s32fp& max, s32fp& rms, int& samples, s32fp prevRms)
{
const s32fp oneOverSqrt2 = FP_FROMFLT(0.707106781187);
int minSamples = pwmfrq / (4 * FP_TOINT(frq));
EdgeType edgeType = NoEdge;

minSamples = MAX(10, minSamples);

if (samples > minSamples)
{
if (lastEdge == NegEdge && il > 0)
edgeType = PosEdge;
else if (lastEdge == PosEdge && il < 0)
edgeType = NegEdge;
}

if (edgeType != NoEdge)
{
rms = (FP_MUL(oneOverSqrt2, max) + prevRms) / 2; // average with previous rms reading

max = 0;
samples = 0;
lastEdge = edgeType;
}

il = ABS(il);
max = MAX(il, max);
samples++;

return edgeType;
}

s32fp PwmGeneration::ProcessCurrents()
{
static s32fp currentMax[2];
static int samples[2] = { 0 };
static int sign = 1;
static EdgeType lastEdge[2] = { PosEdge, PosEdge };

s32fp il1 = GetCurrent(AnaIn::il1, ilofs[0], Param::Get(Param::il1gain));
s32fp il2 = GetCurrent(AnaIn::il2, ilofs[1], Param::Get(Param::il2gain));
s32fp rms;
s32fp il1PrevRms = Param::Get(Param::il1rms);
s32fp il2PrevRms = Param::Get(Param::il2rms);
EdgeType edge = CalcRms(il1, lastEdge[0], currentMax[0], rms, samples[0], il1PrevRms);

if (edge != NoEdge)
{
Param::SetFixed(Param::il1rms, rms);

if (opmode != MOD_BOOST || opmode != MOD_BUCK)
{
//rough approximation as we do not take power factor into account
s32fp idc = (SineCore::GetAmp() * rms) / SineCore::MAXAMP;
idc = FP_MUL(idc, FP_FROMFLT(1.2247)); //multiply by sqrt(3)/sqrt(2)
idc *= fslip < 0 ? -1 : 1;
Param::SetFixed(Param::idc, idc);
}
}
if (CalcRms(il2, lastEdge[1], currentMax[1], rms, samples[1], il2PrevRms))
{
Param::SetFixed(Param::il2rms, rms);
}

s32fp ilMax = sign * GetIlMax(il1, il2);
s32fp ilMax = GetIlMax(il1, il2);

Param::SetFixed(Param::il1, il1);
Param::SetFixed(Param::il2, il2);
Expand Down
13 changes: 2 additions & 11 deletions src/pwmgeneration.cpp
View file
Expand Up @@ -149,7 +149,6 @@ void PwmGeneration::SetOpmode(int _opmode)
break;
case MOD_MANUAL:
case MOD_RUN:
case MOD_SINE:
EnableOutput();
break;
}
Expand Down Expand Up @@ -272,27 +271,19 @@ void PwmGeneration::SetChargeCurrent(float cur)

/*----- Private methods ----------------------------------------- */

void PwmGeneration::CalcNextAngleAsync(int dir)
void PwmGeneration::CalcNextAngleAsync()
{
static uint16_t slipAngle = 0;
uint16_t rotorAngle = Encoder::GetRotorAngle();

frq = polePairRatio * Encoder::GetRotorFrequency() + fslip;
slipAngle += dir * slipIncr;
slipAngle += slipIncr;

if (frq < 0) frq = 0;

angle = polePairRatio * rotorAngle + slipAngle;
}

void PwmGeneration::CalcNextAngleConstant(int dir)
{
frq = fslip;
angle += dir * slipIncr;

if (frq < 0) frq = 0;
}

void PwmGeneration::Charge()
{
static s32fp iFlt;
Expand Down
36 changes: 1 addition & 35 deletions src/vehiclecontrol.cpp
View file
Expand Up @@ -214,12 +214,10 @@ float VehicleControl::ProcessThrottle()
finalSpnt = (rotorfreq / brkrampstr) * finalSpnt;
}

#if CONTROL == CTRL_FOC
if (finalSpnt < 0)
finalSpnt *= Encoder::GetRotorDirection();
else //inconsistency here: in slip control negative always means regen
else
finalSpnt *= Param::GetInt(Param::dir);
#endif // CONTROL
}

return finalSpnt;
Expand Down Expand Up @@ -379,38 +377,6 @@ float VehicleControl::ProcessUdc()
ErrorMessage::Post(ERR_PRECHARGE);
}

#if CONTROL == CTRL_SINE
{
float udcnom = Param::GetFloat(Param::udcnom);
float boost = Param::GetFloat(Param::boost);
float fweak;

if (Param::GetInt(Param::potnom) > 35)
{
fweak = MAP(Param::GetFloat(Param::potnom), 36, 100, (Param::GetFloat(Param::fweakstrt)), (Param::GetFloat(Param::fweak)));
}
else
{
fweak = Param::GetFloat(Param::fweakstrt);
}

if (udcnom > 0)
{
float udcdiff = udcfp - udcnom;
float factor = 1.0 + udcdiff / udcnom;
//increase fweak on voltage above nominal
fweak = fweak * factor;
//decrease boost on voltage above nominal
boost = boost / factor;
}

Param::SetFloat(Param::fweakcalc, fweak);
Param::SetFloat(Param::boostcalc, boost);
MotorVoltage::SetWeakeningFrq(fweak);
MotorVoltage::SetBoost(boost);
}
#endif // CONTROL

Param::SetFloat(Param::udc, udcfp);

return udcfp;
Expand Down

0 comments on commit 3573500

Please sign in to comment.