Base code for Current mode PID.

.gitignore

+.pio
+.vscode
\ No newline at end of file

src/opalib_pid_current_mode.c

+/*
+ * Copyright (c) 2021 LAAS-CNRS
+ *
+ *   This program is free software: you can redistribute it and/or modify
+ *   it under the terms of the GNU Lesser General Public License as published by
+ *   the Free Software Foundation, either version 2.1 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU Lesser General Public License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ * SPDX-License-Identifier: LGLPV2.1
+ */
+
+/**
+ * @author Clément Foucher <clement.foucher@laas.fr>
+ */
+
+
+// Owntech modules
+#include "data_acquisition.h"
+#include "dac.h"
+
+
+static const struct device* dac1 = NULL;
+static const struct device* dac3 = NULL;
+
+static double ihigh;
+static double v1low;
+static double v2low;
+static double vhigh;
+
+static double setPoint;
+
+static double p;
+static double i;
+static double d;
+
+static const double period = 50e-6;
+
+static uint32_t reset_value;
+
+
+/////
+// Private functions
+
+void _opalib_pid_current_acquire_dac_values()
+{
+	static uint32_t v1LowRaw = 0;
+	static uint32_t v2LowRaw = 0;
+	static uint32_t vHighRaw = 0;
+	static uint32_t iHighRaw = 0;
+
+	// I
+
+	while (data_dispatch_get_values_available_in_adc1_channel(0) != 0)
+	{
+		// Ignore I1-low, just empty the buffer
+		data_dispatch_get_next_value_from_adc1_channel(0);
+	}
+	while (data_dispatch_get_values_available_in_adc1_channel(1) != 0)
+	{
+		// Ignore I2-low, just empty the buffer
+		data_dispatch_get_next_value_from_adc1_channel(1);
+	}
+	while (data_dispatch_get_values_available_in_adc1_channel(2) != 0)
+	{
+		// Acquire latest I-high data
+		iHighRaw = data_dispatch_get_next_value_from_adc1_channel(2);
+	}
+
+	// V
+
+	while (data_dispatch_get_values_available_in_adc2_channel(0) != 0)
+	{
+		// Acquire latest V1-low data
+		v1LowRaw = data_dispatch_get_next_value_from_adc2_channel(0);
+	}
+	while (data_dispatch_get_values_available_in_adc2_channel(1) != 0)
+	{
+		// Acquire latest V2-low data
+		v2LowRaw = data_dispatch_get_next_value_from_adc2_channel(1);
+	}
+	while (data_dispatch_get_values_available_in_adc2_channel(2) != 0)
+	{
+		// Acquire latest V-high data
+		vHighRaw = data_dispatch_get_next_value_from_adc2_channel(2);
+	}
+
+	/////
+	// Convert raw values
+
+	ihigh = (0.0303030312F * iHighRaw) - 56.121212F;
+	v1low = (0.0304878056F * v1LowRaw) + 0.000439482072F;
+	v2low = (0.0264900662F * v2LowRaw) - 51.6821175F;
+	vhigh = (0.0664451793F * vHighRaw) + 4.31893682F;
+}
+
+static void _opalib_pid_current_compute_slope()
+{
+	// TODO calibration => update values
+
+	double slope1 = 200000.0F * (10638.2979F * setPoint);
+	uint32_t slope1Raw = ((int)(1638.4F * (0.2F * slope1))) % 65536;
+	if (slope1Raw > 4096)
+		slope1Raw = 4096;
+
+	double slope3 = slope1 * 0.0133333337F;
+	uint32_t slope3Raw = ((int)(1638.4F * (0.2F * slope3))) % 65536;
+	if (slope3Raw > 4096)
+		slope3Raw = 4096;
+
+	dac_function_update_step(dac1, 1, slope1Raw);
+	dac_function_update_step(dac3, 1, slope3Raw);
+}
+
+static void _opalib_pid_current_compute_reset_value()
+{
+	static double integrator_mem = 0;
+	static double derivative_mem = 0;
+
+	/////
+	// Compute error
+
+	double error = setPoint - v1low;
+
+	/////
+	// Compute derivative term
+
+	double derivativeTerm  = d * error / period;
+	double sum = (p * error) + integrator_mem + derivativeTerm - derivative_mem;
+	derivative_mem = derivativeTerm;
+
+	/////
+	// Compute reset value
+
+	// TODO: warning, values have changed!
+	reset_value = (uint32_t)(0.000625F * sum);
+
+	/////
+	// Compute integral term
+
+	integrator_mem += i * error * period;
+}
+
+static void _opalib_pid_current_update_reset_value()
+{
+	dac_function_update_reset(dac1, 1, reset_value);
+	dac_function_update_reset(dac3, 1, reset_value);
+}
+
+
+/////
+// Public API
+
+void opalib_pid_current_init(double setpoint, double p_i, double i_i, double d_i)
+{
+	dac1 = device_get_binding(DAC1_LABEL);
+	dac3 = device_get_binding(DAC3_LABEL);
+
+	setPoint = setpoint;
+	p = p_i;
+	i = i_i;
+	d = d_i;
+
+	_opalib_pid_current_compute_slope();
+}
+
+void opalib_pid_current_update_setpoint(double setpoint)
+{
+	setPoint = setpoint;
+
+	_opalib_pid_current_compute_slope();
+}
+
+void opalib_pid_current_periodic_task()
+{
+	_opalib_pid_current_acquire_dac_values();
+	_opalib_pid_current_compute_reset_value();
+	_opalib_pid_current_update_reset_value();
+}

src/opalib_pid_current_mode.h

+/*
+ * Copyright (c) 2021 LAAS-CNRS
+ *
+ *   This program is free software: you can redistribute it and/or modify
+ *   it under the terms of the GNU Lesser General Public License as published by
+ *   the Free Software Foundation, either version 2.1 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU Lesser General Public License for more details.
+ *
+ *   You should have received a copy of the GNU Lesser General Public License
+ *   along with this program.  If not, see <https://www.gnu.org/licenses/>.
+ *
+ * SPDX-License-Identifier: LGLPV2.1
+ */
+
+/**
+ * @author Clément Foucher <clement.foucher@laas.fr>
+ */
+
+#ifndef OPALIB_PID_CURRENT_MODE_H_
+#define OPALIB_PID_CURRENT_MODE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+void opalib_pid_current_init(double setpoint, double p_i, double i_i, double d_i);
+void opalib_pid_current_update_setpoint(double setpoint);
+void opalib_pid_current_periodic_task();
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // OPALIB_PID_CURRENT_MODE_H_