Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +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

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

@ -193,6 +193,34 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = {
{"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)},
{"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)},
{"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)},
{"fwAltP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwAltI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwAltD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwAltOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwPosP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwPosI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwPosD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"fwPosOut", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)},
{"mcPosAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcPosAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcPosAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisP", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisP", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisP", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisI", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisI", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisI", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisOut",0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisOut",1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcVelAxisOut",2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcSurfaceP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcSurfaceI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcSurfaceD", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
{"mcSurfaceOut",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(MC_NAV)},
/* rcData are encoded together as a group: */
{"rcData", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
{"rcData", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)},
@ -337,6 +365,18 @@ typedef struct blackboxMainState_s {
int32_t axisPID_D[XYZ_AXIS_COUNT];
int32_t axisPID_Setpoint[XYZ_AXIS_COUNT];
int32_t mcPosAxisP[XYZ_AXIS_COUNT];
int32_t mcVelAxisPID[3][XYZ_AXIS_COUNT];
int32_t mcVelAxisOutput[XYZ_AXIS_COUNT];
int32_t mcSurfacePID[3];
int32_t mcSurfacePIDOutput;
int32_t fwAltPID[3];
int32_t fwAltPIDOutput;
int32_t fwPosPID[3];
int32_t fwPosPIDOutput;
int16_t rcData[4];
int16_t rcCommand[4];
int16_t gyroADC[XYZ_AXIS_COUNT];
@ -515,6 +555,20 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV:
#ifdef USE_NAV
return STATE(FIXED_WING);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_MC_NAV:
#ifdef USE_NAV
return !STATE(FIXED_WING);
#else
return false;
#endif
case FLIGHT_LOG_FIELD_CONDITION_RSSI:
return rxConfig()->rssi_channel > 0 || feature(FEATURE_RSSI_ADC);
@ -601,6 +655,27 @@ static void writeIntraframe(void)
}
}
if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3);
blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput);
blackboxWriteSignedVBArray(blackboxCurrent->fwPosPID, 3);
blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput);
}
if (testBlackboxCondition(CONDITION(MC_NAV))) {
blackboxWriteSignedVBArray(blackboxCurrent->mcPosAxisP, XYZ_AXIS_COUNT);
for (int i = 0; i < 3; i++) {
blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisPID[i], XYZ_AXIS_COUNT);
}
blackboxWriteSignedVBArray(blackboxCurrent->mcVelAxisOutput, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(blackboxCurrent->mcSurfacePID, 3);
blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput);
}
// Write raw stick positions
blackboxWriteSigned16VBArray(blackboxCurrent->rcData, 4);
@ -773,6 +848,38 @@ static void writeInterframe(void)
}
}
if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) {
arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3);
blackboxWriteSignedVBArray(deltas, 3);
blackboxWriteSignedVB(blackboxCurrent->fwAltPIDOutput);
arraySubInt32(deltas, blackboxCurrent->fwPosPID, blackboxLast->fwPosPID, 3);
blackboxWriteSignedVBArray(deltas, 3);
blackboxWriteSignedVB(blackboxCurrent->fwPosPIDOutput);
}
if (testBlackboxCondition(CONDITION(MC_NAV))) {
arraySubInt32(deltas, blackboxCurrent->mcPosAxisP, blackboxLast->mcPosAxisP, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
for (int i = 0; i < 3; i++) {
arraySubInt32(deltas, blackboxCurrent->mcVelAxisPID[i], blackboxLast->mcVelAxisPID[i], XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
}
arraySubInt32(deltas, blackboxCurrent->mcVelAxisOutput, blackboxLast->mcVelAxisOutput, XYZ_AXIS_COUNT);
blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT);
arraySubInt32(deltas, blackboxCurrent->mcSurfacePID, blackboxLast->mcSurfacePID, 3);
blackboxWriteSignedVBArray(deltas, 3);
blackboxWriteSignedVB(blackboxCurrent->mcSurfacePIDOutput);
}
/*
* RC tends to stay the same or fairly small for many frames at a time, so use an encoding that
* can pack multiple values per byte:
@ -1124,6 +1231,10 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->time = currentTimeUs;
#ifdef USE_NAV
const navigationPIDControllers_t *nav_pids = getNavigationPIDControllers();
#endif
for (int i = 0; i < XYZ_AXIS_COUNT; i++) {
blackboxCurrent->axisPID_Setpoint[i] = axisPID_Setpoint[i];
blackboxCurrent->axisPID_P[i] = axisPID_P[i];
@ -1133,9 +1244,36 @@ static void loadMainState(timeUs_t currentTimeUs)
blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G);
#ifdef USE_MAG
blackboxCurrent->magADC[i] = mag.magADC[i];
#endif
#ifdef USE_NAV
if (!STATE(FIXED_WING)) {
blackboxCurrent->mcPosAxisP[i] = lrintf(nav_pids->pos[i].output_constrained);
blackboxCurrent->mcVelAxisPID[0][i] = lrintf(nav_pids->vel[i].proportional / 10);
blackboxCurrent->mcVelAxisPID[1][i] = lrintf(nav_pids->vel[i].integral / 10);
blackboxCurrent->mcVelAxisPID[2][i] = lrintf(nav_pids->vel[i].derivative / 10);
}
#endif
}
#ifdef USE_NAV
if (STATE(FIXED_WING)) {
blackboxCurrent->fwAltPID[0] = lrintf(nav_pids->fw_alt.proportional / 10);
blackboxCurrent->fwAltPID[1] = lrintf(nav_pids->fw_alt.integral / 10);
blackboxCurrent->fwAltPID[2] = lrintf(nav_pids->fw_alt.derivative / 10);
blackboxCurrent->fwAltPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10);
blackboxCurrent->fwPosPID[0] = lrintf(nav_pids->fw_nav.proportional / 10);
blackboxCurrent->fwPosPID[1] = lrintf(nav_pids->fw_nav.integral / 10);
blackboxCurrent->fwPosPID[2] = lrintf(nav_pids->fw_nav.derivative / 10);
blackboxCurrent->fwPosPIDOutput = lrintf(nav_pids->fw_nav.output_constrained / 10);
} else {
blackboxCurrent->mcSurfacePID[0] = lrintf(nav_pids->surface.proportional / 10);
blackboxCurrent->mcSurfacePID[1] = lrintf(nav_pids->surface.integral / 10);
blackboxCurrent->mcSurfacePID[2] = lrintf(nav_pids->surface.derivative / 10);
blackboxCurrent->mcSurfacePIDOutput = lrintf(nav_pids->surface.output_constrained / 10);
}
#endif
for (int i = 0; i < 4; i++) {
blackboxCurrent->rcData[i] = rcData[i];
blackboxCurrent->rcCommand[i] = rcCommand[i];

2
src/main/blackbox/blackbox_fielddefs.h
View file

@ -40,6 +40,8 @@ typedef enum FlightLogFieldCondition {
FLIGHT_LOG_FIELD_CONDITION_VBAT,
FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
FLIGHT_LOG_FIELD_CONDITION_SURFACE,
FLIGHT_LOG_FIELD_CONDITION_FIXED_WING_NAV,
FLIGHT_LOG_FIELD_CONDITION_MC_NAV,
FLIGHT_LOG_FIELD_CONDITION_RSSI,
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,

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

44
src/main/navigation/navigation.h
View file

@ -17,6 +17,8 @@
#pragma once
#include "common/axis.h"
#include "common/filter.h"
#include "common/maths.h"
#include "common/vector.h"
@ -207,6 +209,46 @@ typedef struct {
int32_t yaw; // deg * 100
} navWaypointPosition_t;
typedef struct {
float kP;
float kI;
float kD;
float kT; // Tracking gain (anti-windup)
} pidControllerParam_t;
typedef struct {
float kP;
} pControllerParam_t;
typedef struct {
bool reset;
pidControllerParam_t param;
pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
float integrator; // integrator value
float last_input; // last input for derivative
float integral; // used integral value in output
float proportional; // used proportional value in output
float derivative; // used derivative value in output
float output_constrained; // controller output constrained
} pidController_t;
typedef struct {
pControllerParam_t param;
float output_constrained;
} pController_t;
typedef struct navigationPIDControllers_s {
/* Multicopter PIDs */
pController_t pos[XYZ_AXIS_COUNT];
pidController_t vel[XYZ_AXIS_COUNT];
pidController_t surface;
/* Fixed-wing PIDs */
pidController_t fw_alt;
pidController_t fw_nav;
} navigationPIDControllers_t;
/* MultiWii-compatible params for telemetry */
typedef enum {
MW_GPS_MODE_NONE = 0,
@ -346,6 +388,8 @@ bool isFixedWingLaunchDetected(void);
float calculateAverageSpeed();
const navigationPIDControllers_t* getNavigationPIDControllers(void);
/* Returns the heading recorded when home position was acquired.
* Note that the navigation system uses deg*100 as unit and angles
* are in the [0, 360 * 100) interval.

5
src/main/navigation/navigation_multicopter.c
View file

@ -72,6 +72,8 @@ static void updateAltitudeVelocityController_MC(timeDelta_t deltaMicros)
targetVel = constrainf(targetVel, -navConfig()->general.max_auto_climb_rate, navConfig()->general.max_auto_climb_rate);
}
posControl.pids.pos[Z].output_constrained = targetVel;
// limit max vertical acceleration to 1/5G (~200 cm/s/s) if we are increasing velocity.
// if we are decelerating - don't limit (allow better recovery from falling)
if (ABS(targetVel) > ABS(posControl.desiredState.vel.z)) {
@ -352,6 +354,9 @@ static void updatePositionVelocityController_MC(void)
newVelTotal = maxSpeed;
}
posControl.pids.pos[X].output_constrained = newVelX;
posControl.pids.pos[Y].output_constrained = newVelY;
// Apply expo & attenuation if heading in wrong direction - turn first, accelerate later (effective only in WP mode)
const float velHeadFactor = getVelocityHeadingAttenuationFactor();
const float velExpoFactor = getVelocityExpoAttenuationFactor(newVelTotal, maxSpeed);

34
src/main/navigation/navigation_private.h
View file

@ -92,46 +92,12 @@ typedef struct navigationFlags_s {
bool forcedRTHActivated;
} navigationFlags_t;
typedef struct {
float kP;
float kI;
float kD;
float kT; // Tracking gain (anti-windup)
} pidControllerParam_t;
typedef struct {
float kP;
} pControllerParam_t;
typedef enum {
PID_DTERM_FROM_ERROR = 1 << 0,
PID_ZERO_INTEGRATOR = 1 << 1,
PID_SHRINK_INTEGRATOR = 1 << 2,
} pidControllerFlags_e;
typedef struct {
bool reset;
pidControllerParam_t param;
pt1Filter_t dterm_filter_state; // last derivative for low-pass filter
float integrator; // integrator value
float last_input; // last input for derivative
} pidController_t;
typedef struct {
pControllerParam_t param;
} pController_t;
typedef struct navigationPIDControllers_s {
/* Multicopter PIDs */
pController_t pos[XYZ_AXIS_COUNT];
pidController_t vel[XYZ_AXIS_COUNT];
pidController_t surface;
/* Fixed-wing PIDs */
pidController_t fw_alt;
pidController_t fw_nav;
} navigationPIDControllers_t;
typedef struct {
fpVector3_t pos;
fpVector3_t vel;