rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3017 from rav-rav/sharper_rc_response 

Sharper rc response / ninja-mode
This commit is contained in:
Martin Budden 2017-05-08 08:03:21 +01:00 committed by GitHub
parent 2c5deb8198 001fb8d432
commit ebbfda2f6c
12 changed files with 41 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
src/main/fc/fc_rc.c
View File

@ -173,11 +173,11 @@ static void scaleRcCommandToFpvCamAngle(void) {
void processRcCommand(void)
{
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
static int16_t factor, rcInterpolationFactor;
static float rcCommandInterp[4] = { 0, 0, 0, 0 };
static float rcStepSize[4] = { 0, 0, 0, 0 };
static int16_t rcInterpolationStepCount;
static uint16_t currentRxRefreshRate;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2;
const uint8_t interpolationChannels = rxConfig()->rcInterpolationChannels + 2; //"RP", "RPY", "RPYT"
uint16_t rxRefreshRate;
bool readyToCalculateRate = false;
uint8_t readyToCalculateRateAxisCnt = 0;
@ -204,36 +204,34 @@ void processRcCommand(void)
rxRefreshRate = rxGetRefreshRate();
}
if (isRXDataNew) {
rcInterpolationFactor = rxRefreshRate / targetPidLooptime + 1;
if (debugMode == DEBUG_RC_INTERPOLATION) {
for(int axis = 0; axis < 2; axis++) debug[axis] = rcCommand[axis];
debug[3] = rxRefreshRate;
}
if (isRXDataNew && rxRefreshRate > 0) {
rcInterpolationStepCount = rxRefreshRate / targetPidLooptime;
for (int channel=ROLL; channel < interpolationChannels; channel++) {
deltaRC[channel] = rcCommand[channel] - (lastCommand[channel] - deltaRC[channel] * factor / rcInterpolationFactor);
lastCommand[channel] = rcCommand[channel];
rcStepSize[channel] = (rcCommand[channel] - rcCommandInterp[channel]) / (float)rcInterpolationStepCount;
}
factor = rcInterpolationFactor - 1;
if (debugMode == DEBUG_RC_INTERPOLATION) {
debug[0] = lrintf(rcCommand[0]);
debug[1] = lrintf(getTaskDeltaTime(TASK_RX) / 1000);
//debug[1] = lrintf(rcCommandInterp[0]);
//debug[1] = lrintf(rcStepSize[0]*100);
}
} else {
factor--;
rcInterpolationStepCount--;
}
// Interpolate steps of rcCommand
if (factor > 0) {
if (rcInterpolationStepCount > 0) {
for (int channel=ROLL; channel < interpolationChannels; channel++) {
rcCommand[channel] = lastCommand[channel] - deltaRC[channel] * factor/rcInterpolationFactor;
readyToCalculateRateAxisCnt = MAX(channel,FD_YAW); // throttle channel doesn't require rate calculation
readyToCalculateRate = true;
rcCommandInterp[channel] += rcStepSize[channel];
rcCommand[channel] = rcCommandInterp[channel];
readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation
}
} else {
factor = 0;
readyToCalculateRate = true;
}
} else {
factor = 0; // reset factor in case of level modes flip flopping
rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping
}
if (readyToCalculateRate || isRXDataNew) {
@ -243,6 +241,10 @@ void processRcCommand(void)
for (int axis = 0; axis <= readyToCalculateRateAxisCnt; axis++)
calculateSetpointRate(axis);
if (debugMode == DEBUG_RC_INTERPOLATION) {
debug[2] = rcInterpolationStepCount;
debug[3] = setpointRate[0];
}
// Scaling of AngleRate to camera angle (Mixing Roll and Yaw)
if (rxConfig()->fpvCamAngleDegrees && IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX) && !FLIGHT_MODE(HEADFREE_MODE))
scaleRcCommandToFpvCamAngle();
@ -311,7 +313,7 @@ void updateRcCommands(void)
const float radDiff = degreesToRadians(DECIDEGREES_TO_DEGREES(attitude.values.yaw) - headFreeModeHold);
const float cosDiff = cos_approx(radDiff);
const float sinDiff = sin_approx(radDiff);
const int16_t rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
const float rcCommand_PITCH = rcCommand[PITCH] * cosDiff + rcCommand[ROLL] * sinDiff;
rcCommand[ROLL] = rcCommand[ROLL] * cosDiff - rcCommand[PITCH] * sinDiff;
rcCommand[PITCH] = rcCommand_PITCH;
}

2
src/main/fc/rc_controls.c
View File

@ -65,7 +65,7 @@ static pidProfile_t *pidProfile;
// true if arming is done via the sticks (as opposed to a switch)
static bool isUsingSticksToArm = true;
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
float rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
uint32_t rcModeActivationMask; // one bit per mode defined in boxId_e

2
src/main/fc/rc_controls.h
View File

@ -151,7 +151,7 @@ typedef struct modeActivationProfile_s {
#define IS_RANGE_USABLE(range) ((range)->startStep < (range)->endStep)
extern int16_t rcCommand[4];
extern float rcCommand[4];
typedef struct rcControlsConfig_s {
uint8_t deadband; // introduce a deadband around the stick center for pitch and roll axis. Must be greater than zero.

11
src/main/flight/pid.c
View File

@ -421,13 +421,18 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
// -----calculate D component
if (axis != FD_YAW) {
// apply filters
float gyroRateFiltered = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate);
gyroRateFiltered = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateFiltered);
float dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
const float rD = dynC * currentPidSetpoint - gyroRateFiltered; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
// if crash recovery is on check for a crash
@ -441,10 +446,6 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
}
}
// apply filters
delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta);
delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta);
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
}

2
src/test/unit/altitude_hold_unittest.cc.txt
View File

@ -133,7 +133,7 @@ TEST(AltitudeHoldTest, TestCalculateTiltAngle)
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t accTimeSum ; // keep track for integration of acc

2
src/test/unit/battery_unittest.cc.txt
View File

@ -270,7 +270,7 @@ TEST(BatteryTest, RollOverPattern2)
extern "C" {
uint8_t armingFlags = 0;
int16_t rcCommand[4] = {0,0,0,0};
float rcCommand[4] = {0,0,0,0};
bool feature(uint32_t mask)

2
src/test/unit/flight_failsafe_unittest.cc.txt
View File

@ -406,7 +406,7 @@ TEST(FlightFailsafeTest, TestFailsafeNotActivatedWhenDisarmedAndRXLossIsDetected
extern "C" {
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint8_t armingFlags;
int16_t rcCommand[4];
float rcCommand[4];
uint32_t rcModeActivationMask;
int16_t debug[DEBUG16_VALUE_COUNT];
bool isUsingSticksToArm = true;

2
src/test/unit/flight_imu_unittest.cc.txt
View File

@ -76,7 +76,7 @@ TEST(FlightImuTest, TestCalculateHeading)
extern "C" {
uint32_t rcModeActivationMask;
int16_t rcCommand[4];
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
acc_t acc;

2
src/test/unit/flight_mixer_unittest.cc.txt
View File

@ -380,7 +380,7 @@ rollAndPitchInclination_t inclination;
rxRuntimeConfig_t rxRuntimeConfig;
int16_t axisPID[XYZ_AXIS_COUNT];
int16_t rcCommand[4];
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t rcModeActivationMask;

2
src/test/unit/ledstrip_unittest.cc.txt
View File

@ -351,7 +351,7 @@ extern "C" {
uint8_t armingFlags = 0;
uint16_t flightModeFlags = 0;
int16_t rcCommand[4];
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint32_t rcModeActivationMask;

2
src/test/unit/rx_ibus_unittest.cc
View File

@ -38,7 +38,7 @@ extern "C" {
extern "C" {
uint8_t batteryCellCount = 3;
int16_t rcCommand[4] = {0, 0, 0, 0};
float rcCommand[4] = {0, 0, 0, 0};
int16_t telemTemperature1 = 0;
baro_t baro = { .baroTemperature = 50 };
telemetryConfig_t telemetryConfig_System;

2
src/test/unit/telemetry_ibus_unittest.cc
View File

@ -38,7 +38,7 @@ extern "C" {
extern "C" {
uint8_t testBatteryCellCount =3;
int16_t rcCommand[4] = {0, 0, 0, 0};
float rcCommand[4] = {0, 0, 0, 0};
telemetryConfig_t telemetryConfig_System;
}