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

Initial implementation of Runaway Takeoff Prevention (anti-taz) 

Detects runaway pidSum values on takeoff and auto-disarms to prevent the "Tasmanian Devil" caused by incorrect props, wrong motor order/direction, incorrect flight controller orientation, etc.  After a successful takeoff and normal flight is detected the feature is disabled for the remainder of the battery.
This commit is contained in:
Bruce Luckcuck 2018-01-10 21:08:24 -05:00
parent 52c629751c
commit a32b05c284
12 changed files with 222 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/build/debug.c
View File

@ -56,4 +56,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RANGEFINDER_QUALITY",
"LIDAR_TF",
"CORE_TEMP",
"RUNAWAY_TAKEOFF",
};

1
src/main/build/debug.h
View File

@ -74,6 +74,7 @@ typedef enum {
DEBUG_RANGEFINDER_QUALITY,
DEBUG_LIDAR_TF,
DEBUG_CORE_TEMP,
DEBUG_RUNAWAY_TAKEOFF,
DEBUG_COUNT
} debugType_e;

176
src/main/fc/fc_core.c
View File

@ -18,6 +18,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "platform.h"
@ -98,6 +99,20 @@ enum {
#define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
#ifdef USE_RUNAWAY_TAKEOFF
#define RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT 15 // 15% - minimum stick deflection during deactivation phase
#define RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT 100 // 10.0% - pidSum limit during deactivation phase
#define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE 0
#define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY 1
#define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY 2
#define DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME 3
#define DEBUG_RUNAWAY_TAKEOFF_TRUE 1
#define DEBUG_RUNAWAY_TAKEOFF_FALSE 0
#endif
#if defined(USE_GPS) || defined(USE_MAG)
int16_t magHold;
#endif
@ -109,6 +124,14 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
bool isRXDataNew;
static int lastArmingDisabledReason = 0;
#ifdef USE_RUNAWAY_TAKEOFF
static timeUs_t runawayTakeoffDeactivateUs = 0;
static timeUs_t runawayTakeoffAccumulatedUs = 0;
static bool runawayTakeoffCheckDisabled = false;
static timeUs_t runawayTakeoffTriggerUs = 0;
#endif
PG_REGISTER_WITH_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig, PG_THROTTLE_CORRECTION_CONFIG, 0);
PG_RESET_TEMPLATE(throttleCorrectionConfig_t, throttleCorrectionConfig,
@ -220,6 +243,12 @@ void updateArmingStatus(void)
&& !isModeActivationConditionPresent(BOX3DONASWITCH)
&& !(getArmingDisableFlags() & ~(ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE));
#ifdef USE_RUNAWAY_TAKEOFF
if (!IS_RC_MODE_ACTIVE(BOXARM)) {
unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
}
#endif
// If arming is disabled and the ARM switch is on
if (isArmingDisabled()
&& !ignoreGyro
@ -311,6 +340,12 @@ void tryArm(void)
#else
beeper(BEEPER_ARMING);
#endif
#ifdef USE_RUNAWAY_TAKEOFF
runawayTakeoffDeactivateUs = 0;
runawayTakeoffAccumulatedUs = 0;
runawayTakeoffTriggerUs = 0;
#endif
} else {
if (!isFirstArmingGyroCalibrationRunning()) {
int armingDisabledReason = ffs(getArmingDisableFlags());
@ -389,6 +424,54 @@ static bool canUpdateVTX(void)
}
#endif
#ifdef USE_RUNAWAY_TAKEOFF
// determine if the R/P/Y stick deflection exceeds the specified limit - integer math is good enough here.
bool areSticksActive(uint8_t stickPercentLimit)
{
for (int axis = FD_ROLL; axis <= FD_YAW; axis ++) {
const uint8_t deadband = axis == FD_YAW ? rcControlsConfig()->yaw_deadband : rcControlsConfig()->deadband;
uint8_t stickPercent = 0;
if ((rcData[axis] >= PWM_RANGE_MAX) || (rcData[axis] <= PWM_RANGE_MIN)) {
stickPercent = 100;
} else {
if (rcData[axis] > (rxConfig()->midrc + deadband)) {
stickPercent = ((rcData[axis] - rxConfig()->midrc - deadband) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - deadband);
} else if (rcData[axis] < (rxConfig()->midrc - deadband)) {
stickPercent = ((rxConfig()->midrc - deadband - rcData[axis]) * 100) / (rxConfig()->midrc - deadband - PWM_RANGE_MIN);
}
}
if (stickPercent >= stickPercentLimit) {
return true;
}
}
return false;
}
// calculate the throttle stick percent - integer math is good enough here.
uint8_t calculateThrottlePercent(void)
{
uint8_t ret = 0;
if (feature(FEATURE_3D)
&& !IS_RC_MODE_ACTIVE(BOX3DDISABLE)
&& !isModeActivationConditionPresent(BOX3DONASWITCH)) {
if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) {
ret = 100;
} else {
if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) {
ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle);
} else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) {
ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN);
}
}
} else {
ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100);
}
return ret;
}
#endif
/*
* processRx called from taskUpdateRxMain
*/
@ -436,6 +519,66 @@ bool processRx(timeUs_t currentTimeUs)
pidStabilisationState(PID_STABILISATION_ON);
}
#ifdef USE_RUNAWAY_TAKEOFF
// If runaway_takeoff_prevention is enabled, accumulate the amount of time that throttle
// is above runaway_takeoff_deactivate_throttle with the any of the R/P/Y sticks deflected
// to at least runaway_takeoff_stick_percent percent while the pidSum on all axis is kept low.
// Once the amount of accumulated time exceeds runaway_takeoff_deactivate_delay then disable
// prevention for the remainder of the battery.
if (ARMING_FLAG(ARMED)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& !STATE(FIXED_WING)) {
// Determine if we're in "flight"
// - motors running
// - throttle over runaway_takeoff_deactivate_throttle_percent
// - sticks are active and have deflection greater than runaway_takeoff_deactivate_stick_percent
// - pidSum on all axis is less then runaway_takeoff_deactivate_pidlimit
bool inStableFlight = false;
if (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (throttleStatus != THROTTLE_LOW)) { // are motors running?
if ((calculateThrottlePercent() >= pidConfig()->runaway_takeoff_deactivate_throttle)
&& areSticksActive(RUNAWAY_TAKEOFF_DEACTIVATE_STICK_PERCENT)
&& (fabsf(axisPIDSum[FD_PITCH]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(axisPIDSum[FD_ROLL]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)
&& (fabsf(axisPIDSum[FD_YAW]) < RUNAWAY_TAKEOFF_DEACTIVATE_PIDSUM_LIMIT)) {
inStableFlight = true;
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
}
}
// If we're in flight, then accumulate the time and deactivate once it exceeds runaway_takeoff_deactivate_delay milliseconds
if (inStableFlight) {
if (runawayTakeoffDeactivateUs == 0) {
runawayTakeoffDeactivateUs = currentTimeUs;
}
if ((cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) > (pidConfig()->runaway_takeoff_deactivate_delay * 1000)) {
runawayTakeoffCheckDisabled = true;
}
} else {
if (runawayTakeoffDeactivateUs != 0) {
runawayTakeoffAccumulatedUs += cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs);
}
runawayTakeoffDeactivateUs = 0;
}
if (runawayTakeoffDeactivateUs == 0) {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, runawayTakeoffAccumulatedUs / 1000);
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, (cmpTimeUs(currentTimeUs, runawayTakeoffDeactivateUs) + runawayTakeoffAccumulatedUs) / 1000);
}
} else {
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_DEACTIVATING_TIME, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
// When armed and motors aren't spinning, do beeps and then disarm
// board after delay so users without buzzer won't lose fingers.
// mixTable constrains motor commands, so checking throttleStatus is enough
@ -611,6 +754,39 @@ static void subTaskPidController(timeUs_t currentTimeUs)
// PID - note this is function pointer set by setPIDController()
pidController(currentPidProfile, &accelerometerConfig()->accelerometerTrims, currentTimeUs);
DEBUG_SET(DEBUG_PIDLOOP, 1, micros() - startTime);
#ifdef USE_RUNAWAY_TAKEOFF
// Check to see if runaway takeoff detection is active (anti-spaz) and the pidSum is over the threshold.
// If so, disarm for safety
if (ARMING_FLAG(ARMED)
&& !STATE(FIXED_WING)
&& pidConfig()->runaway_takeoff_prevention
&& !runawayTakeoffCheckDisabled
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
const float runawayTakeoffThreshold = pidConfig()->runaway_takeoff_threshold * 10.0f;
if ((fabsf(axisPIDSum[FD_PITCH]) >= runawayTakeoffThreshold)
|| (fabsf(axisPIDSum[FD_ROLL]) >= runawayTakeoffThreshold)
|| (fabsf(axisPIDSum[FD_YAW]) >= runawayTakeoffThreshold)) {
if (runawayTakeoffTriggerUs == 0) {
runawayTakeoffTriggerUs = currentTimeUs + (pidConfig()->runaway_takeoff_activate_delay * 1000);
} else if (currentTimeUs > runawayTakeoffTriggerUs) {
setArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
disarm();
}
} else {
runawayTakeoffTriggerUs = 0;
}
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_TRUE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, runawayTakeoffTriggerUs == 0 ? DEBUG_RUNAWAY_TAKEOFF_FALSE : DEBUG_RUNAWAY_TAKEOFF_TRUE);
} else {
runawayTakeoffTriggerUs = 0;
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE, DEBUG_RUNAWAY_TAKEOFF_FALSE);
DEBUG_SET(DEBUG_RUNAWAY_TAKEOFF, DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY, DEBUG_RUNAWAY_TAKEOFF_FALSE);
}
#endif
}
static void subTaskMainSubprocesses(timeUs_t currentTimeUs)

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

@ -195,6 +195,14 @@ void processRcStickPositions(throttleStatus_e throttleStatus)
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
repeatAfter(STICK_AUTOREPEAT_MS); // disarm tone will repeat
#ifdef USE_RUNAWAY_TAKEOFF
// Unset the ARMING_DISABLED_RUNAWAY_TAKEOFF arming disabled flag that might have been set
// by a runaway pidSum detection auto-disarm.
// This forces the pilot to explicitly perform a disarm sequence (even though we're implicitly disarmed)
// before they're able to rearm
unsetArmingDisabled(ARMING_DISABLED_RUNAWAY_TAKEOFF);
#endif
}
}
return;

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

@ -32,7 +32,7 @@ static uint32_t enabledSensors = 0;
const char *armingDisableFlagNames[]= {
"NOGYRO", "FAILSAFE", "RXLOSS", "BADRX", "BOXFAILSAFE",
"THROTTLE", "ANGLE", "BOOTGRACE", "NOPREARM", "LOAD",
"CALIB", "CLI", "CMS", "OSD", "BST", "MSP", "ARMSWITCH"
"CALIB", "CLI", "CMS", "OSD", "BST", "MSP", "RUNAWAY", "ARMSWITCH"
};
static armingDisableFlags_e armingDisableFlags = 0;

6
src/main/fc/runtime_config.h
View File

@ -51,10 +51,12 @@ typedef enum {
ARMING_DISABLED_OSD_MENU = (1 << 13),
ARMING_DISABLED_BST = (1 << 14),
ARMING_DISABLED_MSP = (1 << 15),
ARMING_DISABLED_ARM_SWITCH = (1 << 16), // Needs to be the last element, since it's always activated if one of the others is active when arming
ARMING_DISABLED_RUNAWAY_TAKEOFF = (1 << 16),
ARMING_DISABLED_ARM_SWITCH = (1 << 17), // Needs to be the last element, since it's always activated if one of the others is active when arming
} armingDisableFlags_e;
#define ARMING_DISABLE_FLAGS_COUNT 17
#define ARMING_DISABLE_FLAGS_COUNT 18
extern const char *armingDisableFlagNames[ARMING_DISABLE_FLAGS_COUNT];
void setArmingDisabled(armingDisableFlags_e flag);

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

@ -69,9 +69,21 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 1);
#else
#define PID_PROCESS_DENOM_DEFAULT 2
#endif
#ifdef USE_RUNAWAY_TAKEOFF
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT,
.runaway_takeoff_prevention = true,
.runaway_takeoff_threshold = 60, // corresponds to a pidSum value of 60% (raw 600) in the blackbox viewer
.runaway_takeoff_activate_delay = 75, // 75ms delay before activation (pidSum above threshold time)
.runaway_takeoff_deactivate_throttle = 25, // throttle level % needed to accumulate deactivation time
.runaway_takeoff_deactivate_delay = 500 // Accumulated time (in milliseconds) before deactivation in successful takeoff
);
#else
PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
.pid_process_denom = PID_PROCESS_DENOM_DEFAULT
);
#endif
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 2);

5
src/main/flight/pid.h
View File

@ -113,6 +113,11 @@ PG_DECLARE_ARRAY(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles);
typedef struct pidConfig_s {
uint8_t pid_process_denom; // Processing denominator for PID controller vs gyro sampling rate
uint8_t runaway_takeoff_prevention; // off, on - enables pidsum runaway disarm logic
uint8_t runaway_takeoff_threshold; // runaway pidsum trigger threshold
uint8_t runaway_takeoff_activate_delay; // delay in ms where pidSum is above threshold before activation
uint16_t runaway_takeoff_deactivate_delay; // delay in ms for "in-flight" conditions before deactivation (successful flight)
uint8_t runaway_takeoff_deactivate_throttle; // minimum throttle percent required during deactivation phase
} pidConfig_t;
PG_DECLARE(pidConfig_t, pidConfig);

7
src/main/interface/settings.c
View File

@ -605,6 +605,13 @@ const clivalue_t valueTable[] = {
// PG_PID_CONFIG
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, MAX_PID_PROCESS_DENOM }, PG_PID_CONFIG, offsetof(pidConfig_t, pid_process_denom) },
#ifdef USE_RUNAWAY_TAKEOFF
{ "runaway_takeoff_prevention", VAR_UINT8 | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_prevention) }, // enables/disables runaway takeoff prevention
{ "runaway_takeoff_threshold", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 30, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_threshold) }, // pidSum limit to trigger prevention
{ "runaway_takeoff_activate_delay", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 255 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_activate_delay) }, // time in ms where pidSum is above threshold to trigger prevention
{ "runaway_takeoff_deactivate_delay", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 100, 5000 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_delay) }, // deactivate time in ms
{ "runaway_takeoff_deactivate_throttle_percent", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 100 }, PG_PID_CONFIG, offsetof(pidConfig_t, runaway_takeoff_deactivate_throttle) }, // minimum throttle percentage during deactivation phase
#endif
// PG_PID_PROFILE
{ "dterm_lowpass_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LOWPASS_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_filter_type) },

1
src/main/target/common_fc_pre.h
View File

@ -123,6 +123,7 @@
#define USE_TELEMETRY_SMARTPORT
#define USE_RESOURCE_MGMT
#define USE_SERVOS
#define USE_RUNAWAY_TAKEOFF // Runaway Takeoff Prevention (anti-taz)
#endif
#if (FLASH_SIZE > 128)

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

@ -64,6 +64,7 @@ extern "C" {
gpsSolutionData_t gpsSol;
uint32_t targetPidLooptime;
bool cmsInMenu = false;
float axisPID_P[3], axisPID_I[3], axisPID_D[3], axisPIDSum[3];
rxRuntimeConfig_t rxRuntimeConfig = {};
}

5
src/test/unit/rc_controls_unittest.cc
View File

@ -50,6 +50,7 @@ extern "C" {
#include "fc/rc_adjustments.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"
#include "scheduler/scheduler.h"
}
@ -57,6 +58,10 @@ extern "C" {
#include "unittest_macros.h"
#include "gtest/gtest.h"
void unsetArmingDisabled(armingDisableFlags_e flag) {
UNUSED(flag);
}
class RcControlsModesTest : public ::testing::Test {
protected:
virtual void SetUp() {