Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 16:25:26 +03:00
Code Issues Wiki Activity

Add navigation PID controllers logging for FW and MR

Browse source
This commit is contained in:
Michel Pastor 2018-06-18 23:24:13 +02:00
parent 5156038d75
commit 1c47fb6051
6 changed files with 201 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

12
src/main/navigation/navigation.c
View file

@ -1611,6 +1611,11 @@ float navPidApply3(pidController_t *pid, const float setpoint, const float measu
const float outVal = newProportional + (pid->integrator * gainScaler) + newDerivative;
const float outValConstrained = constrainf(outVal, outMin, outMax);
pid->proportional = newProportional;
pid->integral = pid->integrator;
pid->derivative = newDerivative;
pid->output_constrained = outValConstrained;
/* Update I-term */
if (!(pidFlags & PID_ZERO_INTEGRATOR)) {
const float newIntegrator = pid->integrator + (error * pid->param.kI * gainScaler * dt) + ((outValConstrained - outVal) * pid->param.kT * dt);
@ -1638,10 +1643,13 @@ float navPidApply2(pidController_t *pid, const float setpoint, const float measu
void navPidReset(pidController_t *pid)
{
pid->reset = true;
pid->proportional = 0.0f;
pid->integrator = 0.0f;
pid->derivative = 0.0f;
pid->last_input = 0.0f;
pid->dterm_filter_state.state = 0.0f;
pid->dterm_filter_state.RC = 0.0f;
pid->output_constrained = 0.0f;
}
void navPidInit(pidController_t *pid, float _kP, float _kI, float _kD)
@ -3059,6 +3067,10 @@ float calculateAverageSpeed() {
return (float)getTotalTravelDistance() / (getFlightTime() * 100);
}
const navigationPIDControllers_t* getNavigationPIDControllers(void) {
return &posControl.pids;
}
#else // NAV
#ifdef USE_GPS