Good day, and sorry for my English I don’t know. I have a question which file in the source is responsible for the PID of the rudder.
I downloaded the source from the link below for APM2.x, I know it is no longer supported.
https://ardupilot.org/dev/docs/building-ardupilot-with-arduino-windows.html
I’m not a programmer either, and I don’t understand much. But in the meantime, I’m clicking on the APM2.x code in Arduino, more for anglers.
I can’t find the function to add the code so it will be in the post.
void AutoPilot_Update() {
if (Pilot_init_flag) {
missionComplete = false; // Zresetuj misję
turnCompleted = true; // Ustaw flagę startu łódki
turnMode = false; // Czy łódka w trybie nawrotu?
turnResetDone = false; // Flaga resetu PID przy nawrocie
cruiseResetDone = false; // Flaga resetu PID po nawrocie
turnStartLat = LAT; // Zapisz miejsce startu
turnStartLng = LNG; // Zapisz miejsce startu
// Resetuj PID
pidSteeringError = 0;
pidSteeringIntegral = 0;
pidSteeringDerivative = 0;
pidSteeringPrevError = 0;
Pilot_init_flag = false; // Wyłącz inicjalizację
}
if (WP >= 11) return; // Walidacja indeksu punktu
float targetLat = LatitudeTAB[WP];
float targetLng = LongitudeTAB[WP];
// Oblicz dystans i kierunek do punktu docelowego
float DistWP = CalculateDistanceToTarget(targetLat, targetLng);
float targetHeading = CalculateTargetHeading(targetLat, targetLng);
// Oblicz dryf boczny
float driftAngle = COG - heading;
if (driftAngle > 180) driftAngle -= 360;
if (driftAngle < -180) driftAngle += 360;
float normalizedDrift = (driftAngle / 180.0f) * 1000; // Normalizacja do [-1000, 1000]
// Oblicz headingError na początku funkcji do sterowania auto
float headingError = targetHeading - heading;
if (headingError > 180) headingError -= 360;
if (headingError < -180) headingError += 360;
float normalizedError = ((headingError / 180.0f)*1000); // Normalizacja do [-1000, 1000]
// Ustaw moc przepustnicy w zależności od odległości
if (DistWP <= Wolny_Stop) {
Moc_Gazu = Stop_Gaz; // Zmniejsz moc przepustnicy do 50%
} else if (turnCompleted && CalculateDistanceFromTurnStart() <= Wolny_Start) {
Moc_Gazu = Start_Gaz; // Połowa mocy (50%) po nawrocie
} else {
Moc_Gazu = Full_Gaz; // Pełna moc (100%)
turnCompleted = false;
}
// Jeśli łódka płynie na wprost (kierunek do celu zbliżony do 0)
if ((DistWP > DISTANCE_THRESHOLD) && (turnMode == false)) {
autopilotThrottleValue = 3000 + ((1000 * Moc_Gazu) / 100); // Wartość 3000 to neutralna (stop), a +/-1000 to maksymalna zmiana
autopilotThrottleValue = constrain(autopilotThrottleValue, 3000, 4000); // Ograniczenie w przedziale [3000, 4000]
}
// Sprawdzenie, czy trzeba zrobić nawrót
if ((abs(headingError) > Turn_K) && (!turnResetDone)) {
autopilotThrottleValue = 3000;//zatrzymaj lodke
turnMode = true; // flaga trybu nawrotu do wyboru pid
turnCompleted = true; // Ustaw flagę nawrotu łódki do regolacji predkosci
turnStartLat = LAT; // Zapisz miejsce nawracania
turnStartLng = LNG; // Zapisz miejsce nawracania
// RESET PID przy wejściu w nawrót (tylko raz)
pidSteeringIntegral = 0;
pidSteeringDerivative = 0;
pidSteeringPrevError = 0;
turnResetDone = true;
cruiseResetDone = false; // Zezwól na reset po nawrocie
}
// Jeśli błąd jest mały, przechodzimy na utrzymanie kursu
if ((turnMode) && (abs(headingError) < Cruise_K) && (!cruiseResetDone)) {
turnMode = false;
// RESET PID po zakończeniu nawrotu (tylko raz)
pidSteeringIntegral = 0;
pidSteeringDerivative = 0;
pidSteeringPrevError = 0;
cruiseResetDone = true;
turnResetDone = false; // Zezwól na kolejny reset przy następnym nawrocie
}
// Wybór PID w zależności od trybu
float KP = turnMode ? TURN_KP : CRUISE_KP;
float KI = turnMode ? TURN_KI : CRUISE_KI;
float KD = turnMode ? TURN_KD : CRUISE_KD;
float FF = turnMode ? T_FF : C_FF;
uint8_t IMAX = turnMode ? T_IMAX : C_IMAX;
uint32_t now = millis(); // Pobierz aktualny czas
float delta_time = (now - last_delta_time) / 1000.0f; // Zamiana na sekundy
last_delta_time = now; // Zapisz czas dla kolejnego wywołania
if (delta_time <= 0) return; // Unikaj dzielenia przez 0
// PID sterowania
// pidSteeringError = headingError; //org pid compas wartosc
pidSteeringError = normalizedError; //test znormalizowane pid zakres -1000 do 1000
pidSteeringIntegral += pidSteeringError * delta_time;
pidSteeringIntegral = constrain(pidSteeringIntegral, -IMAX * 49, IMAX * 49);
pidSteeringDerivative = (pidSteeringError - pidSteeringPrevError) / delta_time;
// Oblicz XTE (odległość od trasy)
float XTE = CalculateXTE(targetLat, targetLng, LAT, LNG, turnStartLat, turnStartLng); //funkcja w globalnych
float normalizedXTE = (XTE / 10.0f) * 1000; // Normalizacja XTE do wartości [-1000, 1000] oraz dzielone przez 10 dla unikniecia duzych wartosci
float xteCorrection = normalizedXTE * 0.1; // Korekta XTE w sterowaniu i Skalowanie wpływu XTE na sterowanie
// Korekta dryfu bocznego
float driftCorrection = normalizedDrift * DRIFT_CORRECTION_FACTOR;
float steeringOutput = (KP * pidSteeringError) +
(KI * pidSteeringIntegral) +
(KD * pidSteeringDerivative) +
(FF * headingError);
//(FF * headingError)+ //właczenie dryfu bocznego po testach
// driftCorrection;
// + xteCorrection; // NOWA korekta XTE jeszcze nie uzyta testy wczesniejszych prac
pidSteeringPrevError = pidSteeringError;
// Ograniczenie wartości sterowania
autopilotSteeringValue = constrain(3000 + (steeringOutput * Dop), 2000, 4000);
// Sprawdź, czy cel został osiągnięty
if (DistWP <= DISTANCE_THRESHOLD) {
autopilotThrottleValue = 3000; // Zatrzymaj łódkę
autopilotSteeringValue = 3000; // Zatrzymaj łódkę
missionComplete = true;
return;
}
autoInput = true;
}
I mean this limitation, in ardurover there is 1.0 which means 100% by what value is multiplied. Because when testing -100 and 100 and so up out of nervousness I entered 49 and the PID started to control straight. I can’t find shit in the ardurover code.
pidSteeringIntegral = constrain(pidSteeringIntegral, -IMAX * 49, IMAX * 49);
And after thinking that my assumptions are wrong like. Why scale 180 degrees from the compass to a range of 1000 i.e. 500um only e.g. 45 degrees to 500um…
What is your actual limitation IMAX here your code…
// Compute integral component if time has elapsed
if ((fabsf(_ki) > 0) && (dt > 0)) {
_integrator += (error * _ki) * scaler * delta_time;
if (_integrator < -_imax) {
_integrator = -_imax;
} else if (_integrator > _imax) {
_integrator = _imax;
}
output += _integrator;
}
return output;
}