1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00

Refactor Rx code and better support 25Hz links (#13435)

* RX task update rate to 22Hz, to improve 25Hz link stability

* modified Rx code

* add LQ to debug

* use max of frameAge or FrameDelta

* Require a dropouit of 200ms, not 100ms, before RXLOSS

* remove FrameAge, use 150ms timeout 50ms checks

* fix tests and tidy up the comments

* Handle NULL input as before, log frame time

* possible solutions to review comment about names

* Remove rxFrameTimeUs

- prepare for direct use of lastRcFrameTimeUs
- refactor rx_spi callback

* remove global currentRxRateHz

* simplify updateRcRefreshRate

* re-name to recheck interval, return frame time debug

* Calculate RxRate only once

* use rxReceivingSignal for consistency

* use signalReceived not rxDataReceived for consistency

* suggestions from review PL

* move defines, thanks K

* fixes from review, thanks MH

* review changes, undo task interval change

rename bool rxIsReceivingSignal to isRxReceivingSignal
thanks Steve for resolving that tasks changes are not needed
thanks PL for your feedback also

---------

Co-authored-by: Petr Ledvina <ledvinap@hp124.ekotip.cz>
This commit is contained in:
ctzsnooze 2024-10-01 09:23:24 +10:00 committed by GitHub
parent a37bd7c974
commit 776e8c7b3c
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
29 changed files with 108 additions and 116 deletions

View file

@ -905,7 +905,7 @@ static void loadSlowState(blackboxSlowState_t *slow)
memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags; memcpy(&slow->flightModeFlags, &rcModeActivationMask, sizeof(slow->flightModeFlags)); //was flightModeFlags;
slow->stateFlags = stateFlags; slow->stateFlags = stateFlags;
slow->failsafePhase = failsafePhase(); slow->failsafePhase = failsafePhase();
slow->rxSignalReceived = rxIsReceivingSignal(); slow->rxSignalReceived = isRxReceivingSignal();
slow->rxFlightChannelsValid = rxAreFlightChannelsValid(); slow->rxFlightChannelsValid = rxAreFlightChannelsValid();
} }

View file

@ -4770,10 +4770,9 @@ if (buildKey) {
// Run status // Run status
const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO))); const int gyroRate = getTaskDeltaTimeUs(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_GYRO)));
int rxRate = getCurrentRxIntervalUs();
if (rxRate != 0) { int rxRate = getRxRateValid() ? getCurrentRxRateHz() : 0;
rxRate = (int)(1000000.0f / ((float)rxRate));
}
const int systemRate = getTaskDeltaTimeUs(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_SYSTEM))); const int systemRate = getTaskDeltaTimeUs(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTimeUs(TASK_SYSTEM)));
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d", cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE), getTaskDeltaTimeUs(TASK_GYRO), gyroRate, rxRate, systemRate); constrain(getAverageSystemLoadPercent(), 0, LOAD_PERCENTAGE_ONE), getTaskDeltaTimeUs(TASK_GYRO), gyroRate, rxRate, systemRate);
@ -4960,12 +4959,11 @@ static void cliRcSmoothing(const char *cmdName, char *cmdline)
if (rxConfig()->rc_smoothing_mode) { if (rxConfig()->rc_smoothing_mode) {
cliPrintLine("FILTER"); cliPrintLine("FILTER");
if (rcSmoothingAutoCalculate()) { if (rcSmoothingAutoCalculate()) {
const uint16_t smoothedRxRateHz = lrintf(rcSmoothingData->smoothedRxRateHz);
cliPrint("# Detected Rx frequency: "); cliPrint("# Detected Rx frequency: ");
if (getCurrentRxIntervalUs() == 0) { if (getRxRateValid()) {
cliPrintLine("NO SIGNAL"); cliPrintLinef("%dHz", lrintf(rcSmoothingData->smoothedRxRateHz));
} else { } else {
cliPrintLinef("%dHz", smoothedRxRateHz); cliPrintLine("NO SIGNAL");
} }
} }
cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency); cliPrintf("# Active setpoint cutoff: %dhz ", rcSmoothingData->setpointCutoffFrequency);

View file

@ -292,7 +292,7 @@ void updateArmingStatus(void)
// If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault // If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
if (!isUsingSticksForArming()) { if (!isUsingSticksForArming()) {
static bool hadRx = false; static bool hadRx = false;
const bool haveRx = rxIsReceivingSignal(); const bool haveRx = isRxReceivingSignal();
const bool justGotRxBack = !hadRx && haveRx; const bool justGotRxBack = !hadRx && haveRx;
@ -762,7 +762,7 @@ bool processRx(timeUs_t currentTimeUs)
return false; return false;
} }
updateRcRefreshRate(currentTimeUs); updateRcRefreshRate(currentTimeUs, isRxReceivingSignal());
// in 3D mode, we need to be able to disarm by switch at any time // in 3D mode, we need to be able to disarm by switch at any time
if (featureIsEnabled(FEATURE_3D)) { if (featureIsEnabled(FEATURE_3D)) {

View file

@ -56,6 +56,8 @@
#include "rc.h" #include "rc.h"
#define RX_INTERVAL_MIN_US 950 // 0.950ms to fit 1kHz without an issue
#define RX_INTERVAL_MAX_US 65500 // 65.5ms or 15.26hz
typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs); typedef float (applyRatesFn)(const int axis, float rcCommandf, const float rcCommandfAbs);
// note that rcCommand[] is an external float // note that rcCommand[] is an external float
@ -68,11 +70,12 @@ static float maxRcDeflectionAbs;
static bool reverseMotors = false; static bool reverseMotors = false;
static applyRatesFn *applyRates; static applyRatesFn *applyRates;
static uint16_t currentRxIntervalUs; // packet interval in microseconds static uint16_t currentRxIntervalUs; // packet interval in microseconds, constrained to above range
static float currentRxRateHz; // packet rate in hertz static uint16_t previousRxIntervalUs; // previous packet interval in microseconds
static float currentRxRateHz; // packet interval in Hz, constrained as above
static bool isRxDataNew = false; static bool isRxDataNew = false;
static bool isRxIntervalValid = false; static bool isRxRateValid = false;
static float rcCommandDivider = 500.0f; static float rcCommandDivider = 500.0f;
static float rcCommandYawDivider = 500.0f; static float rcCommandYawDivider = 500.0f;
@ -110,9 +113,6 @@ static FAST_DATA_ZERO_INIT rcSmoothingFilter_t rcSmoothingData;
static float rcDeflectionSmoothed[3]; static float rcDeflectionSmoothed[3];
#endif // USE_RC_SMOOTHING_FILTER #endif // USE_RC_SMOOTHING_FILTER
#define RX_INTERVAL_MIN_US 950 // 0.950ms to fit 1kHz without an issue
#define RX_INTERVAL_MAX_US 65500 // 65.5ms or 15.26hz
float getSetpointRate(int axis) float getSetpointRate(int axis)
{ {
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
@ -265,32 +265,59 @@ static void scaleRawSetpointToFpvCamAngle(void)
rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX); rawSetpoint[YAW] = constrainf(yaw * cosFactor + roll * sinFactor, SETPOINT_RATE_LIMIT_MIN, SETPOINT_RATE_LIMIT_MAX);
} }
void updateRcRefreshRate(timeUs_t currentTimeUs) void updateRcRefreshRate(timeUs_t currentTimeUs, bool rxReceivingSignal)
{ {
static timeUs_t lastRxTimeUs; // this function runs from processRx in core.c
// rxReceivingSignal is true:
// - every time a new frame is detected,
// - if we stop getting data, at the expiry of RXLOSS_TRIGGER_INTERVAL since the last good frame
// - if that interval is exceeded and still no data, every RX_FRAME_RECHECK_INTERVAL, until a new frame is detected
static timeUs_t lastRxTimeUs = 0;
timeDelta_t delta = 0;
timeDelta_t frameAgeUs; if (rxReceivingSignal) { // true while receiving data and until RXLOSS_TRIGGER_INTERVAL expires, otherwise false
timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs); previousRxIntervalUs = currentRxIntervalUs;
// use driver rx time if available, current time otherwise
const timeUs_t rxTime = rxRuntimeState.lastRcFrameTimeUs ? rxRuntimeState.lastRcFrameTimeUs : currentTimeUs;
if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) { if (lastRxTimeUs) { // report delta only if previous time is available
frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); delta = cmpTimeUs(rxTime, lastRxTimeUs);
}
lastRxTimeUs = rxTime;
DEBUG_SET(DEBUG_RX_TIMING, 1, rxTime / 100); // output value in tenths of ms
} else {
if (lastRxTimeUs) {
// no packet received, use current time for delta
delta = cmpTimeUs(currentTimeUs, lastRxTimeUs);
}
} }
DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX)); // temporary debugs
DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX)); DEBUG_SET(DEBUG_RX_TIMING, 4, MIN(delta / 10, INT16_MAX)); // time between frames based on rxFrameCheck
#ifdef USE_RX_LINK_QUALITY_INFO
DEBUG_SET(DEBUG_RX_TIMING, 6, rxGetLinkQualityPercent()); // raw link quality value
#endif
DEBUG_SET(DEBUG_RX_TIMING, 7, isRxReceivingSignal()); // flag to initiate RXLOSS signal and Stage 1 values
lastRxTimeUs = currentTimeUs; // constrain to a frequency range no lower than about 15Hz and up to about 1000Hz
currentRxIntervalUs = constrain(frameDeltaUs, RX_INTERVAL_MIN_US, RX_INTERVAL_MAX_US); // these intervals and rates will be used for RCSmoothing, Feedforward, etc.
isRxIntervalValid = frameDeltaUs == currentRxIntervalUs; currentRxIntervalUs = constrain(delta, RX_INTERVAL_MIN_US, RX_INTERVAL_MAX_US);
currentRxRateHz = 1e6f / currentRxIntervalUs;
isRxRateValid = delta == currentRxIntervalUs; // delta is not constrained, therefore not outside limits
currentRxRateHz = 1e6f / currentRxIntervalUs; // cannot be zero due to preceding constraint DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(delta / 10, INT16_MAX)); // output value in hundredths of ms
DEBUG_SET(DEBUG_RX_TIMING, 2, isRxIntervalValid); DEBUG_SET(DEBUG_RX_TIMING, 2, isRxRateValid);
DEBUG_SET(DEBUG_RX_TIMING, 3, MIN(currentRxIntervalUs / 10, INT16_MAX)); DEBUG_SET(DEBUG_RX_TIMING, 3, MIN(currentRxIntervalUs / 10, INT16_MAX));
} }
uint16_t getCurrentRxIntervalUs(void) uint16_t getCurrentRxRateHz(void)
{ {
return currentRxIntervalUs; return currentRxRateHz;
}
bool getRxRateValid(void)
{
return isRxRateValid;
} }
#ifdef USE_RC_SMOOTHING_FILTER #ifdef USE_RC_SMOOTHING_FILTER
@ -416,19 +443,19 @@ static FAST_CODE void processRcSmoothingFilter(void)
const bool ready = (currentTimeMs > 1000) && (targetPidLooptime > 0); const bool ready = (currentTimeMs > 1000) && (targetPidLooptime > 0);
if (ready) { // skip during FC initialization if (ready) { // skip during FC initialization
// Wait 1000ms after power to let the PID loop stabilize before starting average frame rate calculation // Wait 1000ms after power to let the PID loop stabilize before starting average frame rate calculation
if (rxIsReceivingSignal() && isRxIntervalValid) { if (isRxReceivingSignal() && isRxRateValid) {
static uint16_t previousRxIntervalUs;
if (abs(currentRxIntervalUs - previousRxIntervalUs) < (previousRxIntervalUs - (previousRxIntervalUs / 8))) { if (abs(currentRxIntervalUs - previousRxIntervalUs) < (previousRxIntervalUs - (previousRxIntervalUs / 8))) {
// exclude large steps, eg after dropouts or telemetry // exclude large steps, eg after dropouts or telemetry
// by using interval here, we catch a dropout/telemetry where the inteval increases by 100%, but accept // by using interval here, we catch a dropout/telemetry where the inteval increases by 100%, but accept
// the return to normal value, which is only 50% different from the 100% interval of a single drop, and 66% of a return after a double drop. // the return to normal value, which is only 50% different from the 100% interval of a single drop, and 66% of a return after a double drop.
static float prevRxRateHz; static float prevSmoothedRxRateHz;
// smooth the current Rx link frequency estimates // smooth the current Rx link frequency estimates
const float kF = 0.1f; // first order lowpass smoothing filter coefficient const float kF = 0.1f; // first order kind of lowpass smoothing filter coefficient
const float smoothedRxRateHz = prevRxRateHz + kF * (currentRxRateHz - prevRxRateHz); // add one tenth of the new estimate to the smoothed estimate.
prevRxRateHz = smoothedRxRateHz; const float smoothedRxRateHz = prevSmoothedRxRateHz + kF * (currentRxRateHz - prevSmoothedRxRateHz);
prevSmoothedRxRateHz = smoothedRxRateHz;
// recalculate cutoffs every 3 acceptable samples // recalculate cutoffs every 3 acceptable samples
if (rcSmoothingData.sampleCount) { if (rcSmoothingData.sampleCount) {
rcSmoothingData.sampleCount --; rcSmoothingData.sampleCount --;
@ -441,7 +468,6 @@ static FAST_CODE void processRcSmoothingFilter(void)
sampleState = 2; sampleState = 2;
} }
} }
previousRxIntervalUs = currentRxIntervalUs;
} else { } else {
// either we stopped receiving rx samples (failsafe?) or the sample interval is unreasonable // either we stopped receiving rx samples (failsafe?) or the sample interval is unreasonable
// require a full re-evaluation period after signal is restored // require a full re-evaluation period after signal is restored

View file

@ -46,6 +46,6 @@ bool rcSmoothingInitializationComplete(void);
float getMaxRcRate(int axis); float getMaxRcRate(int axis);
float getFeedforward(int axis); float getFeedforward(int axis);
void updateRcRefreshRate(timeUs_t currentTimeUs); void updateRcRefreshRate(timeUs_t currentTimeUs, bool rxReceivingSignal);
uint16_t getCurrentRxIntervalUs(void); uint16_t getCurrentRxRateHz(void);
bool getRxRateValid(void); bool getRxRateValid(void);

View file

@ -835,7 +835,7 @@ static void processContinuosAdjustments(controlRateConfig_t *controlRateConfig)
void processRcAdjustments(controlRateConfig_t *controlRateConfig) void processRcAdjustments(controlRateConfig_t *controlRateConfig)
{ {
const bool canUseRxData = rxIsReceivingSignal(); const bool canUseRxData = isRxReceivingSignal();
// Recalculate the new active adjustments if required // Recalculate the new active adjustments if required
if (stepwiseAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) { if (stepwiseAdjustmentCount == ADJUSTMENT_RANGE_COUNT_INVALID) {

View file

@ -149,7 +149,7 @@ static bool failsafeShouldHaveCausedLandingByNow(void)
bool failsafeIsReceivingRxData(void) bool failsafeIsReceivingRxData(void)
{ {
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP); return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
// False with BOXFAILSAFE switch or when no valid packets for 100ms or any flight channel invalid for 300ms, // False with BOXFAILSAFE switch or when no valid packets for RXLOSS_TRIGGER_INTERVAL or any flight channel invalid for 300ms,
// becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received // becomes true immediately BOXFAILSAFE switch reverts, or after recovery period expires when valid packets are received
// rxLinkState RXLINK_DOWN (not up) is the trigger for the various failsafe stage 2 outcomes. // rxLinkState RXLINK_DOWN (not up) is the trigger for the various failsafe stage 2 outcomes.
} }
@ -184,7 +184,7 @@ void failsafeOnValidDataReceived(void)
if (cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.receivingRxDataPeriodPreset) { if (cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.receivingRxDataPeriodPreset) {
// receivingRxDataPeriodPreset is rxDataRecoveryPeriod unless set to zero to allow immediate control recovery after switch induced failsafe // receivingRxDataPeriodPreset is rxDataRecoveryPeriod unless set to zero to allow immediate control recovery after switch induced failsafe
// rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms) // rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (100ms)
// link is not considered 'up', after it has been 'down', until that recovery period has expired // link is not considered 'up', after it has been 'down', until that recovery period has expired
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP; failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
// after the rxDataRecoveryPeriod, typically 1s after receiving valid data, clear RXLOSS in OSD and permit arming // after the rxDataRecoveryPeriod, typically 1s after receiving valid data, clear RXLOSS in OSD and permit arming
@ -193,11 +193,11 @@ void failsafeOnValidDataReceived(void)
} }
void failsafeOnValidDataFailed(void) void failsafeOnValidDataFailed(void)
// run from rc.c when packets are lost for more than the signal validation period (100ms), or immediately BOXFAILSAFE switch is active // run from rc.c when packets are lost for more than RXLOSS_TRIGGER_INTERVAL, or immediately BOXFAILSAFE switch is active
// after the stage 1 delay has expired, sets the rxLinkState to RXLINK_DOWN, ie not up, causing failsafeIsReceivingRxData to become false // after the stage 1 delay has expired, sets the rxLinkState to RXLINK_DOWN, ie not up, causing failsafeIsReceivingRxData to become false
// if failsafe is configured to go direct to stage 2, this is emulated immediately in failsafeUpdateState() // if failsafe is configured to go direct to stage 2, this is emulated immediately in failsafeUpdateState()
{ {
// set RXLOSS in OSD and block arming after 100ms of signal loss // set RXLOSS in OSD and block arming after RXLOSS_TRIGGER_INTERVAL of frame loss
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
failsafeState.validRxDataFailedAt = millis(); failsafeState.validRxDataFailedAt = millis();

View file

@ -425,7 +425,7 @@ static void performSanityChecks(void)
// Handle events that set a failure mode to other than healthy. // Handle events that set a failure mode to other than healthy.
// Disarm via Abort when sanity on, or for hard Rx loss in FS_ONLY mode // Disarm via Abort when sanity on, or for hard Rx loss in FS_ONLY mode
// Otherwise allow 20s of semi-controlled descent with impact disarm detection // Otherwise allow 20s of semi-controlled descent with impact disarm detection
const bool hardFailsafe = !rxIsReceivingSignal(); const bool hardFailsafe = !isRxReceivingSignal();
if (rescueState.failure != RESCUE_HEALTHY) { if (rescueState.failure != RESCUE_HEALTHY) {
// Default to 20s semi-controlled descent with impact detection, then abort // Default to 20s semi-controlled descent with impact detection, then abort

View file

@ -203,7 +203,7 @@ static void updateRxStatus(void)
{ {
i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0); i2c_OLED_set_xy(dev, SCREEN_CHARACTER_COLUMN_COUNT - 2, 0);
char rxStatus = '!'; char rxStatus = '!';
if (rxIsReceivingSignal()) { if (isRxReceivingSignal()) {
rxStatus = 'r'; rxStatus = 'r';
} if (rxAreFlightChannelsValid()) { } if (rxAreFlightChannelsValid()) {
rxStatus = 'R'; rxStatus = 'R';

View file

@ -962,7 +962,7 @@ static void applyLedIndicatorLayer(bool updateNow, timeUs_t *timer)
static bool flash = 0; static bool flash = 0;
if (updateNow) { if (updateNow) {
if (rxIsReceivingSignal()) { if (isRxReceivingSignal()) {
// calculate update frequency // calculate update frequency
int scale = MAX(fabsf(rcCommand[ROLL]), fabsf(rcCommand[PITCH])); // 0 - 500 int scale = MAX(fabsf(rcCommand[ROLL]), fabsf(rcCommand[PITCH])); // 0 - 500
scale = scale - INDICATOR_DEADBAND; // start increasing frequency right after deadband scale = scale - INDICATOR_DEADBAND; // start increasing frequency right after deadband

View file

@ -630,7 +630,6 @@ bool crsfRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = CRSF_MAX_CHANNEL; rxRuntimeState->channelCount = CRSF_MAX_CHANNEL;
rxRuntimeState->rcReadRawFn = crsfReadRawRC; rxRuntimeState->rcReadRawFn = crsfReadRawRC;
rxRuntimeState->rcFrameStatusFn = crsfFrameStatus; rxRuntimeState->rcFrameStatusFn = crsfFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {

View file

@ -394,7 +394,6 @@ bool fportRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = SBUS_MAX_CHANNEL; rxRuntimeState->channelCount = SBUS_MAX_CHANNEL;
rxRuntimeState->rcFrameStatusFn = fportFrameStatus; rxRuntimeState->rcFrameStatusFn = fportFrameStatus;
rxRuntimeState->rcProcessFrameFn = fportProcessFrame; rxRuntimeState->rcProcessFrameFn = fportProcessFrame;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {

View file

@ -417,7 +417,6 @@ bool ghstRxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS; rxRuntimeState->channelCount = GHST_MAX_NUM_CHANNELS;
rxRuntimeState->rcReadRawFn = ghstReadRawRC; rxRuntimeState->rcReadRawFn = ghstReadRawRC;
rxRuntimeState->rcFrameStatusFn = ghstFrameStatus; rxRuntimeState->rcFrameStatusFn = ghstFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
rxRuntimeState->rcProcessFrameFn = ghstProcessFrame; rxRuntimeState->rcProcessFrameFn = ghstProcessFrame;
for (int iChan = 0; iChan < rxRuntimeState->channelCount; ++iChan) { for (int iChan = 0; iChan < rxRuntimeState->channelCount; ++iChan) {

View file

@ -211,7 +211,6 @@ bool ibusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = IBUS_MAX_CHANNEL; rxRuntimeState->channelCount = IBUS_MAX_CHANNEL;
rxRuntimeState->rcReadRawFn = ibusReadRawRC; rxRuntimeState->rcReadRawFn = ibusReadRawRC;
rxRuntimeState->rcFrameStatusFn = ibusFrameStatus; rxRuntimeState->rcFrameStatusFn = ibusFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {

View file

@ -262,7 +262,6 @@ bool jetiExBusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = JETIEXBUS_CHANNEL_COUNT; rxRuntimeState->channelCount = JETIEXBUS_CHANNEL_COUNT;
rxRuntimeState->rcReadRawFn = jetiExBusReadRawRC; rxRuntimeState->rcReadRawFn = jetiExBusReadRawRC;
rxRuntimeState->rcFrameStatusFn = jetiExBusFrameStatus; rxRuntimeState->rcFrameStatusFn = jetiExBusFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
jetiExBusFrameReset(); jetiExBusFrameReset();

View file

@ -132,8 +132,9 @@ uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT];
// will not be actioned until the nearest multiple of 100ms // will not be actioned until the nearest multiple of 100ms
#define PPM_AND_PWM_SAMPLE_COUNT 3 #define PPM_AND_PWM_SAMPLE_COUNT 3
#define DELAY_20_MS (20 * 1000) // 20ms in us #define RSSI_UPDATE_INTERVAL (20 * 1000) // 20ms in us
#define DELAY_100_MS (100 * 1000) // 100ms in us #define RX_FRAME_RECHECK_INTERVAL (50 * 1000) // 50ms in us
#define RXLOSS_TRIGGER_INTERVAL (150 * 1000) // 150ms in us
#define DELAY_1500_MS (1500 * 1000) // 1.5 seconds in us #define DELAY_1500_MS (1500 * 1000) // 1.5 seconds in us
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent) #define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
@ -294,7 +295,7 @@ void rxInit(void)
rxRuntimeState.rcReadRawFn = nullReadRawRC; rxRuntimeState.rcReadRawFn = nullReadRawRC;
rxRuntimeState.rcFrameStatusFn = nullFrameStatus; rxRuntimeState.rcFrameStatusFn = nullFrameStatus;
rxRuntimeState.rcProcessFrameFn = nullProcessFrame; rxRuntimeState.rcProcessFrameFn = nullProcessFrame;
rxRuntimeState.lastRcFrameTimeUs = 0; rxRuntimeState.lastRcFrameTimeUs = 0; // zero when driver does not provide timing info
rcSampleIndex = 0; rcSampleIndex = 0;
uint32_t now = millis(); uint32_t now = millis();
@ -396,7 +397,7 @@ void rxInit(void)
rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeState.channelCount); rxChannelCount = MIN(rxConfig()->max_aux_channel + NON_AUX_CHANNEL_COUNT, rxRuntimeState.channelCount);
} }
bool rxIsReceivingSignal(void) bool isRxReceivingSignal(void)
{ {
return rxSignalReceived; return rxSignalReceived;
} }
@ -502,9 +503,10 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs) FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
{ {
bool signalReceived = false; bool rxDataReceived = false;
bool useDataDrivenProcessing = true; bool useDataDrivenProcessing = true;
timeDelta_t needRxSignalMaxDelayUs = DELAY_100_MS; timeDelta_t needRxSignalMaxDelayUs = RXLOSS_TRIGGER_INTERVAL;
timeDelta_t reCheckRxSignalInterval = RX_FRAME_RECHECK_INTERVAL;
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, MIN(2000, currentDeltaTimeUs / 100)); DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, MIN(2000, currentDeltaTimeUs / 100));
@ -520,14 +522,14 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
#if defined(USE_RX_PWM) || defined(USE_RX_PPM) #if defined(USE_RX_PWM) || defined(USE_RX_PPM)
case RX_PROVIDER_PPM: case RX_PROVIDER_PPM:
if (isPPMDataBeingReceived()) { if (isPPMDataBeingReceived()) {
signalReceived = true; rxDataReceived = true;
resetPPMDataReceivedState(); resetPPMDataReceivedState();
} }
break; break;
case RX_PROVIDER_PARALLEL_PWM: case RX_PROVIDER_PARALLEL_PWM:
if (isPWMDataBeingReceived()) { if (isPWMDataBeingReceived()) {
signalReceived = true; rxDataReceived = true;
useDataDrivenProcessing = false; useDataDrivenProcessing = false;
} }
@ -540,15 +542,15 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
{ {
const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState); const uint8_t frameStatus = rxRuntimeState.rcFrameStatusFn(&rxRuntimeState);
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE)); DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, (frameStatus & RX_FRAME_FAILSAFE));
signalReceived = (frameStatus & RX_FRAME_COMPLETE) && !(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED)); rxDataReceived = (frameStatus & RX_FRAME_COMPLETE) && !(frameStatus & (RX_FRAME_FAILSAFE | RX_FRAME_DROPPED));
setLinkQuality(signalReceived, currentDeltaTimeUs); setLinkQuality(rxDataReceived, currentDeltaTimeUs);
auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED); auxiliaryProcessingRequired |= (frameStatus & RX_FRAME_PROCESSING_REQUIRED);
} }
break; break;
} }
if (signalReceived) { if (rxDataReceived) {
// true only when a new packet arrives // true only when a new packet arrives
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
rxSignalReceived = true; // immediately process packet data rxSignalReceived = true; // immediately process packet data
@ -559,10 +561,10 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
} else { } else {
// watch for next packet // watch for next packet
if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) { if (cmpTimeUs(currentTimeUs, needRxSignalBefore) > 0) {
// initial time to signalReceived failure is 100ms, then we check every 100ms // initial time to rxDataReceived failure is RXLOSS_TRIGGER_INTERVAL (150ms),
rxSignalReceived = false; // after that, we check every RX_FRAME_RECHECK_INTERVAL (50ms)
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs; rxSignalReceived = false; // results in `RXLOSS` message etc
// review and process rcData values every 100ms in case failsafe changed them needRxSignalBefore += reCheckRxSignalInterval;
rxDataProcessingRequired = true; rxDataProcessingRequired = true;
} }
} }
@ -686,7 +688,7 @@ void detectAndApplySignalLossBehaviour(void)
const uint32_t currentTimeMs = millis(); const uint32_t currentTimeMs = millis();
const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE); const bool boxFailsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn; rxFlightChannelsValid = rxSignalReceived && !boxFailsafeSwitchIsOn;
// rxFlightChannelsValid is false after 100ms of no packets, or as soon as use the BOXFAILSAFE switch is actioned // rxFlightChannelsValid is false after RXLOSS_TRIGGER_INTERVAL of no packets, or as soon as use the BOXFAILSAFE switch is actioned
// rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted // rxFlightChannelsValid is true the instant we get a good packet or the BOXFAILSAFE switch is reverted
// can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error) // can also go false with good packets but where one flight channel is bad > 300ms (PPM type receiver error)
@ -862,7 +864,7 @@ static void updateRSSIADC(timeUs_t currentTimeUs)
if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) { if ((int32_t)(currentTimeUs - rssiUpdateAt) < 0) {
return; return;
} }
rssiUpdateAt = currentTimeUs + DELAY_20_MS; rssiUpdateAt = currentTimeUs + RSSI_UPDATE_INTERVAL;
const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI); const uint16_t adcRssiSample = adcGetChannel(ADC_RSSI);
uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR; uint16_t rssiValue = adcRssiSample / RSSI_ADC_DIVISOR;
@ -1022,27 +1024,3 @@ bool isRssiConfigured(void)
return rssiSource != RSSI_SOURCE_NONE; return rssiSource != RSSI_SOURCE_NONE;
} }
timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs)
{
static timeUs_t previousFrameTimeUs = 0;
static timeDelta_t frameTimeDeltaUs = 0;
if (rxRuntimeState.rcFrameTimeUsFn) {
const timeUs_t frameTimeUs = rxRuntimeState.rcFrameTimeUsFn();
*frameAgeUs = cmpTimeUs(micros(), frameTimeUs);
const timeDelta_t deltaUs = cmpTimeUs(frameTimeUs, previousFrameTimeUs);
if (deltaUs) {
frameTimeDeltaUs = deltaUs;
previousFrameTimeUs = frameTimeUs;
}
}
return frameTimeDeltaUs;
}
timeUs_t rxFrameTimeUs(void)
{
return rxRuntimeState.lastRcFrameTimeUs;
}

View file

@ -144,7 +144,6 @@ typedef struct rxRuntimeState_s {
rcReadRawDataFnPtr rcReadRawFn; rcReadRawDataFnPtr rcReadRawFn;
rcFrameStatusFnPtr rcFrameStatusFn; rcFrameStatusFnPtr rcFrameStatusFn;
rcProcessFrameFnPtr rcProcessFrameFn; rcProcessFrameFnPtr rcProcessFrameFn;
rcGetFrameTimeUsFn *rcFrameTimeUsFn;
uint16_t *channelData; uint16_t *channelData;
void *frameData; void *frameData;
timeUs_t lastRcFrameTimeUs; timeUs_t lastRcFrameTimeUs;
@ -176,7 +175,7 @@ void rxInit(void);
void rxProcessPending(bool state); void rxProcessPending(bool state);
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs); void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
bool rxIsReceivingSignal(void); bool isRxReceivingSignal(void);
bool rxAreFlightChannelsValid(void); bool rxAreFlightChannelsValid(void);
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs); bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
@ -225,6 +224,6 @@ void resetAllRxChannelRangeConfigurations(rxChannelRangeConfig_t *rxChannelRange
void suspendRxSignal(void); void suspendRxSignal(void);
void resumeRxSignal(void); void resumeRxSignal(void);
timeDelta_t rxGetFrameDelta(timeDelta_t *frameAgeUs); timeDelta_t rxGetFrameDelta(void);
timeUs_t rxFrameTimeUs(void); timeUs_t rxFrameTimeUs(void);

View file

@ -205,14 +205,15 @@ STATIC_UNIT_TESTED bool rxSpiSetProtocol(rx_spi_protocol_e protocol)
*/ */
static uint8_t rxSpiFrameStatus(rxRuntimeState_t *rxRuntimeState) static uint8_t rxSpiFrameStatus(rxRuntimeState_t *rxRuntimeState)
{ {
UNUSED(rxRuntimeState);
uint8_t status = RX_FRAME_PENDING; uint8_t status = RX_FRAME_PENDING;
rx_spi_received_e result = protocolDataReceived(rxSpiPayload); rx_spi_received_e result = protocolDataReceived(rxSpiPayload);
if (result & RX_SPI_RECEIVED_DATA) { if (result & RX_SPI_RECEIVED_DATA) {
rxSpiNewPacketAvailable = true; rxSpiNewPacketAvailable = true;
// use SPI EXTI time as frame time
// note that there is not rx time without EXTI
rxRuntimeState->lastRcFrameTimeUs = rxSpiGetLastExtiTimeUs();
status = RX_FRAME_COMPLETE; status = RX_FRAME_COMPLETE;
} }
@ -260,8 +261,6 @@ bool rxSpiInit(const rxSpiConfig_t *rxSpiConfig, rxRuntimeState_t *rxRuntimeStat
if (rxSpiExtiConfigured()) { if (rxSpiExtiConfigured()) {
rxSpiExtiInit(extiConfig.ioConfig, extiConfig.trigger); rxSpiExtiInit(extiConfig.ioConfig, extiConfig.trigger);
rxRuntimeState->rcFrameTimeUsFn = rxSpiGetLastExtiTimeUs;
} }
rxSpiNewPacketAvailable = false; rxSpiNewPacketAvailable = false;

View file

@ -177,7 +177,6 @@ bool sbusInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
} }
rxRuntimeState->rcFrameStatusFn = sbusFrameStatus; rxRuntimeState->rcFrameStatusFn = sbusFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {

View file

@ -387,7 +387,6 @@ bool spektrumInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->rcReadRawFn = spektrumReadRawRC; rxRuntimeState->rcReadRawFn = spektrumReadRawRC;
rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus; rxRuntimeState->rcFrameStatusFn = spektrumFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
#if defined(USE_TELEMETRY_SRXL) #if defined(USE_TELEMETRY_SRXL)
rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame; rxRuntimeState->rcProcessFrameFn = spektrumProcessFrame;
#endif #endif

View file

@ -491,7 +491,6 @@ bool srxl2RxInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = SRXL2_MAX_CHANNELS; rxRuntimeState->channelCount = SRXL2_MAX_CHANNELS;
rxRuntimeState->rcReadRawFn = srxl2ReadRawRC; rxRuntimeState->rcReadRawFn = srxl2ReadRawRC;
rxRuntimeState->rcFrameStatusFn = srxl2FrameStatus; rxRuntimeState->rcFrameStatusFn = srxl2FrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
rxRuntimeState->rcProcessFrameFn = srxl2ProcessFrame; rxRuntimeState->rcProcessFrameFn = srxl2ProcessFrame;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);

View file

@ -174,7 +174,6 @@ bool sumdInit(const rxConfig_t *rxConfig, rxRuntimeState_t *rxRuntimeState)
rxRuntimeState->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT); rxRuntimeState->channelCount = MIN(SUMD_MAX_CHANNEL, MAX_SUPPORTED_RC_CHANNEL_COUNT);
rxRuntimeState->rcReadRawFn = sumdReadRawRC; rxRuntimeState->rcReadRawFn = sumdReadRawRC;
rxRuntimeState->rcFrameStatusFn = sumdFrameStatus; rxRuntimeState->rcFrameStatusFn = sumdFrameStatus;
rxRuntimeState->rcFrameTimeUsFn = rxFrameTimeUs;
const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL); const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_RX_SERIAL);
if (!portConfig) { if (!portConfig) {

View file

@ -1051,7 +1051,7 @@ TEST(ArmingPreventionTest, Paralyze)
extern "C" { extern "C" {
uint32_t micros(void) { return simulationTime; } uint32_t micros(void) { return simulationTime; }
uint32_t millis(void) { return micros() / 1000; } uint32_t millis(void) { return micros() / 1000; }
bool rxIsReceivingSignal(void) { return simulationHaveRx; } bool isRxReceivingSignal(void) { return simulationHaveRx; }
bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; }
void warningLedFlash(void) {} void warningLedFlash(void) {}
@ -1128,7 +1128,7 @@ extern "C" {
bool isUpright(void) { return mockIsUpright; } bool isUpright(void) { return mockIsUpright; }
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {}; void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } timeDelta_t rxGetFrameDelta() { return 0; }
void updateRcRefreshRate(timeUs_t) {}; void updateRcRefreshRate(timeUs_t) {};
uint16_t getAverageSystemLoadPercent(void) { return 0; } uint16_t getAverageSystemLoadPercent(void) { return 0; }
bool isMotorProtocolEnabled(void) { return true; } bool isMotorProtocolEnabled(void) { return true; }

View file

@ -380,7 +380,7 @@ void closeSerialPort(serialPort_t *) {}
portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e ) {return PORTSHARING_UNUSED;} portSharing_e determinePortSharing(const serialPortConfig_t *, serialPortFunction_e ) {return PORTSHARING_UNUSED;}
failsafePhase_e failsafePhase(void) {return FAILSAFE_IDLE;} failsafePhase_e failsafePhase(void) {return FAILSAFE_IDLE;}
bool rxAreFlightChannelsValid(void) {return false;} bool rxAreFlightChannelsValid(void) {return false;}
bool rxIsReceivingSignal(void) {return false;} bool isRxReceivingSignal(void) {return false;}
bool isRssiConfigured(void) {return false;} bool isRssiConfigured(void) {return false;}
float getMotorOutputLow(void) {return 0.0;} float getMotorOutputLow(void) {return 0.0;}
float getMotorOutputHigh(void) {return 0.0;} float getMotorOutputHigh(void) {return 0.0;}

View file

@ -387,6 +387,7 @@ bool isModeActivationConditionConfigured(const modeActivationCondition_t *, cons
void delay(uint32_t) {} void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; } displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; } mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; }
uint16_t getCurrentRxIntervalUs(void) { return 0; } uint16_t getCurrentRxRateHz(void) { return 0; }
uint16_t getAverageSystemLoadPercent(void) { return 0; } uint16_t getAverageSystemLoadPercent(void) { return 0; }
bool getRxRateValid(void) { return false; }
} }

View file

@ -588,7 +588,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
// when // when
failsafeUpdateState(); failsafeUpdateState();
// now should be in monitoring mode, with switch holding signalReceived false // now should be in monitoring mode, with switch holding rxDataReceived false
EXPECT_TRUE(failsafeIsActive()); EXPECT_TRUE(failsafeIsActive());
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase()); EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
EXPECT_TRUE(isArmingDisabled()); EXPECT_TRUE(isArmingDisabled());

View file

@ -386,7 +386,7 @@ int scaleRange(int x, int srcMin, int srcMax, int destMin, int destMax)
} }
bool failsafeIsActive() { return false; } bool failsafeIsActive() { return false; }
bool rxIsReceivingSignal() { return true; } bool isRxReceivingSignal() { return true; }
bool isBeeperOn() { return false; }; bool isBeeperOn() { return false; };

View file

@ -622,7 +622,7 @@ void dashboardDisablePageCycling() {}
void dashboardEnablePageCycling() {} void dashboardEnablePageCycling() {}
bool failsafeIsActive() { return false; } bool failsafeIsActive() { return false; }
bool rxIsReceivingSignal() { return true; } bool isRxReceivingSignal() { return true; }
bool failsafeIsReceivingRxData() { return true; } bool failsafeIsReceivingRxData() { return true; }
uint8_t getCurrentControlRateProfileIndex(void) uint8_t getCurrentControlRateProfileIndex(void)

View file

@ -130,7 +130,7 @@ extern "C" {
uint8_t activePidLoopDenom = 1; uint8_t activePidLoopDenom = 1;
uint32_t micros(void) { return simulationTime; } uint32_t micros(void) { return simulationTime; }
uint32_t millis(void) { return micros() / 1000; } uint32_t millis(void) { return micros() / 1000; }
bool rxIsReceivingSignal(void) { return simulationHaveRx; } bool isRxReceivingSignal(void) { return simulationHaveRx; }
bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; }
void warningLedFlash(void) {} void warningLedFlash(void) {}
@ -202,7 +202,7 @@ extern "C" {
bool isUpright(void) { return true; } bool isUpright(void) { return true; }
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {}; void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {}; void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; } timeDelta_t rxGetFrameDelta() { return 0; }
void updateRcRefreshRate(timeUs_t) {}; void updateRcRefreshRate(timeUs_t) {};
uint16_t getAverageSystemLoadPercent(void) { return 0; } uint16_t getAverageSystemLoadPercent(void) { return 0; }
bool isMotorProtocolEnabled(void) { return false; } bool isMotorProtocolEnabled(void) { return false; }