AnglePID.cpp

#include <iostream>
#include <cstdio>
#include <algorithm>
#include <ctime>

int sample_time, current_time, last_time = 0.0f;
float Pitch_PTerm, Pitch_DTerm, Pitch_ITerm = 0.0f;
float Yaw_PTerm, Yaw_DTerm, Yaw_ITerm = 0.0f;float error, last_error, int_error = 0.0f;
float windup_guard = 3.6f; //max value of Integral part of PID
float SetPoint = 0.0f;

using namespace std;

int yawPID(float currentAngle, float targetAngle, float P, float I, float D) {
        SetPoint = targetAngle;
        error = std::min(SetPoint - currentAngle, 360.0f - (SetPoint - currentAngle));
        current_time = millis();
        float delta_time = current_time - last_time;
        float delta_error = error - last_error;
        if (delta_time >= sample_time) {
                Yaw_PTerm = P * error;
                Yaw_ITerm += error * delta_time;
        }
        if (Yaw_ITerm < -windup_guard) {
                Yaw_ITerm = -windup_guard;
        } else if (Yaw_ITerm > windup_guard) {
                Yaw_ITerm = windup_guard;
        }
        Yaw_DTerm = 0.0f;
        if (delta_time > 0.0f) {
                Yaw_DTerm = delta_error / delta_time;
        }
        last_time = current_time;
        last_error = error;
        int output = (int)(Yaw_PTerm + (I * Yaw_ITerm) + (D * Yaw_DTerm));
        return output;
}

int pitchPID(float currentAngle, float targetAngle, float P, float I, float D) {
        SetPoint = targetAngle;
        error = std::min(SetPoint - currentAngle, 360.0f - (SetPoint - currentAngle));
        current_time = millis();
        float delta_time = current_time - last_time;
        float delta_error = error - last_error;
        if (delta_time >= sample_time) {
                Pitch_PTerm = P * error;
                Pitch_ITerm += error * delta_time;
        }
        if (Pitch_ITerm < -windup_guard) {
                Pitch_ITerm = -windup_guard;
        } else if (Pitch_ITerm > windup_guard) {
                Pitch_ITerm = windup_guard;
        }
        Pitch_DTerm = 0.0f;
        if (delta_time > 0.0f) {
                Pitch_DTerm = delta_error / delta_time;
        }
        last_time = current_time;
        last_error = error;
        int output = (int)(Pitch_PTerm + (I * Pitch_ITerm) + (D * Pitch_DTerm));
        return output;
}


//set interval for PID
void setSampleTime(float newSampleTime) {
        sample_time = newSampleTime;
}