Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 08:45:36 +03:00
Code Issues Wiki Activity

Auto-disarm on landing impact (#13803)

Browse source 
* Disarm on landing

* Changes from review comments, thanks PL

* Sorry missed that one

* calculate Acc magnitude once only, not multiple times

* Include gyro factors as in crashRecovery

* Fix bug in CrashRecovery delta gains

Add temporary debugs to monitor error and delta inputs

* remove 1G subtraction for accMagnitude

thanks Karate

* Use AccDelta or Jerk - thanks Karate

* Revert using Gyro Setpoint and Delta

* Fix typo, thanks Mark

* increment PG version to 9
This commit is contained in:
ctzsnooze 2024-08-10 16:34:51 +10:00 • committed by GitHub
parent 8f10f17245
commit f890287598
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
16 changed files with 72 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

25
src/main/flight/pid.c
View file

@ -116,7 +116,7 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define LAUNCH_CONTROL_YAW_ITERM_LIMIT 50 // yaw iterm windup limit when launch mode is "FULL" (all axes)
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 8);
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, PID_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 9);
void resetPidProfile(pidProfile_t *pidProfile)
{
@ -236,6 +236,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.spa_center = { 0, 0, 0 },
.spa_width = { 0, 0, 0 },
.spa_mode = { 0, 0, 0 },
.ez_landing_disarm_threshold = 0 ,
);
#ifndef USE_D_MIN
@ -774,6 +775,20 @@ float pidGetAirmodeThrottleOffset(void)
}
#endif
static FAST_CODE_NOINLINE void disarmOnImpact(void)
{
// if all sticks are within 5% of center, and throttle low, check accDelta for impacts
// threshold should be high enough to avoid unwanted disarms in the air on throttle chops
if (isAirmodeActivated() && getMaxRcDeflectionAbs() < 0.05f && mixerGetRcThrottle() < 0.05f &&
fabsf(acc.accDelta) > pidRuntime.ezLandingDisarmThreshold) {
// disarm on accDelta transients
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
disarm(DISARM_REASON_LANDING);
}
DEBUG_SET(DEBUG_EZLANDING, 6, lrintf(getMaxRcDeflectionAbs() * 100.0f));
DEBUG_SET(DEBUG_EZLANDING, 7, lrintf(acc.accDelta));
}
#ifdef USE_LAUNCH_CONTROL
#define LAUNCH_CONTROL_MAX_RATE 100.0f
#define LAUNCH_CONTROL_MIN_RATE 5.0f
@ -974,6 +989,10 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
rpmFilterUpdate();
#endif
if (pidRuntime.useEzDisarm) {
disarmOnImpact();
}
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@ -1043,6 +1062,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
const float previousIterm = pidData[axis].I;
float itermErrorRate = errorRate;
#ifdef USE_ABSOLUTE_CONTROL
const float uncorrectedSetpoint = currentPidSetpoint;
#endif
@ -1081,12 +1101,14 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
}
float iTermChange = (Ki + pidRuntime.itermAccelerator) * dynCi * pidRuntime.dT * itermErrorRate;
#ifdef USE_WING
if (pidProfile->spa_mode[axis] != SPA_MODE_OFF) {
// slowing down I-term change, or even making it zero if setpoint is high enough
iTermChange *= pidRuntime.spa[axis];
}
#endif // #ifdef USE_WING
pidData[axis].I = constrainf(previousIterm + iTermChange, -pidRuntime.itermLimit, pidRuntime.itermLimit);
// -----calculate D component
@ -1151,6 +1173,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim
// Apply the dMinFactor
preTpaD *= dMinFactor;
#endif
pidData[axis].D = preTpaD * pidRuntime.tpaFactor;
// Log the value of D pre application of TPA