Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 20:35:33 +03:00
Code Issues Wiki Activity

Fast stick commands (#4208)

Browse source 
Implemented time-based stick command handling
This commit is contained in:
jirif 2017-09-26 12:36:54 +02:00 • committed by Andrey Mironov
parent 368a079785
commit cbefe71a73
2 changed files with 49 additions and 40 deletions
Download patch file Download diff file Expand all files Collapse all files

78
src/main/fc/rc_controls.c
View file

@ -57,6 +57,7 @@
#include "sensors/acceleration.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "flight/pid.h"
#include "flight/navigation.h"
@ -118,11 +119,22 @@ throttleStatus_e calculateThrottleStatus(void)
return THROTTLE_HIGH;
}
#define ARM_DELAY_MS 500
#define STICK_DELAY_MS 50
#define STICK_AUTOREPEAT_MS 250
#define repeatAfter(t) { \
rcDelayMs -= (t); \
doNotRepeat = false; \
}
void processRcStickPositions(throttleStatus_e throttleStatus)
{
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint8_t rcSticks; // this hold sticks position for command combos
static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
// time the sticks are maintained
static int16_t rcDelayMs;
// hold sticks position for command combos
static uint8_t rcSticks;
// an extra guard for disarming through switch to prevent that one frame can disarm it
static uint8_t rcDisarmTicks;
static bool doNotRepeat;
uint8_t stTmp = 0;
int i;
@ -132,7 +144,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
}
#endif
// ------------------ STICKS COMMAND HANDLER --------------------
// checking sticks positions
for (i = 0; i < 4; i++) {
stTmp >>= 2;
@ -142,15 +153,16 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
stTmp |= 0x40; // check for MAX
}
if (stTmp == rcSticks) {
if (rcDelayCommand < 250)
rcDelayCommand++;
} else
rcDelayCommand = 0;
if (rcDelayMs <= INT16_MAX - (getTaskDeltaTime(TASK_SELF) / 1000))
rcDelayMs += getTaskDeltaTime(TASK_SELF) / 1000;
} else {
rcDelayMs = 0;
doNotRepeat = false;
}
rcSticks = stTmp;
// perform actions
if (!isUsingSticksToArm) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
// Arming via ARM BOX
@ -158,7 +170,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
} else {
// Disarming via ARM BOX
resetArmingDisabled();
if (ARMING_FLAG(ARMED) && rxIsReceivingSignal() && !failsafeIsActive() ) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) {
@ -170,31 +181,37 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
}
}
}
}
if (rcDelayCommand != 20) {
return;
}
if (isUsingSticksToArm) {
} else if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
doNotRepeat = true;
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
if (ARMING_FLAG(ARMED))
disarm();
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
rcDelayCommand = 0; // reset so disarm tone will repeat
repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
}
}
return;
} else if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
if (rcDelayMs >= ARM_DELAY_MS && !doNotRepeat) {
doNotRepeat = true;
if (!ARMING_FLAG(ARMED)) {
// Arm via YAW
tryArm();
} else {
resetArmingDisabled();
}
}
if (ARMING_FLAG(ARMED)) {
// actions during armed
return;
}
if (ARMING_FLAG(ARMED) || doNotRepeat || rcDelayMs <= STICK_DELAY_MS) {
return;
}
doNotRepeat = true;
// actions during not armed
i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_LO + ROL_CE) {
// GYRO calibration
@ -221,6 +238,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
}
// Multiple configuration profiles
i = 0;
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_LO) // ROLL left -> Profile 1
i = 1;
else if (rcSticks == THR_LO + YAW_LO + PIT_HI + ROL_CE) // PITCH up -> Profile 2
@ -236,18 +254,6 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
saveConfigAndNotify();
}
if (isUsingSticksToArm) {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
tryArm();
return;
} else {
resetArmingDisabled();
}
}
if (rcSticks == THR_HI + YAW_LO + PIT_LO + ROL_CE) {
// Calibrating Acc
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
@ -283,7 +289,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
}
if (shouldApplyRollAndPitchTrimDelta) {
applyAndSaveAccelerometerTrimsDelta(&accelerometerTrimsDelta);
rcDelayCommand = 0; // allow autorepetition
repeatAfter(STICK_AUTOREPEAT_MS);
return;
}

3
src/test/unit/rc_controls_unittest.cc
View file

@ -48,6 +48,8 @@ extern "C" {
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "scheduler/scheduler.h"
}
#include "unittest_macros.h"
@ -703,4 +705,5 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
rxRuntimeConfig_t rxRuntimeConfig;
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
void resetArmingDisabled(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 20000; }
}