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

Nav PID logging fixes

Browse source
This commit is contained in:
Michel Pastor 2018-06-19 15:26:03 +02:00
parent eb0366c109
commit 7545ae57fa
2 changed files with 5 additions and 4 deletions
Download patch file Download diff file Expand all files Collapse all files

6
src/main/blackbox/blackbox.c
View file

@ -866,7 +866,7 @@ static void writeInterframe(void)
arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT); arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
for (int i = 0; i < 3; i++) { for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT); arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
} }
@ -1247,7 +1247,7 @@ static void loadMainState(timeUs_t currentTimeUs)
#endif #endif
#ifdef USE_NAV #ifdef USE_NAV
if (!STATE(FIXED_WING)) { if (!STATE(FIXED_WING)) {
blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained); blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained / 10);
blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional / 10); blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional / 10);
blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral / 10); blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral / 10);
blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative / 10); blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative / 10);
@ -1260,7 +1260,7 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional / 10); blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional / 10);
blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral / 10); blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral / 10);
blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative / 10); blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative / 10);
blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10); blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_alt.output_constrained / 10);
blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10); blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10);
blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10); blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10);

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

@ -1645,8 +1645,9 @@ void navPidReset(pidController_t *pid)
{ {
pid->reset = true; pid->reset = true;
pid->proportional = 0.0f; pid->proportional = 0.0f;
pid->integrator = 0.0f; pid->integral = 0.0f;
pid->derivative = 0.0f; pid->derivative = 0.0f;
pid->integrator = 0.0f;
pid->last_input = 0.0f; pid->last_input = 0.0f;
pid->dterm_filter_state.state = 0.0f; pid->dterm_filter_state.state = 0.0f;
pid->dterm_filter_state.RC = 0.0f; pid->dterm_filter_state.RC = 0.0f;