Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 01:05:27 +03:00
Code Issues Wiki Activity

Improved detection of upright / 'SMALL_ANGLE' state.

Browse source
This commit is contained in:
mikeller 2019-11-17 13:40:30 +13:00 • committed by Michael Keller
parent 6694d4ebc8
commit cc8b8d3bf6
11 changed files with 108 additions and 95 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/fc/core.c
View file

@ -320,7 +320,7 @@ void updateArmingStatus(void)
unsetArmingDisabled(ARMING_DISABLED_THROTTLE);
}
if (!STATE(SMALL_ANGLE) && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
if (!isUpright() && !IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH)) {
setArmingDisabled(ARMING_DISABLED_ANGLE);
} else {
unsetArmingDisabled(ARMING_DISABLED_ANGLE);
@ -627,7 +627,7 @@ static void updateInflightCalibrationState(void)
}
#if defined(USE_GPS) || defined(USE_MAG)
void updateMagHold(void)
static void updateMagHold(void)
{
if (fabsf(rcCommand[YAW]) < 15 && FLIGHT_MODE(MAG_MODE)) {
int16_t dif = DECIDEGREES_TO_DEGREES(attitude.values.yaw) - magHold;
@ -636,7 +636,7 @@ void updateMagHold(void)
if (dif >= +180)
dif -= 360;
dif *= -GET_DIRECTION(rcControlsConfig()->yaw_control_reversed);
if (STATE(SMALL_ANGLE)) {
if (isUpright()) {
rcCommand[YAW] -= dif * currentPidProfile->pid[PID_MAG].P / 30; // 18 deg
}
} else
@ -1130,8 +1130,8 @@ static FAST_CODE_NOINLINE void subTaskPidSubprocesses(timeUs_t currentTimeUs)
startTime = micros();
}
#ifdef USE_MAG
if (sensors(SENSOR_MAG)) {
#if defined(USE_GPS) || defined(USE_MAG)
if (sensors(SENSOR_GPS) || sensors(SENSOR_MAG)) {
updateMagHold();
}
#endif