Legacy branch migration

2024-03-12 21:23:47 +02:00
parent 02cb3a9c70
commit ddf9d263b1
139 changed files with 2476 additions and 14269 deletions
--- a/firmware/logic/coil.c
+++ b/firmware/logic/coil.c
@@ -0,0 +1,127 @@
+/**** Includes ****/
+#include "coil.h"
+
+/**** Private definitions ****/
+
+/**** Private constants ****/
+
+/**** Private variables ****/
+static uint16_t lock_current = 4500; //mA
+static uint16_t min_current = 100; //mA
+static uint16_t target_current = 0; //mA
+static uint8_t new_target = 1;
+
+static uint16_t adj_target_current = 0; //mA
+static uint16_t target_voltage = 0; //mV
+
+static uint8_t act_force = 0;
+
+static uint16_t nominal_resitance = 1500; //mR
+
+static int32_t sum_current = 0;
+static uint8_t sum_cnt = 0;
+
+
+/**** Private function declarations ****/
+static uint16_t UpdateVoltage(uint16_t cur, uint16_t res);
+
+/**** Public function definitions ****/
+void Coil_SetLockCurrent(uint16_t lock_i)
+{
+	lock_current = lock_i;
+}
+
+void Coil_SetTarget_Force(uint8_t force)
+{
+	// Check if worth doing
+	if(force == act_force) return;
+	
+	// Calculate new target current
+	act_force = force;
+	
+	// Check simple answer
+	uint16_t new_current = 0;
+	if(force==0) new_current = 0;
+	else if(force >= 100) new_current = lock_current;
+	else
+	{
+		uint32_t temp = (uint32_t)force * lock_current;
+		temp /= 100;
+		
+		if(temp > 0x0000FFFF) new_current = lock_current;
+		else new_current = (uint16_t) temp;
+	}
+	
+	// Update new target
+	Coil_SetTarget_Current(new_current);
+}
+
+void Coil_SetTarget_Current(uint16_t current)
+{
+	if(current >= lock_current) current = lock_current;
+	else if((current > 0)&&(current <= min_current)) current = min_current;
+	
+	if(current != target_current) new_target = 1;
+	target_current = current;
+}
+
+uint16_t Coil_GetTargetVolatge(void)
+{
+	return target_voltage;
+}
+
+uint16_t Coil_Update(analog_t* meas)
+{
+	// Collect average current
+	sum_current += meas->hb_currnet;
+	sum_cnt++;
+	
+	// Update measurement
+	if(sum_cnt >= 10)
+	{
+		// Calculate average
+		sum_current /= sum_cnt;
+		
+		// Calculate error
+		int32_t error = sum_current - (int32_t)target_current;
+		int32_t temp = (int32_t)adj_target_current - error;
+		
+		// Limit to 16bits
+		if(temp<0) adj_target_current = 0;
+		else if(temp > 0x0000FFFF) adj_target_current = 0xFFFF;
+		else adj_target_current = (uint16_t)temp;
+		sum_cnt = 0;
+		sum_current = 0;
+	};
+	
+	// Closed loop or open loop target set
+	if(new_target)
+	{
+		// Set open-loop HB output
+		target_voltage = UpdateVoltage(target_current, nominal_resitance);
+		adj_target_current = target_current;
+		new_target = 0;
+		sum_cnt = 0;
+		sum_current = 0;
+	}
+	else
+	{
+		target_voltage = UpdateVoltage(adj_target_current, nominal_resitance);
+	}
+	
+	return target_voltage;
+}
+	
+/**** Private function definitions ****/
+static uint16_t UpdateVoltage(uint16_t cur, uint16_t res)
+{
+	// Update settable voltage
+	if(cur==0) return 0;
+	else
+	{
+		uint32_t volt = (uint32_t)cur * res;
+		volt /= 1000;
+		if(volt > 0x0000FFFF) return 0xFFFF;
+		else return (uint16_t)volt;
+	}
+}