1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00

First cut of allowing a 3POS switch on AUX3 to change rc rate.

rcRate is decreased when low, increased when HIGH and no change when the
switch is in the middle.

The rcRate ticks up/down at 0.01 every 500ms if the switch is left on.

If using a momentary switch and if you can toggle the switch between
middle and low or middle and high more frequently than 500ms then the
rate will be increased accordingly.  Similar to how a keyboard repeat
behaves.
This commit is contained in:
Dominic Clifton 2014-10-22 22:53:22 +01:00
parent fd32ad6fcb
commit 6d7035819f
7 changed files with 324 additions and 4 deletions

View file

@ -430,7 +430,7 @@ void activateConfig(void)
{
static imuRuntimeConfig_t imuRuntimeConfig;
generatePitchCurve(&currentProfile->controlRateConfig);
generatePitchRollCurve(&currentProfile->controlRateConfig);
generateThrottleCurve(&currentProfile->controlRateConfig, &masterConfig.escAndServoConfig);
useRcControlsConfig(currentProfile->modeActivationConditions);

View file

@ -27,6 +27,8 @@
#include "config/config.h"
#include "config/runtime_config.h"
#include "drivers/system.h"
#include "flight/flight.h"
#include "drivers/accgyro.h"
@ -39,7 +41,9 @@
#include "mw.h"
#include "rx/rx.h"
#include "io/escservo.h"
#include "io/rc_controls.h"
#include "io/rc_curves.h"
static bool isUsingSticksToArm = true;
@ -246,6 +250,92 @@ void updateActivatedModes(modeActivationCondition_t *modeActivationConditions)
}
}
uint32_t adjustmentFunctionStateMask = 0;
#define MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentFunction) adjustmentFunctionStateMask |= (1 << (adjustmentFunction - ADJUSTMENT_INDEX_OFFSET))
#define MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentFunction) adjustmentFunctionStateMask &= ~(1 << (adjustmentFunction - ADJUSTMENT_INDEX_OFFSET))
#define IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentFunction) (adjustmentFunctionStateMask & (1 << (adjustmentFunction - ADJUSTMENT_INDEX_OFFSET)))
typedef struct adjustmentConfig_s {
uint8_t auxChannelIndex;
uint8_t adjustmentFunction;
uint8_t step;
} adjustmentConfig_t;
static adjustmentConfig_t adjustmentConfigs[1] = {
{
.auxChannelIndex = AUX3 - NON_AUX_CHANNEL_COUNT,
.adjustmentFunction = ADJUSTMENT_RC_RATE,
.step = 1
}
};
void applyAdjustment(controlRateConfig_t *controlRateConfig, uint8_t adjustmentFunction, int step) {
int newValue;
switch(adjustmentFunction) {
case ADJUSTMENT_RC_RATE:
newValue = (int)controlRateConfig->rcRate8 + step;
controlRateConfig->rcRate8 = constrain(newValue, 0, 250); // FIXME magic numbers repeated in serial_cli.c
generatePitchRollCurve(controlRateConfig);
break;
default:
break;
};
}
#define RESET_FREQUENCY_2HZ (1000 / 2)
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig)
{
uint8_t adjustmentIndex;
static uint32_t timeoutAt = 0;
uint32_t now = millis();
int32_t signedDiff = now - timeoutAt;
bool canResetReadyStates = signedDiff >= 0L;
if (canResetReadyStates) {
timeoutAt = now + RESET_FREQUENCY_2HZ;
}
for (adjustmentIndex = 0; adjustmentIndex < ADJUSTMENT_COUNT; adjustmentIndex++) {
adjustmentConfig_t * adjustmentConfig = &adjustmentConfigs[adjustmentIndex];
uint8_t adjustmentFunction = adjustmentConfig->adjustmentFunction;
if (adjustmentFunction == ADJUSTMENT_NONE) {
continue;
}
if (canResetReadyStates) {
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentFunction);
}
uint8_t channelIndex = NON_AUX_CHANNEL_COUNT + adjustmentConfig->auxChannelIndex;
int step;
if (rcData[channelIndex] > rxConfig->maxcheck) {
step = adjustmentConfig->step;
} else if (rcData[channelIndex] < rxConfig->mincheck) {
step = 0 - adjustmentConfig->step;
} else {
// returning the switch to the middle immediately resets the ready state
MARK_ADJUSTMENT_FUNCTION_AS_READY(adjustmentFunction);
timeoutAt = now + RESET_FREQUENCY_2HZ;
continue;
}
if (IS_ADJUSTMENT_FUNCTION_BUSY(adjustmentFunction)) {
continue;
}
MARK_ADJUSTMENT_FUNCTION_AS_BUSY(adjustmentFunction);
applyAdjustment(controlRateConfig, adjustmentFunction, step);
}
}
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions)
{
uint8_t index;

View file

@ -127,6 +127,19 @@ bool areSticksInApModePosition(uint16_t ap_mode);
throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch);
void updateActivatedModes(modeActivationCondition_t *modeActivationConditions);
typedef enum {
ADJUSTMENT_NONE = 0,
ADJUSTMENT_RC_RATE
} adjustmentFunction_e;
#define ADJUSTMENT_COUNT 1
#define ADJUSTMENT_INDEX_OFFSET 1
void processRcAdjustments(controlRateConfig_t *controlRateConfig, rxConfig_t *rxConfig);
void useRcControlsConfig(modeActivationCondition_t *modeActivationConditions);

View file

@ -28,7 +28,7 @@ int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & R
int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchCurve(controlRateConfig_t *controlRateConfig)
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig)
{
uint8_t i;

View file

@ -22,5 +22,5 @@
extern int16_t lookupPitchRollRC[PITCH_LOOKUP_LENGTH]; // lookup table for expo & RC rate PITCH+ROLL
extern int16_t lookupThrottleRC[THROTTLE_LOOKUP_LENGTH]; // lookup table for expo & mid THROTTLE
void generatePitchCurve(controlRateConfig_t *controlRateConfig);
void generatePitchRollCurve(controlRateConfig_t *controlRateConfig);
void generateThrottleCurve(controlRateConfig_t *controlRateConfig, escAndServoConfig_t *escAndServoConfig);

View file

@ -496,6 +496,8 @@ void processRx(void)
updateActivatedModes(currentProfile->modeActivationConditions);
processRcAdjustments(&currentProfile->controlRateConfig, &masterConfig.rxConfig);
bool canUseHorizonMode = true;
if ((IS_RC_MODE_ACTIVE(BOXANGLE) || (feature(FEATURE_FAILSAFE) && failsafe->vTable->hasTimerElapsed())) && (sensors(SENSOR_ACC))) {

View file

@ -157,6 +157,221 @@ TEST(RcControlsTest, updateActivatedModesUsingValidAuxConfigurationAndRXValues)
}
}
enum {
COUNTER_GENERATE_PITCH_ROLL_CURVE = 0
};
#define CALL_COUNT_ITEM_COUNT 1
static int callCounts[CALL_COUNT_ITEM_COUNT];
#define CALL_COUNTER(item) (callCounts[item])
void generatePitchRollCurve(controlRateConfig_t *) {
callCounts[COUNTER_GENERATE_PITCH_ROLL_CURVE]++;
}
void resetCallCounters(void) {
memset(&callCounts, 0, sizeof(callCounts));
}
uint32_t fixedMillis = 0;
uint32_t millis(void) {
return fixedMillis;
}
#define DEFAULT_MIN_CHECK 1100
#define DEFAULT_MAX_CHECK 1900
rxConfig_t rxConfig;
extern uint32_t adjustmentFunctionStateMask;
TEST(RcControlsTest, processRcAdjustmentsSticksInMiddle)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rollPitchRate = 0,
.yawRate = 0,
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
// and
uint8_t index;
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
rcData[index] = PWM_RANGE_MIDDLE;
}
// and
resetCallCounters();
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 90);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 0);
EXPECT_EQ(adjustmentFunctionStateMask, 0);
}
TEST(RcControlsTest, processRcAdjustmentsWithRcRateFunctionSwitchUp)
{
// given
controlRateConfig_t controlRateConfig = {
.rcRate8 = 90,
.rcExpo8 = 0,
.thrMid8 = 0,
.thrExpo8 = 0,
.rollPitchRate = 0,
.yawRate = 0,
};
// and
memset(&rxConfig, 0, sizeof (rxConfig));
rxConfig.mincheck = DEFAULT_MIN_CHECK;
rxConfig.maxcheck = DEFAULT_MAX_CHECK;
// and
uint8_t index;
for (index = AUX1; index < MAX_SUPPORTED_RC_CHANNEL_COUNT; index++) {
rcData[index] = PWM_RANGE_MIDDLE;
}
// and
resetCallCounters();
// and
rcData[AUX3] = PWM_RANGE_MAX;
// and
uint32_t expectedAdjustmentFunctionStateMask =
(1 << (ADJUSTMENT_RC_RATE - ADJUSTMENT_INDEX_OFFSET));
// and
fixedMillis = 496;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 1);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
//
// now pretend a short amount of time has passed, but not enough time to allow the value to have been increased
//
// given
fixedMillis = 497;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
//
// moving the switch back to the middle should immediately reset the state flag without increasing the value
//
// given
rcData[AUX3] = PWM_RANGE_MIDDLE;
// and
fixedMillis = 498;
// and
expectedAdjustmentFunctionStateMask = adjustmentFunctionStateMask &
~(1 << (ADJUSTMENT_RC_RATE - ADJUSTMENT_INDEX_OFFSET));
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
EXPECT_EQ(controlRateConfig.rcRate8, 91);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
//
// flipping the switch again, before the state reset would have occurred, allows the value to be increased again
// given
rcData[AUX3] = PWM_RANGE_MAX;
// and
expectedAdjustmentFunctionStateMask =
(1 << (ADJUSTMENT_RC_RATE - ADJUSTMENT_INDEX_OFFSET));
// and
fixedMillis = 499;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 2);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
//
// leaving the switch up, after the original timer would have reset the state should now NOT cause
// the rate to increase, it should only increase after another 500ms from when the state was reset.
//
// given
fixedMillis = 500;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
//
// should still not be able to be increased
//
// given
fixedMillis = 997;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 92);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
//
// 500ms has now passed since the switch was returned to the middle, now that
// switch is still in the UP position after the timer has elapses it should
// be increased again.
//
// given
fixedMillis = 998;
// when
processRcAdjustments(&controlRateConfig, &rxConfig);
// then
EXPECT_EQ(controlRateConfig.rcRate8, 93);
EXPECT_EQ(CALL_COUNTER(COUNTER_GENERATE_PITCH_ROLL_CURVE), 3);
EXPECT_EQ(adjustmentFunctionStateMask, expectedAdjustmentFunctionStateMask);
}
void changeProfile(uint8_t) {}
void accSetCalibrationCycles(uint16_t) {}
void gyroSetCalibrationCycles(uint16_t) {}