pidcontroller.cpp

// SPDX-License-Identifier: AGPL-3.0-or-later

/**
 * @file pidcontroller.cpp
 * PID (proportional, integral, derivative) Controller
 *
 * @license{AGPL-3.0-or-later}
 */

#include <cmath>

#include "logger.h"
#include "pidcontroller.h"

#ifdef PID_CONTROLLER_TUNING_AID_ADDRESS
#include <arpa/inet.h>
#include <cstdio>
#include <cstring>
#include <sys/socket.h>
#include <unistd.h>
#endif

/**
 * Create a PID Controller.
 *
 * @param kp            Proportional gain
 * @param ki            Integral gain
 * @param kd            Derivative gain
 * @param maxOutput     Max allowed output value (absolute)
 */
cPidController::cPidController(double kp, double ki, double kd, double maxOutput)
	: proportionalGain(kp),
	  integralGain(ki),
	  derivativeGain(kd),
	  maxOutput(maxOutput)
{
	// Calculate max integral so the I-term alone can never exceed the max output
	if (integralGain > 0)
		maxIntegral = maxOutput / integralGain;
	else
		maxIntegral = maxOutput;
}

/**
 * Calculate the new output value.
 *
 * @param currentValue     The current value
 * @param dt               The time elapsed since the last update (seconds)
 * @return The output value
 */
double cPidController::Update(double currentValue, double dt)
{
	if (dt <= 0.0)
		return 0.0;

	// Error > 0 means we are below target
	double error = targetValue - currentValue;

	pTerm = proportionalGain * error;

	if (!firstRun) { // the dt value is not yet valid on the first run
		integralSum += error * dt;

		// Anti-Windup: Clamp the integrator
		integralSum = std::min(integralSum, maxIntegral);
		integralSum = std::max(integralSum, -maxIntegral);

		iTerm = integralGain * integralSum;
		dTerm = derivativeGain * ((error - previousError) / dt);
	}

	double output = pTerm + iTerm + dTerm;

	previousError = error;
	firstRun = false;

	// Limit the output. The controller is not reset at the limit: this would drop the
	// integral term, the correction would jump back and forth between the limit and zero.
	bool atLimit = std::abs(output) >= maxOutput;
	if (atLimit && !outputSaturated)
		LOGWARNING("pidcontroller: max output value reached");
	outputSaturated = atLimit;

	output = std::min(output, maxOutput);
	output = std::max(output, -maxOutput);
#ifdef PID_CONTROLLER_TUNING_AID_ADDRESS
	SendTuningAidData(pTerm, iTerm, dTerm, currentValue, output, targetValue);
#endif

	return output;
}

/**
 * Reset the internal state (integral sum and error history).
 */
void cPidController::Reset(void)
{
	firstRun = true;
	pTerm = 0;
	iTerm = 0;
	dTerm = 0;
	integralSum = 0.0;
	outputSaturated = false;
	previousError = 0.0;
}

#ifdef PID_CONTROLLER_TUNING_AID_ADDRESS
/**
 * Live streams the internal PID controller state via UDP for visualization/tuning.
 *
 * This function is intended for debugging and tuning purposes only.
 * It sends a JSON payload containing P, I, D terms, input, output, and target values.
 *
 * To enable this feature:
 * 1. Define PID_CONTROLLER_TUNING_AID_ADDRESS at the top of this file.
 * 2. Update the IP address in the implementation to match your receiving
 *    machine running PlotJuggler with UDP server.
 */
void cPidController::SendTuningAidData(double pTerm, double iTerm, double dTerm, double input, double output, double targetValue)
{
	static int sock = -1;
	static struct sockaddr_in dest_addr;

	// One-time setup check
	if (sock < 0) {
		sock = socket(AF_INET, SOCK_DGRAM, 0);

		memset(&dest_addr, 0, sizeof(dest_addr));
		dest_addr.sin_family = AF_INET;
		dest_addr.sin_port = htons(9870); // PlotJuggler port
		inet_pton(AF_INET, PID_CONTROLLER_TUNING_AID_ADDRESS, &dest_addr.sin_addr); // replace with your PC's IP
	}

	// Actual Payload
	char payload[512];
	int len = snprintf(payload, sizeof(payload),
		"{\"bufferFillLevelMs\":%g,\"targetBufferFillLevelMs\":%g,\"pTerm\":%g,\"iTerm\":%g,\"dTerm\":%g,\"outputPpm\":%g}",
		input, targetValue, pTerm, iTerm, dTerm, output);

	// Send (Non-blocking usually, very fast)
	sendto(sock, payload, len, 0, (struct sockaddr*)&dest_addr, sizeof(dest_addr));
}
#endif