From d837b5087d3f88aeb33bb56639a95a5ded8e9c12 Mon Sep 17 00:00:00 2001 From: ctzsnooze Date: Fri, 29 Nov 2024 21:51:16 +1100 Subject: [PATCH] revert more compact initialisation code due to SITL error otherwise --- src/main/flight/autopilot.c | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/src/main/flight/autopilot.c b/src/main/flight/autopilot.c index 6d729d27e4..c15955f010 100644 --- a/src/main/flight/autopilot.c +++ b/src/main/flight/autopilot.c @@ -148,7 +148,14 @@ void autopilotInit(void) positionPidCoeffs.Kd = cfg->position_D * POSITION_D_SCALE; positionPidCoeffs.Kf = cfg->position_A * POSITION_A_SCALE; // Kf used for acceleration // initialise filters with approximate filter gains; location isn't used at this point. - resetPositionControl(&gpsSol.llh, 100); + ap.upsampleLpfGain = pt3FilterGain(UPSAMPLING_CUTOFF_HZ, 0.01f); // 5Hz, assuming 100Hz task rate at init + resetUpsampleFilters(); + // Initialise PT1 filters for deivative and acceleration in earth frame axes + ap.vaLpfCutoff = cfg->position_cutoff * 0.01f; + const float vaGain = pt1FilterGain(ap.vaLpfCutoff, 0.1f); // assume 10Hz GPS connection at start; value is overwritten before first filter use + for (unsigned i = 0; i < ARRAYLEN(ap.efAxis); i++) { + resetEFAxisFilters(&ap.efAxis[i], vaGain); + } } void resetAltitudeControl (void) {