Skip to content

Commit

Permalink
Use the cached value of useDshotTelemetry to ensure consistent runtim…
Browse files Browse the repository at this point in the history
…e use if dshot_bidir is changed (betaflight#13589)
  • Loading branch information
SteveCEvans authored Apr 27, 2024
1 parent 5437794 commit 565de1b
Show file tree
Hide file tree
Showing 11 changed files with 19 additions and 20 deletions.
4 changes: 2 additions & 2 deletions src/main/blackbox/blackbox.c
Original file line number Diff line number Diff line change
Expand Up @@ -469,7 +469,7 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
case CONDITION(MOTOR_6_HAS_RPM):
case CONDITION(MOTOR_7_HAS_RPM):
case CONDITION(MOTOR_8_HAS_RPM):
return (getMotorCount() >= condition - CONDITION(MOTOR_1_HAS_RPM) + 1) && (motorConfig()->dev.useDshotTelemetry) && isFieldEnabled(FIELD_SELECT(RPM));
return (getMotorCount() >= condition - CONDITION(MOTOR_1_HAS_RPM) + 1) && useDshotTelemetry && isFieldEnabled(FIELD_SELECT(RPM));
#endif

case CONDITION(TRICOPTER):
Expand Down Expand Up @@ -1524,7 +1524,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DYN_NOTCH_MIN_HZ, "%d", dynNotchConfig()->dyn_notch_min_hz);
#endif
#ifdef USE_DSHOT_TELEMETRY
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d", motorConfig()->dev.useDshotTelemetry);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_DSHOT_BIDIR, "%d", useDshotTelemetry);
BLACKBOX_PRINT_HEADER_LINE(PARAM_NAME_MOTOR_POLES, "%d", motorConfig()->motorPoleCount);
#endif
#ifdef USE_RPM_FILTER
Expand Down
2 changes: 1 addition & 1 deletion src/main/drivers/dshot.c
Original file line number Diff line number Diff line change
Expand Up @@ -291,7 +291,7 @@ static void dshotUpdateTelemetryData(uint8_t motorIndex, dshotTelemetryType_t ty

FAST_CODE_NOINLINE void updateDshotTelemetry(void)
{
if (!motorConfig()->dev.useDshotTelemetry) {
if (!useDshotTelemetry) {
return;
}

Expand Down
2 changes: 1 addition & 1 deletion src/main/drivers/dshot.h
Original file line number Diff line number Diff line change
Expand Up @@ -89,9 +89,9 @@ float dshotConvertFromExternal(uint16_t externalValue);
uint16_t dshotConvertToExternal(float motorValue);

uint16_t prepareDshotPacket(dshotProtocolControl_t *pcb);
extern bool useDshotTelemetry;

#ifdef USE_DSHOT_TELEMETRY
extern bool useDshotTelemetry;

typedef struct dshotTelemetryMotorState_s {
uint16_t rawValue;
Expand Down
4 changes: 2 additions & 2 deletions src/main/drivers/motor.c
Original file line number Diff line number Diff line change
Expand Up @@ -63,7 +63,7 @@ void motorWriteAll(float *values)
#ifdef USE_PWM_OUTPUT
if (motorDevice->enabled) {
#ifdef USE_DSHOT_BITBANG
if (isDshotBitbangActive(&motorConfig()->dev)) {
if (useDshotTelemetry && isDshotBitbangActive(&motorConfig()->dev)) {
// Initialise the output buffers
if (motorDevice->vTable.updateInit) {
motorDevice->vTable.updateInit();
Expand Down Expand Up @@ -297,7 +297,7 @@ bool isMotorProtocolDshot(void)

bool isMotorProtocolBidirDshot(void)
{
return isMotorProtocolDshot() && motorConfig()->dev.useDshotTelemetry;
return isMotorProtocolDshot() && useDshotTelemetry;
}

void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount)
Expand Down
6 changes: 3 additions & 3 deletions src/main/fc/core.c
Original file line number Diff line number Diff line change
Expand Up @@ -363,15 +363,15 @@ void updateArmingStatus(void)

#ifdef USE_DSHOT_TELEMETRY
// If Dshot Telemetry is enabled and any motor isn't providing telemetry, then disable arming
if (motorConfig()->dev.useDshotTelemetry && !isDshotTelemetryActive()) {
if (useDshotTelemetry && !isDshotTelemetryActive()) {
setArmingDisabled(ARMING_DISABLED_DSHOT_TELEM);
} else {
unsetArmingDisabled(ARMING_DISABLED_DSHOT_TELEM);
}
#endif

#ifdef USE_DSHOT_BITBANG
if (isDshotBitbangActive(&motorConfig()->dev) && dshotBitbangGetStatus() != DSHOT_BITBANG_STATUS_OK) {
if (useDshotTelemetry && isDshotBitbangActive(&motorConfig()->dev) && dshotBitbangGetStatus() != DSHOT_BITBANG_STATUS_OK) {
setArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG);
} else {
unsetArmingDisabled(ARMING_DISABLED_DSHOT_BITBANG);
Expand Down Expand Up @@ -514,7 +514,7 @@ void tryArm(void)
if (isMotorProtocolDshot()) {
#if defined(USE_ESC_SENSOR) && defined(USE_DSHOT_TELEMETRY)
// Try to activate extended DSHOT telemetry only if no esc sensor exists and dshot telemetry is active
if (!featureIsEnabled(FEATURE_ESC_SENSOR) && motorConfig()->dev.useDshotTelemetry) {
if (!featureIsEnabled(FEATURE_ESC_SENSOR) && useDshotTelemetry) {
dshotCleanTelemetryData();
if (motorConfig()->dev.useDshotEdt) {
dshotCommandWrite(ALL_MOTORS, getMotorCount(), DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE, DSHOT_CMD_TYPE_INLINE);
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/mixer.c
Original file line number Diff line number Diff line change
Expand Up @@ -680,7 +680,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs)
#endif

#ifdef USE_RPM_LIMIT
if (RPM_LIMIT_ACTIVE && motorConfig()->dev.useDshotTelemetry && ARMING_FLAG(ARMED)) {
if (RPM_LIMIT_ACTIVE && useDshotTelemetry && ARMING_FLAG(ARMED)) {
applyRpmLimiter(&mixerRuntime);
}
#endif
Expand Down
2 changes: 1 addition & 1 deletion src/main/flight/mixer_init.c
Original file line number Diff line number Diff line change
Expand Up @@ -328,7 +328,7 @@ void initEscEndpoints(void)
void mixerInitProfile(void)
{
#ifdef USE_DYN_IDLE
if (motorConfigMutable()->dev.useDshotTelemetry) {
if (useDshotTelemetry) {
mixerRuntime.dynIdleMinRps = currentPidProfile->dyn_idle_min_rpm * 100.0f / 60.0f;
} else {
mixerRuntime.dynIdleMinRps = 0.0f;
Expand Down
5 changes: 2 additions & 3 deletions src/main/flight/rpm_filter.c
Original file line number Diff line number Diff line change
Expand Up @@ -68,15 +68,14 @@ FAST_DATA_ZERO_INIT static int notchUpdatesPerIteration;
FAST_DATA_ZERO_INIT static int motorIndex;
FAST_DATA_ZERO_INIT static int harmonicIndex;


void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)
{
motorIndex = 0;
harmonicIndex = 0;
rpmFilter.numHarmonics = 0; // disable RPM Filtering

// if bidirectional DShot is not available
if (!motorConfig()->dev.useDshotTelemetry) {
if (!useDshotTelemetry) {
return;
}

Expand Down Expand Up @@ -112,7 +111,7 @@ void rpmFilterInit(const rpmFilterConfig_t *config, const timeUs_t looptimeUs)

FAST_CODE_NOINLINE void rpmFilterUpdate(void)
{
if (!motorConfig()->dev.useDshotTelemetry) {
if (!useDshotTelemetry) {
return;
}

Expand Down
6 changes: 3 additions & 3 deletions src/main/msp/msp.c
Original file line number Diff line number Diff line change
Expand Up @@ -1218,7 +1218,7 @@ case MSP_NAME:
bool rpmDataAvailable = false;

#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
if (useDshotTelemetry) {
rpm = lrintf(getDshotRpm(i));
rpmDataAvailable = true;
invalidPct = 10000; // 100.00%
Expand Down Expand Up @@ -1448,7 +1448,7 @@ case MSP_NAME:
sbufWriteU8(dst, getMotorCount());
sbufWriteU8(dst, motorConfig()->motorPoleCount);
#ifdef USE_DSHOT_TELEMETRY
sbufWriteU8(dst, motorConfig()->dev.useDshotTelemetry);
sbufWriteU8(dst, useDshotTelemetry);
#else
sbufWriteU8(dst, 0);
#endif
Expand Down Expand Up @@ -1479,7 +1479,7 @@ case MSP_NAME:
} else
#endif
#if defined(USE_DSHOT_TELEMETRY)
if (motorConfig()->dev.useDshotTelemetry) {
if (useDshotTelemetry) {
sbufWriteU8(dst, getMotorCount());
for (int i = 0; i < getMotorCount(); i++) {
sbufWriteU8(dst, dshotTelemetryState.motorState[i].telemetryData[DSHOT_TELEMETRY_TYPE_TEMPERATURE]);
Expand Down
2 changes: 1 addition & 1 deletion src/main/osd/osd.c
Original file line number Diff line number Diff line change
Expand Up @@ -608,7 +608,7 @@ static int32_t getAverageEscRpm(void)
}
#endif
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
if (useDshotTelemetry) {
return lrintf(getDshotRpmAverage());
}
#endif
Expand Down
4 changes: 2 additions & 2 deletions src/main/osd/osd_elements.c
Original file line number Diff line number Diff line change
Expand Up @@ -267,7 +267,7 @@ typedef int (*getEscRpmOrFreqFnPtr)(int i);
static int getEscRpm(int i)
{
#ifdef USE_DSHOT_TELEMETRY
if (motorConfig()->dev.useDshotTelemetry) {
if (useDshotTelemetry) {
return lrintf(getDshotRpm(i));
}
#endif
Expand Down Expand Up @@ -2064,7 +2064,7 @@ void osdAddActiveElements(void)
#endif // GPS

#if defined(USE_DSHOT_TELEMETRY) || defined(USE_ESC_SENSOR)
if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || (motorConfig()->dev.useDshotTelemetry)) {
if ((featureIsEnabled(FEATURE_ESC_SENSOR)) || useDshotTelemetry) {
osdAddActiveElement(OSD_ESC_TMP);
osdAddActiveElement(OSD_ESC_RPM);
osdAddActiveElement(OSD_ESC_RPM_FREQ);
Expand Down

0 comments on commit 565de1b

Please sign in to comment.