rusefi
/
wideband
mirror of https://github.com/rusefi/wideband.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

add D term

This commit is contained in:
Matthew Kennedy 2021-07-09 11:16:46 -07:00
parent d13af9b94b
commit b44fe3b98c
4 changed files with 11 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
firmware/heater_control.cpp
View File

@ -78,7 +78,7 @@ static HeaterState GetNextState(HeaterState state, float sensorEsr)
return state;
}
static Pid heaterPid(0.003f, 0.005f, 0.6f, HEATER_CONTROL_PERIOD);
static Pid heaterPid(0.003f, 0.005f, 0, 0.6f, HEATER_CONTROL_PERIOD);
static float GetDutyForState(HeaterState state, float heaterEsr)
{

7
firmware/pid.cpp
View File

@ -7,10 +7,15 @@ float Pid::GetOutput(float setpoint, float observation)
// Integrate error
m_integrator += error * m_period * m_ki;
// Differentiate error
float errorDelta = error - m_lastError;
float dEdt = errorDelta / m_period;
m_lastError = error;
// Clamp to +- 1
if (m_integrator > m_clamp) m_integrator = m_clamp;
if (m_integrator < -m_clamp) m_integrator = -m_clamp;
// Multiply by gains and sum
return m_kp * error + m_integrator;
return m_kp * error + m_integrator + m_kd * dEdt;
}

4
firmware/pid.h
View File

@ -3,10 +3,11 @@
class Pid
{
public:
Pid(float kP, float kI, float clamp, int periodMs)
Pid(float kP, float kI, float kD, float clamp, int periodMs)
: m_period(periodMs / 1000.0f)
, m_kp(kP)
, m_ki(kI)
, m_kd(kD)
, m_clamp(clamp)
{
}
@ -17,6 +18,7 @@ private:
const float m_period;
const float m_kp;
const float m_ki;
const float m_kd;
const float m_clamp;
float m_lastError;

2
firmware/pump_control.cpp
View File

@ -8,7 +8,7 @@
#include "ch.h"
// Bosch CJ125 is somewhere VERY ROUGHLY like 200-400A/(v*s) integrator gain
static Pid pumpPid(50.0f, 10000.0f, 10, 2);
static Pid pumpPid(50.0f, 10000.0f, 0, 10, 2);
static THD_WORKING_AREA(waPumpThread, 256);
static void PumpThread(void*)