mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-14 03:50:02 +03:00
Fix failsafe timings and behaviour to match Failsafe.md
This commit is contained in:
Steve Evans
parent
a6207a100e
commit
07f6bea174
18 changed files with 214 additions and 136 deletions
|
|
@ -6162,8 +6162,8 @@ static void cliDshotTelemetryInfo(const char *cmdName, char *cmdline)
|
|||
if (useDshotTelemetry) {
|
||||
cliPrintLinef("Dshot reads: %u", dshotTelemetryState.readCount);
|
||||
cliPrintLinef("Dshot invalid pkts: %u", dshotTelemetryState.invalidPacketCount);
|
||||
uint32_t directionChangeCycles = dshotDMAHandlerCycleCounters.changeDirectionCompletedAt - dshotDMAHandlerCycleCounters.irqAt;
|
||||
uint32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
|
||||
int32_t directionChangeCycles = cmp32(dshotDMAHandlerCycleCounters.changeDirectionCompletedAt, dshotDMAHandlerCycleCounters.irqAt);
|
||||
int32_t directionChangeDurationUs = clockCyclesToMicros(directionChangeCycles);
|
||||
cliPrintLinef("Dshot directionChange cycles: %u, micros: %u", directionChangeCycles, directionChangeDurationUs);
|
||||
cliPrintLinefeed();
|
||||
|
||||
|
|
|
|||
|
|
@ -153,7 +153,7 @@ uint32_t getCycleCounter(void)
|
|||
return DWT->CYCCNT;
|
||||
}
|
||||
|
||||
uint32_t clockCyclesToMicros(uint32_t clockCycles)
|
||||
int32_t clockCyclesToMicros(int32_t clockCycles)
|
||||
{
|
||||
return clockCycles / usTicks;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -65,7 +65,7 @@ void systemReset(void);
|
|||
void systemResetToBootloader(bootloaderRequestType_e requestType);
|
||||
bool isMPUSoftReset(void);
|
||||
void cycleCounterInit(void);
|
||||
uint32_t clockCyclesToMicros(uint32_t clockCycles);
|
||||
int32_t clockCyclesToMicros(int32_t clockCycles);
|
||||
int32_t clockCyclesTo10thMicros(int32_t clockCycles);
|
||||
uint32_t clockMicrosToCycles(uint32_t micros);
|
||||
uint32_t getCycleCounter(void);
|
||||
|
|
|
|||
|
|
@ -744,12 +744,6 @@ bool isAirmodeActivated()
|
|||
*/
|
||||
bool processRx(timeUs_t currentTimeUs)
|
||||
{
|
||||
timeDelta_t frameAgeUs;
|
||||
timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
|
||||
|
||||
DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
|
||||
DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
|
||||
|
||||
if (!calculateRxChannelsAndUpdateFailsafe(currentTimeUs)) {
|
||||
return false;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -298,13 +298,18 @@ void updateRcRefreshRate(timeUs_t currentTimeUs)
|
|||
static timeUs_t lastRxTimeUs;
|
||||
|
||||
timeDelta_t frameAgeUs;
|
||||
timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
|
||||
if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
|
||||
refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
|
||||
timeDelta_t frameDeltaUs = rxGetFrameDelta(&frameAgeUs);
|
||||
|
||||
if (!frameDeltaUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
|
||||
frameDeltaUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RX_TIMING, 0, MIN(frameDeltaUs / 10, INT16_MAX));
|
||||
DEBUG_SET(DEBUG_RX_TIMING, 1, MIN(frameAgeUs / 10, INT16_MAX));
|
||||
|
||||
lastRxTimeUs = currentTimeUs;
|
||||
isRxRateValid = (refreshRateUs >= RC_RX_RATE_MIN_US && refreshRateUs <= RC_RX_RATE_MAX_US);
|
||||
currentRxRefreshRate = constrain(refreshRateUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
|
||||
isRxRateValid = (frameDeltaUs >= RC_RX_RATE_MIN_US && frameDeltaUs <= RC_RX_RATE_MAX_US);
|
||||
currentRxRefreshRate = constrain(frameDeltaUs, RC_RX_RATE_MIN_US, RC_RX_RATE_MAX_US);
|
||||
}
|
||||
|
||||
uint16_t getCurrentRxRefreshRate(void)
|
||||
|
|
|
|||
|
|
@ -65,11 +65,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CO
|
|||
PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
|
||||
.failsafe_throttle = 1000, // default throttle off.
|
||||
.failsafe_throttle_low_delay = 100, // default throttle low delay for "just disarm" on failsafe condition
|
||||
.failsafe_delay = 4, // 0,4sec
|
||||
.failsafe_off_delay = 10, // 1sec
|
||||
.failsafe_switch_mode = 0, // default failsafe switch action is identical to rc link loss
|
||||
.failsafe_delay = 10, // 1 sec, can regain control on signal recovery, at idle in drop mode
|
||||
.failsafe_off_delay = 10, // 1 sec in landing phase, if enabled
|
||||
.failsafe_switch_mode = FAILSAFE_SWITCH_MODE_STAGE1, // default failsafe switch action is identical to rc link loss
|
||||
.failsafe_procedure = FAILSAFE_PROCEDURE_DROP_IT,// default full failsafe procedure is 0: auto-landing
|
||||
.failsafe_recovery_delay = 20, // 2 sec of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
|
||||
.failsafe_recovery_delay = 10, // 1 sec of valid rx data needed to allow recovering from failsafe procedure
|
||||
.failsafe_stick_threshold = 30 // 30 percent of stick deflection to exit GPS Rescue procedure
|
||||
);
|
||||
|
||||
|
|
@ -86,8 +86,12 @@ const char * const failsafeProcedureNames[FAILSAFE_PROCEDURE_COUNT] = {
|
|||
*/
|
||||
void failsafeReset(void)
|
||||
{
|
||||
failsafeState.rxDataFailurePeriod = PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND;
|
||||
failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY + failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
|
||||
failsafeState.rxDataFailurePeriod = failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND; // time to start stage2
|
||||
failsafeState.rxDataRecoveryPeriod = failsafeConfig()->failsafe_recovery_delay * MILLIS_PER_TENTH_SECOND;
|
||||
if (failsafeState.rxDataRecoveryPeriod < PERIOD_RXDATA_RECOVERY){
|
||||
// PERIOD_RXDATA_RECOVERY (200ms) is the minimum allowed RxData recovery time
|
||||
failsafeState.rxDataRecoveryPeriod = PERIOD_RXDATA_RECOVERY;
|
||||
}
|
||||
failsafeState.validRxDataReceivedAt = 0;
|
||||
failsafeState.validRxDataFailedAt = 0;
|
||||
failsafeState.throttleLowPeriod = 0;
|
||||
|
|
@ -138,26 +142,12 @@ static void failsafeActivate(void)
|
|||
failsafeState.phase = FAILSAFE_LANDING;
|
||||
|
||||
ENABLE_FLIGHT_MODE(FAILSAFE_MODE);
|
||||
|
||||
failsafeState.landingShouldBeFinishedAt = millis() + failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND;
|
||||
|
||||
failsafeState.events++;
|
||||
}
|
||||
|
||||
static void failsafeApplyControlInput(void)
|
||||
{
|
||||
#ifdef USE_GPS_RESCUE
|
||||
if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) {
|
||||
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
|
||||
return;
|
||||
}
|
||||
#endif
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
rcData[i] = rxConfig()->midrc;
|
||||
}
|
||||
rcData[THROTTLE] = failsafeConfig()->failsafe_throttle;
|
||||
}
|
||||
|
||||
bool failsafeIsReceivingRxData(void)
|
||||
{
|
||||
return (failsafeState.rxLinkState == FAILSAFE_RXLINK_UP);
|
||||
|
|
@ -177,42 +167,61 @@ void failsafeOnRxResume(void)
|
|||
void failsafeOnValidDataReceived(void)
|
||||
{
|
||||
failsafeState.validRxDataReceivedAt = millis();
|
||||
if ((failsafeState.validRxDataReceivedAt - failsafeState.validRxDataFailedAt) > failsafeState.rxDataRecoveryPeriod) {
|
||||
if ((cmp32(failsafeState.validRxDataReceivedAt, failsafeState.validRxDataFailedAt) > (int32_t)failsafeState.rxDataRecoveryPeriod) ||
|
||||
(cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > 0)) {
|
||||
// rxDataRecoveryPeriod defaults to 1.0s with minimum of PERIOD_RXDATA_RECOVERY (200ms)
|
||||
failsafeState.rxLinkState = FAILSAFE_RXLINK_UP;
|
||||
// Allow arming now we have an RX link
|
||||
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
|
||||
}
|
||||
}
|
||||
|
||||
void failsafeOnValidDataFailed(void)
|
||||
{
|
||||
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
|
||||
failsafeState.validRxDataFailedAt = millis();
|
||||
if ((failsafeState.validRxDataFailedAt - failsafeState.validRxDataReceivedAt) > failsafeState.rxDataFailurePeriod) {
|
||||
if ((cmp32(failsafeState.validRxDataFailedAt, failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod)) {
|
||||
// rxDataFailurePeriod is stage 1 guard time
|
||||
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
|
||||
// Prevent arming with no RX link
|
||||
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
|
||||
}
|
||||
}
|
||||
|
||||
void failsafeCheckDataFailurePeriod(void)
|
||||
// forces link down directly from scheduler?
|
||||
{
|
||||
if (cmp32(millis(), failsafeState.validRxDataReceivedAt) > (int32_t)failsafeState.rxDataFailurePeriod) {
|
||||
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE); // To prevent arming with no RX link
|
||||
failsafeState.rxLinkState = FAILSAFE_RXLINK_DOWN;
|
||||
// Prevent arming with no RX link
|
||||
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
|
||||
}
|
||||
}
|
||||
|
||||
uint32_t failsafeFailurePeriodMs(void)
|
||||
{
|
||||
return failsafeState.rxDataFailurePeriod;
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
||||
// triggered directly, and ONLY, by the cheduler, at 10ms = PERIOD_RXDATA_FAILURE - intervals
|
||||
{
|
||||
if (!failsafeIsMonitoring()) {
|
||||
return;
|
||||
}
|
||||
|
||||
bool receivingRxData = failsafeIsReceivingRxData();
|
||||
// should be true when FAILSAFE_RXLINK_UP
|
||||
// FAILSAFE_RXLINK_UP is set in failsafeOnValidDataReceived
|
||||
// failsafeOnValidDataReceived runs from detectAndApplySignalLossBehaviour
|
||||
|
||||
bool armed = ARMING_FLAG(ARMED);
|
||||
bool failsafeSwitchIsOn = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
|
||||
beeperMode_e beeperMode = BEEPER_SILENCE;
|
||||
|
||||
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2) {
|
||||
receivingRxData = false; // force Stage2
|
||||
if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_STAGE2)) {
|
||||
// Aux switch set to failsafe stage2 emulates loss of signal without waiting
|
||||
failsafeOnValidDataFailed();
|
||||
receivingRxData = false;
|
||||
}
|
||||
|
||||
// Beep RX lost only if we are not seeing data and we have been armed earlier
|
||||
|
|
@ -232,12 +241,13 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
|||
if (THROTTLE_HIGH == calculateThrottleStatus()) {
|
||||
failsafeState.throttleLowPeriod = millis() + failsafeConfig()->failsafe_throttle_low_delay * MILLIS_PER_TENTH_SECOND;
|
||||
}
|
||||
// Kill switch logic (must be independent of receivingRxData to skip PERIOD_RXDATA_FAILURE delay before disarming)
|
||||
if (failsafeSwitchIsOn && failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL) {
|
||||
// KillswitchEvent: failsafe switch is configured as KILL switch and is switched ON
|
||||
if (failsafeSwitchIsOn && (failsafeConfig()->failsafe_switch_mode == FAILSAFE_SWITCH_MODE_KILL)) {
|
||||
// Failsafe switch is configured as KILL switch and is switched ON
|
||||
failsafeActivate();
|
||||
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_1_SECONDS; // require 1 seconds of valid rxData
|
||||
// Skip auto-landing procedure
|
||||
failsafeState.phase = FAILSAFE_LANDED;
|
||||
// Require 3 seconds of valid rxData
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
|
||||
reprocessState = true;
|
||||
} else if (!receivingRxData) {
|
||||
if (millis() > failsafeState.throttleLowPeriod
|
||||
|
|
@ -279,7 +289,8 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
|||
case FAILSAFE_PROCEDURE_DROP_IT:
|
||||
// Drop the craft
|
||||
failsafeActivate();
|
||||
failsafeState.phase = FAILSAFE_LANDED; // skip auto-landing procedure
|
||||
// skip auto-landing procedure
|
||||
failsafeState.phase = FAILSAFE_LANDED;
|
||||
break;
|
||||
#ifdef USE_GPS_RESCUE
|
||||
case FAILSAFE_PROCEDURE_GPS_RESCUE:
|
||||
|
|
@ -296,15 +307,16 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
|||
if (receivingRxData) {
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
if (armed) {
|
||||
failsafeApplyControlInput();
|
||||
beeperMode = BEEPER_RX_LOST_LANDING;
|
||||
}
|
||||
if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
|
||||
failsafeState.phase = FAILSAFE_LANDED;
|
||||
reprocessState = true;
|
||||
} else {
|
||||
if (armed) {
|
||||
beeperMode = BEEPER_RX_LOST_LANDING;
|
||||
}
|
||||
if (failsafeShouldHaveCausedLandingByNow() || crashRecoveryModeActive() || !armed) {
|
||||
// require 3 seconds of valid rxData
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
|
||||
failsafeState.phase = FAILSAFE_LANDED;
|
||||
reprocessState = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
#ifdef USE_GPS_RESCUE
|
||||
|
|
@ -314,19 +326,22 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
|||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
}
|
||||
if (armed) {
|
||||
failsafeApplyControlInput();
|
||||
beeperMode = BEEPER_RX_LOST_LANDING;
|
||||
} else {
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData
|
||||
failsafeState.phase = FAILSAFE_LANDED;
|
||||
reprocessState = true;
|
||||
if (armed) {
|
||||
ENABLE_FLIGHT_MODE(GPS_RESCUE_MODE);
|
||||
beeperMode = BEEPER_RX_LOST_LANDING;
|
||||
} else {
|
||||
// require 3 seconds of valid rxData
|
||||
failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_3_SECONDS;
|
||||
failsafeState.phase = FAILSAFE_LANDED;
|
||||
reprocessState = true;
|
||||
}
|
||||
}
|
||||
break;
|
||||
#endif
|
||||
case FAILSAFE_LANDED:
|
||||
setArmingDisabled(ARMING_DISABLED_FAILSAFE); // To prevent accidently rearming by an intermittent rx link
|
||||
// Prevent accidently rearming by an intermittent rx link
|
||||
setArmingDisabled(ARMING_DISABLED_FAILSAFE);
|
||||
disarm(DISARM_REASON_FAILSAFE);
|
||||
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
|
||||
|
|
@ -337,12 +352,9 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
|||
// Monitoring the rx link to allow rearming when it has become good for > `receivingRxDataPeriodPreset` time.
|
||||
if (receivingRxData) {
|
||||
if (millis() > failsafeState.receivingRxDataPeriod) {
|
||||
// rx link is good now, when arming via ARM switch, it must be OFF first
|
||||
if (!(!isUsingSticksForArming() && IS_RC_MODE_ACTIVE(BOXARM))) {
|
||||
unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
// rx link is good now
|
||||
failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED;
|
||||
reprocessState = true;
|
||||
}
|
||||
} else {
|
||||
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset;
|
||||
|
|
@ -357,6 +369,7 @@ FAST_CODE_NOINLINE void failsafeUpdateState(void)
|
|||
failsafeState.phase = FAILSAFE_IDLE;
|
||||
failsafeState.active = false;
|
||||
DISABLE_FLIGHT_MODE(FAILSAFE_MODE);
|
||||
unsetArmingDisabled(ARMING_DISABLED_FAILSAFE);
|
||||
reprocessState = true;
|
||||
break;
|
||||
|
||||
|
|
|
|||
|
|
@ -28,18 +28,17 @@
|
|||
#define PERIOD_OF_1_SECONDS 1 * MILLIS_PER_SECOND
|
||||
#define PERIOD_OF_3_SECONDS 3 * MILLIS_PER_SECOND
|
||||
#define PERIOD_OF_30_SECONDS 30 * MILLIS_PER_SECOND
|
||||
#define PERIOD_RXDATA_FAILURE 200 // millis
|
||||
#define PERIOD_RXDATA_FAILURE 10 // millis
|
||||
#define PERIOD_RXDATA_RECOVERY 200 // millis
|
||||
|
||||
|
||||
typedef struct failsafeConfig_s {
|
||||
uint16_t failsafe_throttle; // Throttle level used for landing - specify value between 1000..2000 (pwm pulse width for slightly below hover). center throttle = 1500.
|
||||
uint16_t failsafe_throttle_low_delay; // Time throttle stick must have been below 'min_check' to "JustDisarm" instead of "full failsafe procedure".
|
||||
uint8_t failsafe_delay; // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example (10)
|
||||
uint8_t failsafe_off_delay; // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example (200)
|
||||
uint8_t failsafe_switch_mode; // failsafe switch action is 0: stage1 (identical to rc link loss), 1: disarms instantly, 2: stage2
|
||||
uint8_t failsafe_switch_mode; // failsafe switch action is 0: Stage 1, 1: Disarms instantly, 2: Stage 2
|
||||
uint8_t failsafe_procedure; // selected full failsafe procedure is 0: auto-landing, 1: Drop it
|
||||
uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (plus 200ms) needed to allow recovering from failsafe procedure
|
||||
uint16_t failsafe_recovery_delay; // Time (in 0.1sec) of valid rx data (min 200ms PERIOD_RXDATA_RECOVERY) to allow recovering from failsafe procedure
|
||||
uint8_t failsafe_stick_threshold; // Stick deflection percentage to exit GPS Rescue procedure
|
||||
} failsafeConfig_t;
|
||||
|
||||
|
|
@ -106,6 +105,5 @@ bool failsafeIsReceivingRxData(void);
|
|||
void failsafeCheckDataFailurePeriod(void);
|
||||
void failsafeOnRxSuspend(uint32_t suspendPeriod);
|
||||
void failsafeOnRxResume(void);
|
||||
|
||||
void failsafeOnValidDataReceived(void);
|
||||
void failsafeOnValidDataFailed(void);
|
||||
|
|
|
|||
|
|
@ -67,6 +67,7 @@
|
|||
#if defined(USE_DYN_NOTCH_FILTER)
|
||||
#include "flight/dyn_notch_filter.h"
|
||||
#endif
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/position.h"
|
||||
|
|
@ -1037,6 +1038,7 @@ STATIC_UNIT_TESTED bool osdProcessStats1(timeUs_t currentTimeUs)
|
|||
resumeRefreshAt = osdShowArmed() + currentTimeUs;
|
||||
} else if (isSomeStatEnabled()
|
||||
&& !suppressStatsDisplay
|
||||
&& !failsafeIsActive()
|
||||
&& (!(getArmingDisableFlags() & (ARMING_DISABLED_RUNAWAY_TAKEOFF | ARMING_DISABLED_CRASH_DETECTED))
|
||||
|| !VISIBLE(osdElementConfig()->item_pos[OSD_WARNINGS]))) { // suppress stats if runaway takeoff triggered disarm and WARNINGS element is visible
|
||||
osdStatsEnabled = true;
|
||||
|
|
|
|||
117
src/main/rx/rx.c
117
src/main/rx/rx.c
|
|
@ -43,6 +43,7 @@
|
|||
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "fc/tasks.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
|
|
@ -105,17 +106,15 @@ static bool rxFlightChannelsValid = false;
|
|||
static bool rxIsInFailsafeMode = true;
|
||||
static uint8_t rxChannelCount;
|
||||
|
||||
static timeUs_t rxNextUpdateAtUs = 0;
|
||||
static uint32_t needRxSignalBefore = 0;
|
||||
static uint32_t needRxSignalMaxDelayUs;
|
||||
static uint32_t suspendRxSignalUntil = 0;
|
||||
static timeUs_t needRxSignalBefore = 0;
|
||||
static timeUs_t suspendRxSignalUntil = 0;
|
||||
static uint8_t skipRxSamples = 0;
|
||||
|
||||
static float rcRaw[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
|
||||
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
uint32_t validRxSignalTimeout[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
|
||||
#define MAX_INVALID_PULS_TIME 300
|
||||
#define MAX_INVALID_PULSE_TIME 300 // hold time in millisecons after bad channel or Rx link loss
|
||||
#define PPM_AND_PWM_SAMPLE_COUNT 3
|
||||
|
||||
#define DELAY_50_HZ (1000000 / 50)
|
||||
|
|
@ -284,11 +283,10 @@ void rxInit(void)
|
|||
rxRuntimeState.rcProcessFrameFn = nullProcessFrame;
|
||||
rxRuntimeState.lastRcFrameTimeUs = 0;
|
||||
rcSampleIndex = 0;
|
||||
needRxSignalMaxDelayUs = DELAY_10_HZ;
|
||||
|
||||
for (int i = 0; i < MAX_SUPPORTED_RC_CHANNEL_COUNT; i++) {
|
||||
rcData[i] = rxConfig()->midrc;
|
||||
rcInvalidPulsPeriod[i] = millis() + MAX_INVALID_PULS_TIME;
|
||||
validRxSignalTimeout[i] = millis() + MAX_INVALID_PULSE_TIME;
|
||||
}
|
||||
|
||||
rcData[THROTTLE] = (featureIsEnabled(FEATURE_3D)) ? rxConfig()->midrc : rxConfig()->rx_min_usec;
|
||||
|
|
@ -330,7 +328,6 @@ void rxInit(void)
|
|||
#ifdef USE_RX_MSP
|
||||
case RX_PROVIDER_MSP:
|
||||
rxMspInit(rxConfig(), &rxRuntimeState);
|
||||
needRxSignalMaxDelayUs = DELAY_5_HZ;
|
||||
|
||||
break;
|
||||
#endif
|
||||
|
|
@ -473,13 +470,17 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
|
|||
UNUSED(currentTimeUs);
|
||||
UNUSED(currentDeltaTimeUs);
|
||||
|
||||
return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired || !failsafeIsReceivingRxData();
|
||||
return taskUpdateRxMainInProgress() || rxDataProcessingRequired || auxiliaryProcessingRequired;
|
||||
}
|
||||
|
||||
FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs)
|
||||
FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs)
|
||||
{
|
||||
bool signalReceived = false;
|
||||
bool useDataDrivenProcessing = true;
|
||||
// Need the next packet in 2.5 x the anticipated time
|
||||
timeDelta_t needRxSignalMaxDelayUs = anticipatedDeltaTime10thUs * 2 / 8;
|
||||
|
||||
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 2, needRxSignalMaxDelayUs / 10);
|
||||
|
||||
if (taskUpdateRxMainInProgress()) {
|
||||
// No need to check for new data as a packet is being processed already
|
||||
|
|
@ -495,7 +496,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
|
|||
if (isPPMDataBeingReceived()) {
|
||||
signalReceived = true;
|
||||
rxIsInFailsafeMode = false;
|
||||
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
|
||||
resetPPMDataReceivedState();
|
||||
}
|
||||
|
||||
|
|
@ -504,7 +504,6 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
|
|||
if (isPWMDataBeingReceived()) {
|
||||
signalReceived = true;
|
||||
rxIsInFailsafeMode = false;
|
||||
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
|
||||
useDataDrivenProcessing = false;
|
||||
}
|
||||
|
||||
|
|
@ -519,13 +518,8 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
|
|||
rxIsInFailsafeMode = (frameStatus & RX_FRAME_FAILSAFE) != 0;
|
||||
bool rxFrameDropped = (frameStatus & RX_FRAME_DROPPED) != 0;
|
||||
signalReceived = !(rxIsInFailsafeMode || rxFrameDropped);
|
||||
if (signalReceived) {
|
||||
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
|
||||
}
|
||||
|
||||
setLinkQuality(signalReceived, currentDeltaTimeUs);
|
||||
}
|
||||
|
||||
if (frameStatus & RX_FRAME_PROCESSING_REQUIRED) {
|
||||
auxiliaryProcessingRequired = true;
|
||||
}
|
||||
|
|
@ -535,12 +529,19 @@ FAST_CODE_NOINLINE void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t current
|
|||
}
|
||||
|
||||
if (signalReceived) {
|
||||
rxSignalReceived = true;
|
||||
} else if (currentTimeUs >= needRxSignalBefore) {
|
||||
rxSignalReceived = false;
|
||||
needRxSignalBefore = currentTimeUs + needRxSignalMaxDelayUs;
|
||||
rxSignalReceived = true; // immediately process data
|
||||
} else {
|
||||
// no signal, wait 100ms and perform signal loss behaviour every 100ms
|
||||
if (cmpTimeUs(needRxSignalBefore, currentTimeUs) > (timeDelta_t)DELAY_10_HZ) {
|
||||
rxSignalReceived = false;
|
||||
// rcData[] needs to be processed to rcCommand
|
||||
rxDataProcessingRequired = true;
|
||||
needRxSignalBefore = currentTimeUs + (timeDelta_t)DELAY_10_HZ;
|
||||
}
|
||||
}
|
||||
|
||||
if ((signalReceived && useDataDrivenProcessing) || cmpTimeUs(currentTimeUs, rxNextUpdateAtUs) > 0) {
|
||||
if (signalReceived && useDataDrivenProcessing) {
|
||||
rxDataProcessingRequired = true;
|
||||
}
|
||||
}
|
||||
|
|
@ -645,50 +646,83 @@ static void readRxChannelsApplyRanges(void)
|
|||
static void detectAndApplySignalLossBehaviour(void)
|
||||
{
|
||||
const uint32_t currentTimeMs = millis();
|
||||
|
||||
const bool useValueFromRx = rxSignalReceived && !rxIsInFailsafeMode;
|
||||
const bool failsafeAuxSwitch = IS_RC_MODE_ACTIVE(BOXFAILSAFE);
|
||||
bool validRxPacket = rxSignalReceived && !failsafeAuxSwitch;
|
||||
// becomes false when a packet is bad or we use a failsafe switch - logged to blackbox
|
||||
rxFlightChannelsValid = true;
|
||||
// becomes false when one or more flight channels has bad Rx data for longer than hold time
|
||||
|
||||
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 0, rxSignalReceived);
|
||||
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 1, rxIsInFailsafeMode);
|
||||
|
||||
rxFlightChannelsValid = true;
|
||||
for (int channel = 0; channel < rxChannelCount; channel++) {
|
||||
float sample = rcRaw[channel];
|
||||
const bool thisChannelValid = validRxPacket && isPulseValid(sample); // for all or just one channel alone
|
||||
|
||||
const bool validPulse = useValueFromRx && isPulseValid(sample);
|
||||
|
||||
if (validPulse) {
|
||||
rcInvalidPulsPeriod[channel] = currentTimeMs + MAX_INVALID_PULS_TIME;
|
||||
if (thisChannelValid) {
|
||||
// reset invalid pulse period timer for each good-channel
|
||||
validRxSignalTimeout[channel] = currentTimeMs + MAX_INVALID_PULSE_TIME;
|
||||
}
|
||||
// set failsafe and hold values
|
||||
if (failsafeIsActive() && ARMING_FLAG(ARMED)) {
|
||||
// STAGE 2 failsafe is active, and armed. Apply failsafe values until failsafe ends or disarmed
|
||||
if (channel < NON_AUX_CHANNEL_COUNT) {
|
||||
if (channel == THROTTLE ) {
|
||||
sample = failsafeConfig()->failsafe_throttle;
|
||||
} else {
|
||||
sample = rxConfig()->midrc;
|
||||
}
|
||||
} else if (!failsafeAuxSwitch) {
|
||||
// Aux channels as Set in Configurator, unless failsafe initiated by switch
|
||||
sample = getRxfailValue(channel);
|
||||
}
|
||||
} else {
|
||||
if (cmp32(currentTimeMs, rcInvalidPulsPeriod[channel]) < 0) {
|
||||
continue; // skip to next channel to hold channel value MAX_INVALID_PULS_TIME
|
||||
} else {
|
||||
sample = getRxfailValue(channel); // after that apply rxfail value
|
||||
if (channel < NON_AUX_CHANNEL_COUNT) {
|
||||
rxFlightChannelsValid = false;
|
||||
if (!thisChannelValid) {
|
||||
if (cmp32(currentTimeMs, validRxSignalTimeout[channel]) < 0) {
|
||||
// HOLD PERIOD for any invalid channels
|
||||
} else {
|
||||
// STAGE 1 failsafe settings apply to any channels invalid for more than hold time
|
||||
if (channel < NON_AUX_CHANNEL_COUNT) {
|
||||
rxFlightChannelsValid = false;
|
||||
sample = getRxfailValue(channel);
|
||||
// affected RPYT values will be set as per Stage 1 Configurator settings
|
||||
} else if (!failsafeAuxSwitch) {
|
||||
// Aux channels as Set in Configurator, unless failsafe initiated by switch
|
||||
sample = getRxfailValue(channel);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#if defined(USE_PWM) || defined(USE_PPM)
|
||||
if (rxRuntimeState.rxProvider == RX_PROVIDER_PARALLEL_PWM || rxRuntimeState.rxProvider == RX_PROVIDER_PPM) {
|
||||
// smooth output for PWM and PPM
|
||||
// smooth output for PWM and PPM using moving average
|
||||
rcData[channel] = calculateChannelMovingAverage(channel, sample);
|
||||
} else
|
||||
#endif
|
||||
{
|
||||
// set rcData to either clean raw incoming values, or failsafe values
|
||||
rcData[channel] = sample;
|
||||
}
|
||||
}
|
||||
|
||||
if (rxFlightChannelsValid && !IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
|
||||
if (!rxFlightChannelsValid) {
|
||||
// show RXLOSS in OSD if any RPYT channel, or any packets, are lost for more than hold time
|
||||
setArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
|
||||
}
|
||||
|
||||
if (validRxPacket && rxFlightChannelsValid) {
|
||||
// failsafe switches are off, packet is good, no invalid channel for more than hold time
|
||||
// --> start the timer to exit stage 2 failsafe
|
||||
unsetArmingDisabled(ARMING_DISABLED_RX_FAILSAFE);
|
||||
failsafeOnValidDataReceived();
|
||||
} else {
|
||||
rxIsInFailsafeMode = true;
|
||||
// failsafe switch is on, or a bad packet, or at least one invalid channel for more than hold time
|
||||
// -> start timer to enter stage2 failsafe
|
||||
// note stage 2 onset will be delayed by hold time if we only have some bad channels in otherwise good packets
|
||||
failsafeOnValidDataFailed();
|
||||
for (int channel = 0; channel < rxChannelCount; channel++) {
|
||||
rcData[channel] = getRxfailValue(channel);
|
||||
}
|
||||
}
|
||||
|
||||
DEBUG_SET(DEBUG_RX_SIGNAL_LOSS, 3, rcData[THROTTLE]);
|
||||
}
|
||||
|
||||
|
|
@ -703,7 +737,6 @@ bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)
|
|||
}
|
||||
|
||||
rxDataProcessingRequired = false;
|
||||
rxNextUpdateAtUs = currentTimeUs + DELAY_15_HZ;
|
||||
|
||||
// only proceed when no more samples to skip and suspend period is over
|
||||
if (skipRxSamples || currentTimeUs <= suspendRxSignalUntil) {
|
||||
|
|
|
|||
|
|
@ -177,7 +177,7 @@ extern rxRuntimeState_t rxRuntimeState; //!!TODO remove this extern, only needed
|
|||
void rxInit(void);
|
||||
void rxProcessPending(bool state);
|
||||
bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
|
||||
void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs);
|
||||
void rxFrameCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTimeUs, timeDelta_t anticipatedDeltaTime10thUs);
|
||||
bool rxIsReceivingSignal(void);
|
||||
bool rxAreFlightChannelsValid(void);
|
||||
bool calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs);
|
||||
|
|
|
|||
|
|
@ -494,7 +494,7 @@ FAST_CODE void scheduler(void)
|
|||
}
|
||||
DEBUG_SET(DEBUG_SCHEDULER_DETERMINISM, 0, clockCyclesTo10thMicros(cmpTimeCycles(nowCycles, lastTargetCycles)));
|
||||
#endif
|
||||
currentTimeUs = clockCyclesToMicros(nowCycles);
|
||||
currentTimeUs = micros();
|
||||
taskExecutionTimeUs += schedulerExecuteTask(gyroTask, currentTimeUs);
|
||||
|
||||
if (gyroFilterReady()) {
|
||||
|
|
@ -504,15 +504,15 @@ FAST_CODE void scheduler(void)
|
|||
taskExecutionTimeUs += schedulerExecuteTask(getTask(TASK_PID), currentTimeUs);
|
||||
}
|
||||
if (rxFrameReady()) {
|
||||
// Check to for incoming RX data. Don't do this in the checker as that is called repeatedly within
|
||||
// Check for incoming RX data. Don't do this in the checker as that is called repeatedly within
|
||||
// a given gyro loop, and ELRS takes a long time to process this and so can only be safely processed
|
||||
// before the checkers
|
||||
rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs));
|
||||
rxFrameCheck(currentTimeUs, cmpTimeUs(currentTimeUs, getTask(TASK_RX)->lastExecutedAtUs), getTask(TASK_RX)->movingSumDeltaTime10thUs / TASK_STATS_MOVING_SUM_COUNT);
|
||||
}
|
||||
|
||||
// Check for failsafe conditions without reliance on the RX task being well behaved
|
||||
if (cmp32(millis(), lastFailsafeCheckMs) > PERIOD_RXDATA_FAILURE) {
|
||||
// This is very low cost taking less that 4us every 200ms
|
||||
// This is very low cost taking less that 4us every 10ms
|
||||
failsafeCheckDataFailurePeriod();
|
||||
failsafeUpdateState();
|
||||
lastFailsafeCheckMs = millis();
|
||||
|
|
|
|||
|
|
@ -28,7 +28,7 @@
|
|||
#define TASK_PERIOD_MS(ms) ((ms) * 1000)
|
||||
#define TASK_PERIOD_US(us) (us)
|
||||
|
||||
#define TASK_STATS_MOVING_SUM_COUNT 64
|
||||
#define TASK_STATS_MOVING_SUM_COUNT 8
|
||||
|
||||
#define LOAD_PERCENTAGE_ONE 100
|
||||
|
||||
|
|
|
|||
|
|
@ -334,7 +334,7 @@ uint32_t millis(void) {
|
|||
return millis64() & 0xFFFFFFFF;
|
||||
}
|
||||
|
||||
uint32_t clockCyclesToMicros(uint32_t clockCycles)
|
||||
int32_t clockCyclesToMicros(int32_t clockCycles)
|
||||
{
|
||||
return clockCycles;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -183,7 +183,7 @@ TEST(FlightFailsafeTest, TestFailsafeDetectsRxLossAndStartsLanding)
|
|||
failsafeOnValidDataReceived(); // set last valid sample at current time
|
||||
|
||||
// when
|
||||
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(PERIOD_RXDATA_FAILURE + failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) {
|
||||
for (sysTickUptime = 0; sysTickUptime < (uint32_t)(failsafeConfig()->failsafe_delay * MILLIS_PER_TENTH_SECOND); sysTickUptime++) {
|
||||
failsafeOnValidDataFailed();
|
||||
|
||||
failsafeUpdateState();
|
||||
|
|
@ -237,7 +237,7 @@ TEST(FlightFailsafeTest, TestFailsafeCausesLanding)
|
|||
EXPECT_TRUE(isArmingDisabled());
|
||||
|
||||
// given
|
||||
sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time
|
||||
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
|
||||
failsafeOnValidDataReceived();
|
||||
|
||||
// when
|
||||
|
|
@ -361,7 +361,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeKill)
|
|||
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
||||
|
||||
// given
|
||||
sysTickUptime += PERIOD_OF_1_SECONDS + 1; // adjust time to point just past the required additional recovery time
|
||||
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
|
||||
failsafeOnValidDataReceived();
|
||||
|
||||
// when
|
||||
|
|
@ -435,8 +435,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
|
|||
failsafeUpdateState(); // should activate stage2 immediately
|
||||
|
||||
// then
|
||||
EXPECT_TRUE(failsafeIsActive());
|
||||
EXPECT_TRUE(isArmingDisabled());
|
||||
EXPECT_TRUE(failsafeIsActive()); // stick induced failsafe allows re-arming
|
||||
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
||||
EXPECT_EQ(FAILSAFE_LANDING, failsafePhase());
|
||||
|
||||
|
|
@ -456,7 +455,7 @@ TEST(FlightFailsafeTest, TestFailsafeSwitchModeStage2Land)
|
|||
EXPECT_EQ(FAILSAFE_RX_LOSS_MONITORING, failsafePhase());
|
||||
|
||||
// given
|
||||
sysTickUptime += PERIOD_OF_30_SECONDS + 1; // adjust time to point just past the required additional recovery time
|
||||
sysTickUptime += PERIOD_OF_3_SECONDS + 1; // adjust time to point just past the required additional recovery time
|
||||
|
||||
// and
|
||||
deactivateBoxFailsafe();
|
||||
|
|
@ -497,7 +496,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
|
|||
sysTickUptime = 0; // restart time from 0
|
||||
failsafeOnValidDataReceived(); // set last valid sample at current time
|
||||
|
||||
// when
|
||||
// enter stage 1 failsafe
|
||||
for (sysTickUptime = 0; sysTickUptime < PERIOD_RXDATA_FAILURE; sysTickUptime++) {
|
||||
failsafeOnValidDataFailed();
|
||||
|
||||
|
|
@ -520,10 +519,20 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
|
|||
EXPECT_FALSE(failsafeIsActive());
|
||||
EXPECT_EQ(FAILSAFE_IDLE, failsafePhase());
|
||||
EXPECT_EQ(0, CALL_COUNTER(COUNTER_MW_DISARM));
|
||||
EXPECT_TRUE(isArmingDisabled());
|
||||
EXPECT_FALSE(isArmingDisabled()); // arming not blocked in stage 1
|
||||
|
||||
// given
|
||||
// enough valid data is received
|
||||
// add enough time to enter stage 2
|
||||
sysTickUptime += failsafeConfig()->failsafe_off_delay * MILLIS_PER_TENTH_SECOND + 1;
|
||||
failsafeOnValidDataFailed();
|
||||
|
||||
// when
|
||||
failsafeUpdateState();
|
||||
|
||||
// then
|
||||
EXPECT_TRUE(isArmingDisabled()); // arming blocked until recovery time
|
||||
EXPECT_FALSE(failsafeIsActive()); // failsafe is still not active
|
||||
|
||||
// given valid data is received for the recovery time, allow re-arming
|
||||
uint32_t sysTickTarget = sysTickUptime + PERIOD_RXDATA_RECOVERY;
|
||||
for (; sysTickUptime < sysTickTarget; sysTickUptime++) {
|
||||
failsafeOnValidDataReceived();
|
||||
|
|
@ -546,6 +555,7 @@ extern "C" {
|
|||
float rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
|
||||
float rcCommand[4];
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
uint8_t debugMode = 0;
|
||||
bool isUsingSticksToArm = true;
|
||||
|
||||
PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
|
||||
|
|
|
|||
|
|
@ -45,6 +45,7 @@ extern "C" {
|
|||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
|
||||
#include "flight/failsafe.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/mixer.h"
|
||||
#include "flight/pid.h"
|
||||
|
|
@ -86,6 +87,7 @@ extern "C" {
|
|||
PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0);
|
||||
PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0);
|
||||
PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0);
|
||||
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
|
||||
|
||||
timeUs_t simulationTime = 0;
|
||||
|
||||
|
|
@ -502,6 +504,7 @@ extern "C" {
|
|||
void persistentObjectWrite(persistentObjectId_e, uint32_t) {}
|
||||
void failsafeOnRxSuspend(uint32_t ) {}
|
||||
void failsafeOnRxResume(void) {}
|
||||
uint32_t failsafeFailurePeriodMs(void) { return 400; }
|
||||
void featureDisableImmediate(uint32_t) { }
|
||||
bool rxMspFrameComplete(void) { return false; }
|
||||
bool isPPMDataBeingReceived(void) { return false; }
|
||||
|
|
|
|||
|
|
@ -25,11 +25,13 @@ extern "C" {
|
|||
#include "build/debug.h"
|
||||
#include "drivers/io.h"
|
||||
#include "common/maths.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "pg/pg.h"
|
||||
#include "pg/pg_ids.h"
|
||||
#include "pg/rx.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "rx/rx.h"
|
||||
}
|
||||
|
||||
|
|
@ -39,10 +41,12 @@ extern "C" {
|
|||
extern "C" {
|
||||
|
||||
PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
|
||||
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
|
||||
|
||||
boxBitmask_t rcModeActivationMask;
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
uint8_t debugMode = 0;
|
||||
uint8_t armingFlags = 0;
|
||||
|
||||
extern float applyRxChannelRangeConfiguraton(float sample, const rxChannelRangeConfig_t *range);
|
||||
}
|
||||
|
|
@ -107,8 +111,11 @@ extern "C" {
|
|||
|
||||
void failsafeOnRxSuspend(uint32_t ) {}
|
||||
void failsafeOnRxResume(void) {}
|
||||
bool failsafeIsActive(void) { return false; }
|
||||
bool failsafeIsReceivingRxData(void) { return true; }
|
||||
bool taskUpdateRxMainInProgress() { return true; }
|
||||
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
|
||||
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
|
||||
|
||||
uint32_t micros(void) { return 0; }
|
||||
uint32_t millis(void) { return 0; }
|
||||
|
|
@ -225,6 +232,11 @@ void failsafeOnValidDataFailed(void)
|
|||
{
|
||||
}
|
||||
|
||||
uint32_t failsafeFailurePeriodMs(void)
|
||||
{
|
||||
return 400;
|
||||
}
|
||||
|
||||
float pt1FilterGain(float f_cut, float dT)
|
||||
{
|
||||
UNUSED(f_cut);
|
||||
|
|
|
|||
|
|
@ -27,6 +27,8 @@ extern "C" {
|
|||
#include "build/debug.h"
|
||||
#include "drivers/io.h"
|
||||
#include "fc/rc_controls.h"
|
||||
#include "fc/runtime_config.h"
|
||||
#include "flight/failsafe.h"
|
||||
#include "rx/rx.h"
|
||||
#include "fc/rc_modes.h"
|
||||
#include "common/maths.h"
|
||||
|
|
@ -39,6 +41,7 @@ extern "C" {
|
|||
boxBitmask_t rcModeActivationMask;
|
||||
int16_t debug[DEBUG16_VALUE_COUNT];
|
||||
uint8_t debugMode = 0;
|
||||
uint8_t armingFlags = 0;
|
||||
|
||||
bool isPulseValid(uint16_t pulseDuration);
|
||||
|
||||
|
|
@ -47,6 +50,7 @@ extern "C" {
|
|||
);
|
||||
|
||||
PG_REGISTER(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
|
||||
PG_REGISTER(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 0);
|
||||
}
|
||||
|
||||
#include "unittest_macros.h"
|
||||
|
|
@ -210,7 +214,9 @@ extern "C" {
|
|||
|
||||
void failsafeOnRxSuspend(uint32_t ) {}
|
||||
void failsafeOnRxResume(void) {}
|
||||
bool failsafeIsActive(void) { return false; }
|
||||
bool failsafeIsReceivingRxData(void) { return true; }
|
||||
uint32_t failsafeFailurePeriodMs(void) { return 400; }
|
||||
|
||||
uint32_t micros(void) { return 0; }
|
||||
uint32_t millis(void) { return 0; }
|
||||
|
|
@ -236,6 +242,8 @@ extern "C" {
|
|||
void xBusInit(const rxConfig_t *, rxRuntimeState_t *) {}
|
||||
void rxMspInit(const rxConfig_t *, rxRuntimeState_t *) {}
|
||||
void rxPwmInit(const rxConfig_t *, rxRuntimeState_t *) {}
|
||||
void setArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
|
||||
void unsetArmingDisabled(armingDisableFlags_e flag) { UNUSED(flag); }
|
||||
bool taskUpdateRxMainInProgress() { return true; }
|
||||
float pt1FilterGain(float f_cut, float dT)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -74,7 +74,7 @@ extern "C" {
|
|||
uint32_t simulatedTime = 0;
|
||||
uint32_t micros(void) { return simulatedTime; }
|
||||
uint32_t millis(void) { return simulatedTime/1000; } // Note simplistic mapping suitable only for short unit tests
|
||||
uint32_t clockCyclesToMicros(uint32_t x) { return x/10;}
|
||||
int32_t clockCyclesToMicros(int32_t x) { return x/10;}
|
||||
int32_t clockCyclesTo10thMicros(int32_t x) { return x;}
|
||||
uint32_t clockMicrosToCycles(uint32_t x) { return x*10;}
|
||||
uint32_t getCycleCounter(void) {return simulatedTime * 10;}
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue