lots of stuff

source/direct.c

@@ -0,0 +1,294 @@
+#include "direct.h"
+#include "driver.h"
+#include "ports.h"
+#include "fgen.h"
+#include "utils.h"
+#include "System/system.h"
+
+extern SYSTEM_DATA_t sys;
+
+enum
+{
+	SET_ALKALINE = 0,
+	SET_LITHIUM,
+	SET_HF,
+	NUM_SETPOINTS
+};
+
+
+
+static float _setpoints[NUM_SETPOINTS][POWER_LEVEL_COUNT] = {
+	{ 0.010, 0.050, 0.100, 0.150, 0.200 },		// Alkaline
+	{ 0.010, 0.050, 0.150, 0.400, 1.000 },		// Lithium
+	{ 0.010, 0.050, 0.100, 0.150, 0.200 }		// HF
+};
+
+static const float DIRECT_KP = 10.1f;
+static const float DIRECT_KI = 10.5f;
+static const float DIRECT_KD = 0.0f;
+
+enum
+{
+	KP = 0,
+	KI,
+	KD,
+	TERM_COUNT
+};
+
+typedef struct 
+{
+	float k[TERM_COUNT];
+	float err;
+	float output;
+	float err_int;
+	float setpoint;
+	bool  enabled;
+} Controller_t;
+
+
+static FREQUENCY_t * setFrequency(ACCESSORY_t *accy, FREQUENCY_t *freq)
+{
+	// is this an appropriate frequency?  if not, return one that is
+
+    return freq;
+}
+
+static int setPower(ACCESSORY_t *accy, PowerLevel_t power)
+{
+    FREQUENCY_t *f = driver_getFrequency();
+	PowerLevel_t prevPower = driver_getPowerLevel();
+
+	Controller_t *controller = (Controller_t*)accy->data;
+
+	if (prevPower == POWER_LEVEL_0 && power > POWER_LEVEL_0)
+	{
+		driver_bypass(true);
+		driver_isolateOutput(false);
+
+
+		controller->enabled = true;
+		
+	}
+	else
+	if (prevPower > POWER_LEVEL_0 && power == POWER_LEVEL_0)
+	{
+		driver_bypass(false);
+		driver_isolateOutput(true);
+
+		driver_setAmplitude(0);
+		controller->enabled = false;
+	}
+
+	if (driver_isLowFreq())
+	{
+		if (sys.batteryType == ALKALINE)
+		{
+			controller->setpoint = _setpoints[SET_ALKALINE][power];
+		}
+		else
+		{
+			controller->setpoint = _setpoints[SET_LITHIUM][power];
+		}
+	}
+	else
+	{
+		controller->setpoint = _setpoints[SET_HF][power];
+	}
+
+#if 0
+	driver_setAmplitude(power * 20);
+
+	if (prevPower == POWER_LEVEL_0 && power > POWER_LEVEL_0)
+	{
+		driver_bypass(true);
+		driver_isolateOutput(false);
+	}
+	else
+	if (prevPower > POWER_LEVEL_0 && power == POWER_LEVEL_0)
+	{
+		driver_bypass(false);
+		driver_isolateOutput(true);
+	}
+#endif
+    return 0;
+}
+
+
+
+
+static void regulate(ACCESSORY_t *accy)
+{
+	Controller_t *c = (Controller_t*)accy->data;
+
+	// check for disconnect
+	if (accy->loadConnected)
+	{
+		if (sys.adc.Ohms_slowfilt > 10000)
+		{
+			driver_setAmplitude(20);
+			accy->loadConnected = false;
+			c->enabled = false;
+		}
+	}
+	else
+	{
+		if (sys.adc.Ohms_slowfilt < 3000)
+		{
+			driver_setAmplitude(20);
+			accy->loadConnected = true;
+			c->enabled = true;
+		}
+	}
+
+	if (!c->enabled)
+	{
+		return;
+	}
+
+#if 0
+
+	float err = c->setpoint - sys.adc.I_OUT_SlowFilt;
+	float err_int = c->err_int + err;
+
+	c->output = err * c->k[KP] + err_int * c->k[KI];
+
+	if (c->output > 180.0f)
+	{
+		c->output = 180.0f;
+	}
+	else
+	{
+		c->err_int = err_int;
+	}
+	
+	driver_setAmplitude((uint8_t)(c->output + 0.5f));
+#endif	
+
+	float targetCurrent = driver_maxLoadCurrent();
+
+	if (c->setpoint < targetCurrent)
+	{
+		targetCurrent = c->setpoint;
+	}
+
+	int pot = driver_getAmplitude();
+	int newPot = 3;
+
+	if (pot > 0)
+	{
+		newPot = (targetCurrent / sys.adc.I_OUT_SlowFilt) * pot;  // Request divided by Pot current contents
+	}
+
+
+	// limit change to 20
+	if (newPot - pot > 20)
+	{
+		newPot = pot + 20;
+	}
+	else
+	if (newPot - pot < -20)
+	{
+		newPot = pot - 20;
+	}
+
+	if (newPot > 255)
+	{
+		newPot = 255;
+	}
+
+	// limit if the estimated voltage is over the maximum
+
+	driver_setAmplitude((uint8_t)newPot);
+
+}
+
+static void _stateInit(ACCESSORY_t *accy)
+{
+    FREQUENCY_t *freq = driver_getFrequency();
+
+    if (accy->initState)
+    {
+        accy->setFrequency = setFrequency;
+        accy->setPower = setPower;
+
+        accy->signalPath = SIGNAL_PATH_AMP;
+
+		accy->driveVoltage[LOW_FREQ] = V_36V;
+        accy->driveVoltage[HIGH_FREQ] = V_24V;
+
+        driver_setFrequency(freq);
+
+        driver_broadcastOn(true);
+        
+        ACCY_setTimeout(accy, 500);
+    }
+
+    if (ACCY_timedOut(accy))
+    {
+        accy->state = PORT_STATE_WAIT_FOR_DRIVER;
+    }
+}
+
+
+int direct_service(ACCESSORY_t *accy)
+{
+	switch (accy->state)
+	{
+		case PORT_STATE_INIT:
+		{
+            _stateInit(accy);
+			break;
+		}
+
+		case PORT_STATE_DEINIT:
+		{
+			if (accy->initState)
+			{
+
+				accy->stateTimer = sys.systemTime + 500;
+			}
+
+			if (sys.systemTime >= accy->stateTimer)
+			{
+				accy->state = PORT_STATE_STANDBY;
+			}
+
+			break;
+		}
+
+		case PORT_STATE_WAIT_FOR_DRIVER:
+		{
+			if (driver_outputReady())
+			{
+				accy->state = PORT_STATE_RUNNING;
+			}
+			break;
+		}
+
+		case PORT_STATE_RUNNING:
+		{
+			if (accy->initState)
+			{
+				Controller_t *controller = (Controller_t*)accy->data;
+				controller->k[KP] = DIRECT_KP;
+				controller->k[KI] = DIRECT_KI;
+				controller->k[KD] = DIRECT_KD;
+
+				controller->err_int = 0.0f;
+
+				controller->enabled = false;
+
+				setPower(accy, driver_getPowerLevel());
+
+			}
+
+			regulate(accy);
+			break;	
+		}
+		
+		default:
+			return -1;
+	}
+
+	return 0;
+}