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

Cleaned up the RX handler some.

Browse source
This commit is contained in:
mikeller 2020-03-08 17:19:09 +13:00
parent 6a6fd23742
commit d13e83b91b
10 changed files with 50 additions and 35 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/main/cli/cli.c
View file

@ -4670,7 +4670,10 @@ static void cliStatus(char *cmdline)
// Run status
const int gyroRate = getTaskDeltaTime(TASK_GYRO) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_GYRO)));
const int rxRate = currentRxRefreshRate == 0 ? 0 : (int)(1000000.0f / ((float)currentRxRefreshRate));
int rxRate = getCurrentRxRefreshRate();
if (rxRate != 0) {
rxRate = (int)(1000000.0f / ((float)rxRate));
}
const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM)));
cliPrintLinef("CPU:%d%%, cycle time: %d, GYRO rate: %d, RX rate: %d, System rate: %d",
constrain(averageSystemLoadPercent, 0, 100), getTaskDeltaTime(TASK_GYRO), gyroRate, rxRate, systemRate);

3
src/main/fc/core.c
View file

@ -149,7 +149,6 @@ static bool flipOverAfterCrashActive = false;
static timeUs_t disarmAt; // Time of automatic disarm when "Don't spin the motors when armed" is enabled and auto_disarm_delay is nonzero
bool isRXDataNew;
static int lastArmingDisabledReason = 0;
static timeUs_t lastDisarmTimeUs;
static int tryingToArm = ARMING_DELAYED_DISARMED;
@ -752,6 +751,8 @@ bool processRx(timeUs_t currentTimeUs)
return false;
}
updateRcRefreshRate(currentTimeUs);
// in 3D mode, we need to be able to disarm by switch at any time
if (featureIsEnabled(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))

2
src/main/fc/core.h
View file

@ -27,8 +27,6 @@
extern int16_t magHold;
#endif
extern bool isRXDataNew;
typedef struct throttleCorrectionConfig_s {
uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg
uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle.

45
src/main/fc/rc.c
View file

@ -33,8 +33,6 @@
#include "config/config.h"
#include "config/feature.h"
#include "drivers/time.h"
#include "fc/controlrate_profile.h"
#include "fc/core.h"
#include "fc/rc.h"
@ -71,7 +69,8 @@ static float setpointRate[3], rcDeflection[3], rcDeflectionAbs[3];
static float throttlePIDAttenuation;
static bool reverseMotors = false;
static applyRatesFn *applyRates;
uint16_t currentRxRefreshRate;
static uint16_t currentRxRefreshRate;
static bool isRxDataNew = false;
FAST_RAM_ZERO_INIT uint8_t interpolationChannels;
static FAST_RAM_ZERO_INIT uint32_t rcFrameNumber;
@ -330,7 +329,7 @@ static FAST_CODE uint8_t processRcInterpolation(void)
rxRefreshRate = rxGetRefreshRate();
}
if (isRXDataNew && rxRefreshRate > 0) {
if (isRxDataNew && rxRefreshRate > 0) {
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
for (int channel = 0; channel < PRIMARY_CHANNEL_COUNT; channel++) {
@ -364,6 +363,26 @@ static FAST_CODE uint8_t processRcInterpolation(void)
}
void updateRcRefreshRate(timeUs_t currentTimeUs)
{
static timeUs_t lastRxTimeUs;
timeDelta_t frameAgeUs;
timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) {
refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
}
}
lastRxTimeUs = currentTimeUs;
currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000);
}
uint16_t getCurrentRxRefreshRate(void)
{
return currentRxRefreshRate;
}
#ifdef USE_RC_SMOOTHING_FILTER
// Determine a cutoff frequency based on filter type and the calculated
// average rx frame time
@ -549,7 +568,7 @@ static FAST_CODE uint8_t processRcSmoothingFilter(void)
}
}
if (isRXDataNew) {
if (isRxDataNew) {
// store the new raw channel values
for (int i = 0; i < PRIMARY_CHANNEL_COUNT; i++) {
@ -655,16 +674,16 @@ FAST_CODE void processRcCommand(void)
{
uint8_t updatedChannel;
if (isRXDataNew) {
if (isRxDataNew) {
rcFrameNumber++;
}
if (isRXDataNew && pidAntiGravityEnabled()) {
if (isRxDataNew && pidAntiGravityEnabled()) {
checkForThrottleErrorResetState(currentRxRefreshRate);
}
#ifdef USE_INTERPOLATED_SP
if (isRXDataNew) {
if (isRxDataNew) {
for (int i = FD_ROLL; i <= FD_YAW; i++) {
oldRcCommand[i] = rcCommand[i];
const float rcCommandf = rcCommand[i] / 500.0f;
@ -687,8 +706,8 @@ FAST_CODE void processRcCommand(void)
break;
}
if (isRXDataNew || updatedChannel) {
const uint8_t maxUpdatedAxis = isRXDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
if (isRxDataNew || updatedChannel) {
const uint8_t maxUpdatedAxis = isRxDataNew ? FD_YAW : MIN(updatedChannel, FD_YAW); // throttle channel doesn't require rate calculation
#if defined(SIMULATOR_BUILD)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunsafe-loop-optimizations"
@ -708,13 +727,15 @@ FAST_CODE void processRcCommand(void)
}
}
if (isRXDataNew) {
isRXDataNew = false;
if (isRxDataNew) {
isRxDataNew = false;
}
}
FAST_CODE_NOINLINE void updateRcCommands(void)
{
isRxDataNew = true;
// PITCH & ROLL only dynamic PID adjustment, depending on throttle value
int32_t prop;
if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) {

6
src/main/fc/rc.h
View file

@ -20,6 +20,8 @@
#pragma once
#include "drivers/time.h"
#include "fc/rc_controls.h"
typedef enum {
@ -30,8 +32,6 @@ typedef enum {
INTERPOLATION_CHANNELS_RPT,
} interpolationChannels_e;
extern uint16_t currentRxRefreshRate;
#ifdef USE_RC_SMOOTHING_FILTER
#define RC_SMOOTHING_AUTO_FACTOR_MIN 0
#define RC_SMOOTHING_AUTO_FACTOR_MAX 50
@ -55,3 +55,5 @@ float getRawDeflection(int axis);
float applyCurve(int axis, float deflection);
uint32_t getRcFrameNumber();
float getRcCurveSlope(int axis, float deflection);
void updateRcRefreshRate(timeUs_t currentTimeUs);
uint16_t getCurrentRxRefreshRate(void);

19
src/main/fc/tasks.c
View file

@ -160,32 +160,19 @@ static void taskUpdateAccelerometer(timeUs_t currentTimeUs)
static void taskUpdateRxMain(timeUs_t currentTimeUs)
{
static timeUs_t lastRxTimeUs;
if (!processRx(currentTimeUs)) {
return;
}
timeDelta_t frameAgeUs;
timeDelta_t refreshRateUs = rxGetFrameDelta(&frameAgeUs);
if (!refreshRateUs || cmpTimeUs(currentTimeUs, lastRxTimeUs) <= frameAgeUs) {
if (!rxTryGetFrameDeltaOrZero(&refreshRateUs)) {
refreshRateUs = cmpTimeUs(currentTimeUs, lastRxTimeUs); // calculate a delta here if not supplied by the protocol
}
}
lastRxTimeUs = currentTimeUs;
currentRxRefreshRate = constrain(refreshRateUs, 1000, 30000);
isRXDataNew = true;
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
updateArmingStatus();
#ifdef USE_USB_CDC_HID
if (!ARMING_FLAG(ARMED)) {
sendRcDataToHid();
}
#endif
// updateRcCommands sets rcCommand, which is needed by updateAltHoldState and updateSonarAltHoldState
updateRcCommands();
updateArmingStatus();
}
#ifdef USE_BARO

2
src/main/flight/interpolated_setpoint.c
View file

@ -65,7 +65,7 @@ FAST_CODE_NOINLINE float interpolatedSpApply(int axis, bool newRcFrame, ffInterp
if (newRcFrame) {
float rawSetpoint = getRawSetpoint(axis);
const float rxInterval = currentRxRefreshRate * 1e-6f;
const float rxInterval = getCurrentRxRefreshRate() * 1e-6f;
const float rxRate = 1.0f / rxInterval;
float setpointSpeed = (rawSetpoint - prevRawSetpoint[axis]) * rxRate;
float setpointAcceleration = setpointSpeed - prevSetpointSpeed[axis];

1
src/test/unit/arming_prevention_unittest.cc
View file

@ -1102,4 +1102,5 @@ extern "C" {
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
void updateRcRefreshRate(timeUs_t) {};
}

1
src/test/unit/cli_unittest.cc
View file

@ -359,4 +359,5 @@ bool isModeActivationConditionConfigured(const modeActivationCondition_t *, cons
void delay(uint32_t) {}
displayPort_t *osdGetDisplayPort(osdDisplayPortDevice_e *) { return NULL; }
mcuTypeId_e getMcuTypeId(void) { return MCU_TYPE_UNKNOWN; }
uint16_t getCurrentRxRefreshRate(void) { return 0; }
}

1
src/test/unit/vtx_unittest.cc
View file

@ -187,4 +187,5 @@ extern "C" {
void blackboxLogEvent(FlightLogEvent, union flightLogEventData_u *) {};
void gyroFiltering(timeUs_t) {};
timeDelta_t rxGetFrameDelta(timeDelta_t *) { return 0; }
void updateRcRefreshRate(timeUs_t) {};
}