From 33043d79dca87621c387c0d90e0a54c86f6d3df9 Mon Sep 17 00:00:00 2001 From: rav Date: Thu, 4 May 2017 00:16:46 +0200 Subject: [PATCH 1/4] use floats for rc interpolation do not filter setpoint data --- src/main/fc/fc_rc.c | 44 +++++++++++++++++++++------------------ src/main/fc/rc_controls.c | 2 +- src/main/fc/rc_controls.h | 2 +- src/main/flight/pid.c | 11 +++++----- 4 files changed, 32 insertions(+), 27 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 3754a4e616..2dd0c7fb59 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -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; @@ -205,35 +205,35 @@ void processRcCommand(void) } 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; - } + 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 + rcCommandInterp[channel] += rcStepSize[channel]; + rcCommand[channel] = rcCommandInterp[channel]; + readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation readyToCalculateRate = true; } } else { - factor = 0; + rcInterpolationStepCount = 0; } } 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 +243,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 +315,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; } diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 74fc3200d1..c5a4f62ecb 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -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 diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index fe35b88e08..9be4198d2a 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -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. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c397ef900f..a36832c30a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -437,13 +437,18 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate D component if (axis != FD_YAW) { + // apply filters + float gyroRateD = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); + gyroRateD = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateD); + 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 - gyroRateD; // 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 @@ -457,10 +462,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; } From d4d59fc718a739e685e8925996f11365ae3ebd5d Mon Sep 17 00:00:00 2001 From: rav Date: Fri, 5 May 2017 08:22:31 +0200 Subject: [PATCH 2/4] fix unit tests --- src/test/unit/altitude_hold_unittest.cc.txt | 2 +- src/test/unit/battery_unittest.cc.txt | 2 +- src/test/unit/flight_failsafe_unittest.cc.txt | 2 +- src/test/unit/flight_imu_unittest.cc.txt | 2 +- src/test/unit/flight_mixer_unittest.cc.txt | 2 +- src/test/unit/ledstrip_unittest.cc.txt | 2 +- src/test/unit/rx_ibus_unittest.cc | 2 +- src/test/unit/telemetry_ibus_unittest.cc | 2 +- 8 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/test/unit/altitude_hold_unittest.cc.txt b/src/test/unit/altitude_hold_unittest.cc.txt index d3bd1c8560..863b6b9cdd 100644 --- a/src/test/unit/altitude_hold_unittest.cc.txt +++ b/src/test/unit/altitude_hold_unittest.cc.txt @@ -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 diff --git a/src/test/unit/battery_unittest.cc.txt b/src/test/unit/battery_unittest.cc.txt index 3834ff7540..d3a8cb889d 100644 --- a/src/test/unit/battery_unittest.cc.txt +++ b/src/test/unit/battery_unittest.cc.txt @@ -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) diff --git a/src/test/unit/flight_failsafe_unittest.cc.txt b/src/test/unit/flight_failsafe_unittest.cc.txt index 581f06a89a..c82c424b27 100644 --- a/src/test/unit/flight_failsafe_unittest.cc.txt +++ b/src/test/unit/flight_failsafe_unittest.cc.txt @@ -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; diff --git a/src/test/unit/flight_imu_unittest.cc.txt b/src/test/unit/flight_imu_unittest.cc.txt index c87e3d54b0..797c539b3f 100644 --- a/src/test/unit/flight_imu_unittest.cc.txt +++ b/src/test/unit/flight_imu_unittest.cc.txt @@ -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; diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index 9178976d8e..8a30af72fd 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -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; diff --git a/src/test/unit/ledstrip_unittest.cc.txt b/src/test/unit/ledstrip_unittest.cc.txt index 4e518163c3..2f53aad885 100644 --- a/src/test/unit/ledstrip_unittest.cc.txt +++ b/src/test/unit/ledstrip_unittest.cc.txt @@ -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; diff --git a/src/test/unit/rx_ibus_unittest.cc b/src/test/unit/rx_ibus_unittest.cc index c9a249968d..4537a82180 100644 --- a/src/test/unit/rx_ibus_unittest.cc +++ b/src/test/unit/rx_ibus_unittest.cc @@ -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; diff --git a/src/test/unit/telemetry_ibus_unittest.cc b/src/test/unit/telemetry_ibus_unittest.cc index 2dd03a924b..bdbe1b0199 100644 --- a/src/test/unit/telemetry_ibus_unittest.cc +++ b/src/test/unit/telemetry_ibus_unittest.cc @@ -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; } From d7c0fa4d6e9ac16f976206beebcf7848fa2eb490 Mon Sep 17 00:00:00 2001 From: rav Date: Fri, 5 May 2017 16:10:49 +0200 Subject: [PATCH 3/4] double semicolon --- src/main/fc/fc_rc.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index 2dd0c7fb59..f4bbb04b2e 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -174,7 +174,7 @@ static void scaleRcCommandToFpvCamAngle(void) { void processRcCommand(void) { static float rcCommandInterp[4] = { 0, 0, 0, 0 }; - static float rcStepSize[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; //"RP", "RPY", "RPYT" From 001fb8d4322ca32a41edfa91c0a314b56ad01d73 Mon Sep 17 00:00:00 2001 From: rav Date: Fri, 5 May 2017 17:04:27 +0200 Subject: [PATCH 4/4] make sure rxRefreshRate > 0 --- src/main/fc/fc_rc.c | 6 ++---- src/main/flight/pid.c | 6 +++--- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index f4bbb04b2e..2511fcfd5c 100755 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -204,7 +204,7 @@ void processRcCommand(void) rxRefreshRate = rxGetRefreshRate(); } - if (isRXDataNew) { + if (isRXDataNew && rxRefreshRate > 0) { rcInterpolationStepCount = rxRefreshRate / targetPidLooptime; for (int channel=ROLL; channel < interpolationChannels; channel++) { @@ -227,10 +227,8 @@ void processRcCommand(void) rcCommandInterp[channel] += rcStepSize[channel]; rcCommand[channel] = rcCommandInterp[channel]; readyToCalculateRateAxisCnt = MAX(channel, FD_YAW); // throttle channel doesn't require rate calculation - readyToCalculateRate = true; } - } else { - rcInterpolationStepCount = 0; + readyToCalculateRate = true; } } else { rcInterpolationStepCount = 0; // reset factor in case of level modes flip flopping diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a36832c30a..9b4b7d47ea 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -438,14 +438,14 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an // -----calculate D component if (axis != FD_YAW) { // apply filters - float gyroRateD = dtermNotchFilterApplyFn(dtermFilterNotch[axis], gyroRate); - gyroRateD = dtermLpfApplyFn(dtermFilterLpf[axis], gyroRateD); + 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 - gyroRateD; // 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;