Skip to content

Commit

Permalink
Moved derating functions to throttle
Browse files Browse the repository at this point in the history
  • Loading branch information
@jsphuebner
jsphuebner committed Apr 6, 2019
1 parent 8b55f53 commit 15b4278
Show file tree
Hide file tree
Showing 4 changed files with 88 additions and 62 deletions.
5 changes: 3 additions & 2 deletions include/project/param_prj.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -66,7 +66,7 @@
#define POTMODE_DUALCHANNEL 1
#define POTMODE_CAN 2

#define VER 4.15.B
#define VER 4.17.R
#define VERSTR STRINGIFY(4=VER)

enum _modes
Expand Down Expand Up @@ -171,7 +171,7 @@ extern const char* errorListString;
PARAM_ENTRY(CAT_REGEN, brknompedal, "%", -100, 0, -50, 38 ) \
PARAM_ENTRY(CAT_REGEN, brkpedalramp,"%/10ms", 1, 100, 100, 68 ) \
PARAM_ENTRY(CAT_REGEN, brknom, "%", 0, 100, 30, 19 ) \
PARAM_ENTRY(CAT_REGEN, brkmax, "%", 0, 100, 30, 49 ) \
PARAM_ENTRY(CAT_REGEN, brkmax, "%", -100, 0, -30, 49 ) \
PARAM_ENTRY(CAT_REGEN, brkrampstr, "Hz", 0, 400, 10, 39 ) \
PARAM_ENTRY(CAT_REGEN, brkout, "%", -100, -1, -50, 67 ) \
PARAM_ENTRY(CAT_AUTOM, idlespeed, "rpm", -100, 1000, -100, 54 ) \
Expand Down Expand Up @@ -216,6 +216,7 @@ extern const char* errorListString;
VALUE_ENTRY(tmphs, "°C", 2019 ) \
VALUE_ENTRY(tmpm, "°C", 2020 ) \
VALUE_ENTRY(uaux, "V", 2021 ) \
VALUE_ENTRY(pwmio, "", 12022 ) \
VALUE_ENTRY(canio, CANIOS, 2022 ) \
VALUE_ENTRY(din_cruise, ONOFF, 2023 ) \
VALUE_ENTRY(din_start, ONOFF, 2024 ) \
Expand Down
11 changes: 11 additions & 0 deletions include/project/throttle.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,9 @@ class Throttle
static int CalcIdleSpeed(int speed);
static int CalcCruiseSpeed(int speed);
static bool TemperatureDerate(s32fp tmphs, int& finalSpnt);
static void BmsLimitCommand(int& finalSpnt, bool dinbms);
static void UdcLimitCommand(int& finalSpnt, s32fp udc);
static void IdcLimitCommand(int& finalSpnt, s32fp idc);
static int potmin[2];
static int potmax[2];
static int brknom;
Expand All @@ -46,6 +49,14 @@ class Throttle
static s32fp idleThrotLim;
static int brkPedalRamp;
static int throttleRamp;
static int bmslimhigh;
static int bmslimlow;
static int accelmax;
static int accelflt;
static s32fp udcmin;
static s32fp udcmax;
static s32fp idcmin;
static s32fp idcmax;

private:
static int speedFiltered;
Expand Down
74 changes: 16 additions & 58 deletions src/project/stm32_sine.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -526,61 +526,6 @@ static void GetCruiseCreepCommand(int& finalSpnt, int throtSpnt)
}
}

static void BmsLimitCommand(int& finalSpnt)
{
if (hwRev != HW_TESLA && Param::GetBool(Param::din_bms))
{
if (finalSpnt >= 0)
finalSpnt = (finalSpnt * Param::GetInt(Param::bmslimhigh)) / 100;
else
finalSpnt = -(finalSpnt * Param::GetInt(Param::bmslimlow)) / 100;
}
}

static void UdcLimitCommand(int& finalSpnt)
{
s32fp udc = Param::Get(Param::udc);

if (finalSpnt >= 0)
{
s32fp udcErr = udc - FP_MUL(Param::Get(Param::udcmin), FP_FROMFLT(0.95));
int res = FP_TOINT(udcErr * 5);
res = MAX(0, res);
finalSpnt = MIN(finalSpnt, res);
}
else
{
s32fp udcErr = udc - FP_MUL(Param::Get(Param::udcmax), FP_FROMFLT(1.05));
int res = FP_TOINT(udcErr * 5);
res = MIN(0, res);
finalSpnt = MAX(finalSpnt, res);
}
}

static void IdcLimitCommand(int& finalSpnt)
{
s32fp idc = Param::Get(Param::idc);

if (finalSpnt >= 0)
{
s32fp idcmax = Param::Get(Param::idcmax);
s32fp idcerr = idcmax - idc;
int res = FP_TOINT(idcerr * 10);

res = MAX(0, res);
finalSpnt = MIN(res, finalSpnt);
}
else
{
s32fp idcmin = Param::Get(Param::idcmin);
s32fp idcerr = idcmin - idc;
int res = FP_TOINT(idcerr * 10);

res = MIN(0, res);
finalSpnt = MAX(res, finalSpnt);
}
}

static void ProcessThrottle()
{
int throtSpnt, finalSpnt;
Expand All @@ -592,9 +537,12 @@ static void ProcessThrottle()

throtSpnt = GetUserThrottleCommand();
GetCruiseCreepCommand(finalSpnt, throtSpnt);
BmsLimitCommand(finalSpnt);
UdcLimitCommand(finalSpnt);
IdcLimitCommand(finalSpnt);

if (hwRev != HW_TESLA)
Throttle::BmsLimitCommand(finalSpnt, Param::GetBool(Param::din_bms));

Throttle::UdcLimitCommand(finalSpnt, Param::Get(Param::udc));
Throttle::IdcLimitCommand(finalSpnt, Param::Get(Param::idc));

if (Throttle::TemperatureDerate(Param::Get(Param::tmphs), finalSpnt))
{
Expand Down Expand Up @@ -824,6 +772,12 @@ extern void parm_Change(Param::PARAM_NUM paramNum)
Throttle::speedkp = Param::Get(Param::speedkp);
Throttle::speedflt = Param::GetInt(Param::speedflt);
Throttle::idleThrotLim = Param::Get(Param::idlethrotlim);
Throttle::bmslimlow = Param::GetInt(Param::bmslimlow);
Throttle::bmslimhigh = Param::GetInt(Param::bmslimhigh);
Throttle::udcmin = FP_MUL(Param::Get(Param::udcmin), FP_FROMFLT(0.95)); //Leave some room for the notification light
Throttle::udcmax = FP_MUL(Param::Get(Param::udcmax), FP_FROMFLT(1.05));
Throttle::idcmin = Param::Get(Param::idcmin);
Throttle::idcmax = Param::Get(Param::idcmax);

if (Param::GetInt(Param::pwmfunc) == PWM_FUNC_SPEEDFRQ)
gpio_set_mode(GPIOB, GPIO_MODE_OUTPUT_50_MHZ, GPIO_CNF_OUTPUT_PUSHPULL, GPIO9);
Expand All @@ -837,6 +791,8 @@ static void InitPWMIO()
uint8_t outputMode = Param::GetInt(Param::pwmpol) == 0 ? GPIO_MODE_OUTPUT_50_MHZ : GPIO_MODE_INPUT;
uint8_t outputConf = Param::GetInt(Param::pwmpol) == 0 ? GPIO_CNF_OUTPUT_ALTFN_PUSHPULL : GPIO_CNF_INPUT_FLOAT;

Param::SetInt(Param::pwmio, gpio_get(GPIOA, GPIO8 | GPIO9 | GPIO10) | gpio_get(GPIOB, GPIO13 | GPIO14 | GPIO15));

gpio_set_mode(GPIOA, outputMode, outputConf, GPIO8 | GPIO9 | GPIO10);
gpio_set_mode(GPIOB, outputMode, outputConf, GPIO13 | GPIO14 | GPIO15);
}
Expand Down Expand Up @@ -905,6 +861,8 @@ extern "C" int main(void)

if (Param::GetInt(Param::snsm) < 12)
Param::SetInt(Param::snsm, Param::GetInt(Param::snsm) + 10); //upgrade parameter
if (Param::Get(Param::brkmax) > 0)
Param::Set(Param::brkmax, -Param::Get(Param::brkmax));

term_Run();

Expand Down
60 changes: 58 additions & 2 deletions src/project/throttle.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,12 @@ int Throttle::brkPedalRamp;
int Throttle::brkRamped;
int Throttle::throttleRamp;
int Throttle::throttleRamped;
int Throttle::bmslimhigh;
int Throttle::bmslimlow;
s32fp Throttle::udcmin;
s32fp Throttle::udcmax;
s32fp Throttle::idcmin;
s32fp Throttle::idcmax;

bool Throttle::CheckAndLimitRange(int* potval, int potIdx)
{
Expand Down Expand Up @@ -88,7 +94,7 @@ bool Throttle::CheckDualThrottle(int* potval, int pot2val)
int Throttle::CalcThrottle(int potval, int pot2val, bool brkpedal)
{
int potnom;
int scaledBrkMax = brkpedal ? brknompedal : -brkmax;
int scaledBrkMax = brkpedal ? brknompedal : brkmax;

if (pot2val > potmin[1])
{
Expand Down Expand Up @@ -139,7 +145,7 @@ int Throttle::CalcCruiseSpeed(int speed)
{
speedFiltered = IIRFILTER(speedFiltered, speed, speedflt);
int speederr = cruiseSpeed - speedFiltered;
return FP_TOINT(MAX(FP_FROMINT(-brkmax), MIN(FP_FROMINT(100), speedkp * speederr)));
return FP_TOINT(MAX(FP_FROMINT(brkmax), MIN(FP_FROMINT(100), speedkp * speederr)));
}

bool Throttle::TemperatureDerate(s32fp tmphs, int& finalSpnt)
Expand All @@ -158,3 +164,53 @@ bool Throttle::TemperatureDerate(s32fp tmphs, int& finalSpnt)

return limit < 100;
}

void Throttle::BmsLimitCommand(int& finalSpnt, bool dinbms)
{
if (dinbms)
{
if (finalSpnt >= 0)
finalSpnt = (finalSpnt * bmslimhigh) / 100;
else
finalSpnt = -(finalSpnt * bmslimlow) / 100;
}
}

void Throttle::UdcLimitCommand(int& finalSpnt, s32fp udc)
{
if (finalSpnt >= 0)
{
s32fp udcErr = udc - udcmin;
int res = FP_TOINT(udcErr * 5);
res = MAX(0, res);
finalSpnt = MIN(finalSpnt, res);
}
else
{
s32fp udcErr = udc - udcmax;
int res = FP_TOINT(udcErr * 5);
res = MIN(0, res);
finalSpnt = MAX(finalSpnt, res);
}
}

void Throttle::IdcLimitCommand(int& finalSpnt, s32fp idc)
{
if (finalSpnt >= 0)
{
s32fp idcerr = idcmax - idc;
int res = FP_TOINT(idcerr * 10);

res = MAX(0, res);
finalSpnt = MIN(res, finalSpnt);
}
else
{
s32fp idcerr = idcmin - idc;
int res = FP_TOINT(idcerr * 10);

res = MIN(0, res);
finalSpnt = MAX(res, finalSpnt);
}
}

0 comments on commit 15b4278

Please sign in to comment.