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:
parent
a37bd7c974
commit
776e8c7b3c
29 changed files with 108 additions and 116 deletions
|
@ -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();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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)) {
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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();
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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';
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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();
|
||||||
|
|
||||||
|
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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;
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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
|
||||||
|
|
|
@ -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);
|
||||||
|
|
|
@ -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) {
|
||||||
|
|
|
@ -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; }
|
||||||
|
|
|
@ -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;}
|
||||||
|
|
|
@ -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; }
|
||||||
}
|
}
|
||||||
|
|
|
@ -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());
|
||||||
|
|
|
@ -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; };
|
||||||
|
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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; }
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue