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

Add gyro rate checking to runaway takeoff activation to improve bench testing 

Changed the triggering phase so that in addition to the pidSum needing to exceed runaway_takeoff_threshold, the gyro rate on any axis must exceed a threshold value to indicate the quad is actually moving. The threshold for the yaw axis is much higher since the torque of the motors can still yaw the craft without props.
This commit is contained in:
Bruce Luckcuck 2018-02-17 14:18:21 -05:00
parent 7c3bb49759
commit 9ea965c549
3 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -100,6 +100,8 @@ enum {
#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 RUNAWAY_TAKEOFF_GYRO_LIMIT_RP 15 // Roll/pitch 15 deg/sec threshold to prevent triggering during bench testing without props
#define RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW 50 // Yaw 50 deg/sec threshold to prevent triggering during bench testing without props
#define DEBUG_RUNAWAY_TAKEOFF_ENABLED_STATE 0
#define DEBUG_RUNAWAY_TAKEOFF_ACTIVATING_DELAY 1
@ -773,7 +775,8 @@ static void subTaskPidController(timeUs_t 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.
// Check to see if runaway takeoff detection is active (anti-taz), the pidSum is over the threshold,
// and gyro rate for any axis is above the limit for at least the activate delay period.
// If so, disarm for safety
if (ARMING_FLAG(ARMED)
&& !STATE(FIXED_WING)
@ -784,9 +787,13 @@ static void subTaskPidController(timeUs_t currentTimeUs)
&& (!feature(FEATURE_MOTOR_STOP) || isAirmodeActive() || (calculateThrottleStatus() != THROTTLE_LOW))) {
const float runawayTakeoffThreshold = pidConfig()->runaway_takeoff_threshold * 10.0f;
if ((fabsf(axisPIDSum[FD_PITCH]) >= runawayTakeoffThreshold)
if (((fabsf(axisPIDSum[FD_PITCH]) >= runawayTakeoffThreshold)
|| (fabsf(axisPIDSum[FD_ROLL]) >= runawayTakeoffThreshold)
|| (fabsf(axisPIDSum[FD_YAW]) >= runawayTakeoffThreshold)) {
|| (fabsf(axisPIDSum[FD_YAW]) >= runawayTakeoffThreshold))
&& ((ABS(gyroAbsRateDps(FD_PITCH)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_ROLL)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_RP)
|| (ABS(gyroAbsRateDps(FD_YAW)) > RUNAWAY_TAKEOFF_GYRO_LIMIT_YAW))) {
if (runawayTakeoffTriggerUs == 0) {
runawayTakeoffTriggerUs = currentTimeUs + (pidConfig()->runaway_takeoff_activate_delay * 1000);

5
src/main/sensors/gyro.c
View File

@ -915,3 +915,8 @@ bool gyroOverflowDetected(void)
{
return gyroSensor1.overflowDetected;
}
uint16_t gyroAbsRateDps(int axis)
{
return fabsf(gyro.gyroADCf[axis]);
}

1
src/main/sensors/gyro.h
View File

@ -96,3 +96,4 @@ void gyroReadTemperature(void);
int16_t gyroGetTemperature(void);
int16_t gyroRateDps(int axis);
bool gyroOverflowDetected(void);
uint16_t gyroAbsRateDps(int axis);