Skip to content

Commit

Permalink
Merge pull request #4875 from iNavFlight/agh_osd_fix_piff_display
Browse files Browse the repository at this point in the history 
[OSD]: Fix display of PIFF FF term
  • Loading branch information
@digitalentity
digitalentity authored Jun 29, 2019
2 parents af93d62 + 1933126 commit 54d4b9e
Show file tree
Hide file tree
Showing 3 changed files with 47 additions and 12 deletions.
18 changes: 18 additions & 0 deletions src/main/flight/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -981,3 +981,21 @@ void FAST_CODE pidController(void)
}
}
}

pidType_e pidIndexGetType(pidIndex_e pidIndex)
{
if (STATE(FIXED_WING)) {
// FW specific
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return PID_TYPE_PIFF;
}
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
return PID_TYPE_NONE;
}
}
// Common
if (pidIndex == PID_SURFACE) {
return PID_TYPE_NONE;
}
return PID_TYPE_PID;
}
9 changes: 9 additions & 0 deletions src/main/flight/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,6 +68,13 @@ typedef enum {
PID_ITEM_COUNT
} pidIndex_e;

// TODO(agh): PIDFF
typedef enum {
PID_TYPE_NONE, // Not used in the current platform/mixer/configuration
PID_TYPE_PID, // Uses P, I and D terms
PID_TYPE_PIFF, // Uses P, I and FF, ignoring D
} pidType_e;

typedef struct pid8_s {
uint8_t P;
uint8_t I;
Expand Down Expand Up @@ -182,3 +189,5 @@ int16_t getHeadingHoldTarget(void);

void autotuneUpdateState(void);
void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput);

pidType_e pidIndexGetType(pidIndex_e pidIndex);
32 changes: 20 additions & 12 deletions src/main/io/osd.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -1189,13 +1189,23 @@ static void osdDisplayBatteryVoltage(uint8_t elemPosX, uint8_t elemPosY, uint16_
displayWriteWithAttr(osdDisplayPort, elemPosX + 1, elemPosY, buff, elemAttr);
}

static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, const pid8_t *pid, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *str, pidIndex_e pidIndex, adjustmentFunction_e adjFuncP, adjustmentFunction_e adjFuncI, adjustmentFunction_e adjFuncD)
{
textAttributes_t elemAttr;
char buff[4];

const pid8_t *pid = &pidBank()->pid[pidIndex];
pidType_e pidType = pidIndexGetType(pidIndex);

displayWrite(osdDisplayPort, elemPosX, elemPosY, str);

if (pidType == PID_TYPE_NONE) {
// PID is not used in this configuration. Draw dashes.
// XXX: Keep this in sync with the %3d format and spacing used below
displayWrite(osdDisplayPort, elemPosX + 6, elemPosY, "- - -");
return;
}

elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->P);
if ((isAdjustmentFunctionSelected(adjFuncP)) || (((adjFuncP == ADJUSTMENT_ROLL_P) || (adjFuncP == ADJUSTMENT_PITCH_P)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_P))))
Expand All @@ -1209,7 +1219,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char *
displayWriteWithAttr(osdDisplayPort, elemPosX + 8, elemPosY, buff, elemAttr);

elemAttr = TEXT_ATTRIBUTES_NONE;
tfp_sprintf(buff, "%3d", pid->D);
tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D);
if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D))))
TEXT_ATTRIBUTES_ADD_BLINK(elemAttr);
displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr);
Expand Down Expand Up @@ -1927,37 +1937,35 @@ static bool osdDrawSingleElement(uint8_t item)
#endif

case OSD_ROLL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "ROL", &pidBank()->pid[PID_ROLL], ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D);
osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D);
return true;

case OSD_PITCH_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PIT", &pidBank()->pid[PID_PITCH], ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D);
osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D);
return true;

case OSD_YAW_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "YAW", &pidBank()->pid[PID_YAW], ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D);
osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D);
return true;

case OSD_LEVEL_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "LEV", &pidBank()->pid[PID_LEVEL], ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
osdDisplayPIDValues(elemPosX, elemPosY, "LEV", PID_LEVEL, ADJUSTMENT_LEVEL_P, ADJUSTMENT_LEVEL_I, ADJUSTMENT_LEVEL_D);
return true;

case OSD_POS_XY_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PXY", &pidBank()->pid[PID_POS_XY], ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
osdDisplayPIDValues(elemPosX, elemPosY, "PXY", PID_POS_XY, ADJUSTMENT_POS_XY_P, ADJUSTMENT_POS_XY_I, ADJUSTMENT_POS_XY_D);
return true;

case OSD_POS_Z_PIDS:
osdDisplayPIDValues(elemPosX, elemPosY, "PZ", &pidBank()->pid[PID_POS_Z], ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
osdDisplayPIDValues(elemPosX, elemPosY, "PZ", PID_POS_Z, ADJUSTMENT_POS_Z_P, ADJUSTMENT_POS_Z_I, ADJUSTMENT_POS_Z_D);
return true;

case OSD_VEL_XY_PIDS:
if (!STATE(FIXED_WING))
osdDisplayPIDValues(elemPosX, elemPosY, "VXY", &pidBank()->pid[PID_VEL_XY], ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
osdDisplayPIDValues(elemPosX, elemPosY, "VXY", PID_VEL_XY, ADJUSTMENT_VEL_XY_P, ADJUSTMENT_VEL_XY_I, ADJUSTMENT_VEL_XY_D);
return true;

case OSD_VEL_Z_PIDS:
if (!STATE(FIXED_WING))
osdDisplayPIDValues(elemPosX, elemPosY, "VZ", &pidBank()->pid[PID_VEL_Z], ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
osdDisplayPIDValues(elemPosX, elemPosY, "VZ", PID_VEL_Z, ADJUSTMENT_VEL_Z_P, ADJUSTMENT_VEL_Z_I, ADJUSTMENT_VEL_Z_D);
return true;

case OSD_HEADING_P:
Expand Down

0 comments on commit 54d4b9e

Please sign in to comment.