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

Merge pull request #3826 from DanNixon/arming_conditions 

Safer arming condition checks
This commit is contained in:
Michael Keller 2017-08-14 23:03:42 +12:00 committed by GitHub
parent 1e9b553392 ad540d8ad8
commit 8995dcec0e
9 changed files with 475 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/fc/config.c
View File

@ -134,6 +134,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig,
.debug_mode = DEBUG_MODE,
.task_statistics = true,
.cpu_overclock = false,
.powerOnArmingGraceTime = 5,
.boardIdentifier = TARGET_BOARD_IDENTIFIER
);
#else

1
src/main/fc/config.h
View File

@ -72,6 +72,7 @@ typedef struct systemConfig_s {
#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
uint8_t cpu_overclock;
#endif
uint8_t powerOnArmingGraceTime; // in seconds
char boardIdentifier[sizeof(TARGET_BOARD_IDENTIFIER) + 1];
} systemConfig_t;
#endif

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

@ -152,6 +152,30 @@ void updateArmingStatus(void)
if (ARMING_FLAG(ARMED)) {
LED0_ON;
} else {
// Check if the power on arming grace time has elapsed
if ((getArmingDisableFlags() & ARMING_DISABLED_BOOT_GRACE_TIME) && (millis() >= systemConfig()->powerOnArmingGraceTime * 1000)) {
// If so, unset the grace time arming disable flag
unsetArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
}
// If switch is used for arming then check it is not defaulting to on when the RX link recovers from a fault
if (!isUsingSticksForArming()) {
static bool hadRx = false;
const bool haveRx = rxIsReceivingSignal();
const bool justGotRxBack = !hadRx && haveRx;
if (justGotRxBack && IS_RC_MODE_ACTIVE(BOXARM)) {
// If the RX has just started to receive a signal again and the arm switch is on, apply arming restriction
setArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
} else if (haveRx && !IS_RC_MODE_ACTIVE(BOXARM)) {
// If RX signal is OK and the arm switch is off, remove arming restriction
unsetArmingDisabled(ARMING_DISABLED_BAD_RX_RECOVERY);
}
hadRx = haveRx;
}
if (IS_RC_MODE_ACTIVE(BOXFAILSAFE)) {
setArmingDisabled(ARMING_DISABLED_BOXFAILSAFE);
} else {
@ -189,6 +213,15 @@ void updateArmingStatus(void)
setArmingDisabled(ARMING_DISABLED_NOPREARM);
}
if (!isUsingSticksForArming()) {
// If arming is disabled and the ARM switch is on
if (isArmingDisabled() && IS_RC_MODE_ACTIVE(BOXARM)) {
setArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
} else if (!IS_RC_MODE_ACTIVE(BOXARM)) {
unsetArmingDisabled(ARMING_DISABLED_ARM_SWITCH);
}
}
if (isArmingDisabled()) {
warningLedFlash();
} else {

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

@ -719,6 +719,8 @@ void init(void)
latchActiveFeatures();
pwmEnableMotors();
setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
#ifdef USE_OSD_SLAVE
osdSlaveTasksInit();
#else

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

@ -31,8 +31,9 @@ static uint32_t enabledSensors = 0;
#if defined(OSD) || !defined(MINIMAL_CLI)
const char *armingDisableFlagNames[]= {
"NOGYRO", "FAILSAFE", "RX LOSS", "BOXFAILSAFE", "THROTTLE",
"ANGLE", "NO PREARM", "LOAD", "CALIB", "CLI", "CMS", "OSD", "BST"
"NOGYRO", "FAILSAFE", "RX LOSS", "BAD RX", "BOXFAILSAFE",
"THROTTLE", "ANGLE", "BOOT GRACE", "NO PREARM", "ARM SWITCH",
"LOAD", "CALIB", "CLI", "CMS", "OSD", "BST"
};
#endif

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

@ -38,19 +38,22 @@ typedef enum {
ARMING_DISABLED_NO_GYRO = (1 << 0),
ARMING_DISABLED_FAILSAFE = (1 << 1),
ARMING_DISABLED_RX_FAILSAFE = (1 << 2),
ARMING_DISABLED_BOXFAILSAFE = (1 << 3),
ARMING_DISABLED_THROTTLE = (1 << 4),
ARMING_DISABLED_ANGLE = (1 << 5),
ARMING_DISABLED_NOPREARM = (1 << 6),
ARMING_DISABLED_LOAD = (1 << 7),
ARMING_DISABLED_CALIBRATING = (1 << 8),
ARMING_DISABLED_CLI = (1 << 9),
ARMING_DISABLED_CMS_MENU = (1 << 10),
ARMING_DISABLED_OSD_MENU = (1 << 11),
ARMING_DISABLED_BST = (1 << 12)
ARMING_DISABLED_BAD_RX_RECOVERY = (1 << 3),
ARMING_DISABLED_BOXFAILSAFE = (1 << 4),
ARMING_DISABLED_THROTTLE = (1 << 5),
ARMING_DISABLED_ANGLE = (1 << 6),
ARMING_DISABLED_BOOT_GRACE_TIME = (1 << 7),
ARMING_DISABLED_NOPREARM = (1 << 8),
ARMING_DISABLED_ARM_SWITCH = (1 << 9),
ARMING_DISABLED_LOAD = (1 << 10),
ARMING_DISABLED_CALIBRATING = (1 << 11),
ARMING_DISABLED_CLI = (1 << 12),
ARMING_DISABLED_CMS_MENU = (1 << 13),
ARMING_DISABLED_OSD_MENU = (1 << 14),
ARMING_DISABLED_BST = (1 << 15)
} armingDisableFlags_e;
#define NUM_ARMING_DISABLE_FLAGS 13
#define NUM_ARMING_DISABLE_FLAGS 16
#if defined(OSD) || !defined(MINIMAL_CLI)
extern const char *armingDisableFlagNames[NUM_ARMING_DISABLE_FLAGS];

1
src/main/fc/settings.c
View File

@ -704,6 +704,7 @@ const clivalue_t valueTable[] = {
#if defined(STM32F4) && !defined(DISABLE_OVERCLOCK)
{ "cpu_overclock", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, cpu_overclock) },
#endif
{ "pwr_on_arm_grace", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 30 }, PG_SYSTEM_CONFIG, offsetof(systemConfig_t, powerOnArmingGraceTime) },
// PG_VTX_CONFIG
#ifdef VTX_RTC6705

8
src/test/Makefile
View File

@ -28,6 +28,14 @@ altitude_hold_unittest_SRC := \
$(USER_DIR)/flight/altitude.c
arming_prevention_unittest_SRC := \
$(USER_DIR)/fc/fc_core.c \
$(USER_DIR)/fc/rc_controls.c \
$(USER_DIR)/fc/rc_modes.c \
$(USER_DIR)/fc/runtime_config.c \
$(USER_DIR)/common/bitarray.c
baro_bmp085_unittest_SRC := \
$(USER_DIR)/drivers/barometer/barometer_bmp085.c \
$(USER_DIR)/drivers/io.c

407
src/test/unit/arming_prevention_unittest.cc Normal file
View File

@ -0,0 +1,407 @@
/*
* This file is part of Cleanflight.
*
* Cleanflight is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* Cleanflight is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
*/
#include <stdint.h>
extern "C" {
#include "blackbox/blackbox.h"
#include "build/debug.h"
#include "common/maths.h"
#include "config/parameter_group.h"
#include "config/parameter_group_ids.h"
#include "fc/config.h"
#include "fc/controlrate_profile.h"
#include "fc/fc_core.h"
#include "fc/rc_controls.h"
#include "fc/rc_modes.h"
#include "fc/runtime_config.h"
#include "flight/imu.h"
#include "flight/mixer.h"
#include "flight/pid.h"
#include "flight/servos.h"
#include "io/beeper.h"
#include "io/gps.h"
#include "rx/rx.h"
#include "scheduler/scheduler.h"
#include "sensors/acceleration.h"
#include "sensors/gyro.h"
#include "telemetry/telemetry.h"
PG_REGISTER(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 0);
PG_REGISTER(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 0);
PG_REGISTER(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 0);
PG_REGISTER(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
PG_REGISTER(pidConfig_t, pidConfig, PG_PID_CONFIG, 0);
PG_REGISTER(rxConfig_t, rxConfig, PG_RX_CONFIG, 0);
PG_REGISTER(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0);
PG_REGISTER(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 0);
float rcCommand[4];
int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
uint16_t averageSystemLoadPercent = 0;
uint8_t cliMode = 0;
uint8_t debugMode = 0;
int16_t debug[DEBUG16_VALUE_COUNT];
pidProfile_t *currentPidProfile;
controlRateConfig_t *currentControlRateProfile;
attitudeEulerAngles_t attitude;
gpsSolutionData_t gpsSol;
uint32_t targetPidLooptime;
bool cmsInMenu = false;
}
uint32_t simulationTime = 0;
bool gyroCalibDone = false;
bool simulationHaveRx = false;
#include "gtest/gtest.h"
TEST(ArmingPreventionTest, CalibrationPowerOnGraceAngleThrottleArmSwitch)
{
// given
simulationTime = 0;
gyroCalibDone = false;
// and
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
modeActivationConditionsMutable(0)->modeId = BOXARM;
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
useRcControlsConfig(NULL);
// and
rxConfigMutable()->mincheck = 1050;
// and
// default channel positions
rcData[THROTTLE] = 1400;
rcData[4] = 1800;
// and
systemConfigMutable()->powerOnArmingGraceTime = 5;
setArmingDisabled(ARMING_DISABLED_BOOT_GRACE_TIME);
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_CALIBRATING | ARMING_DISABLED_ANGLE | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
// given
// gyro calibration is done
gyroCalibDone = true;
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_ANGLE | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
// given
// quad is level
ENABLE_STATE(SMALL_ANGLE);
// when
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_ARM_SWITCH | ARMING_DISABLED_THROTTLE, getArmingDisableFlags());
// given
rcData[THROTTLE] = 1000;
// when
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_BOOT_GRACE_TIME | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
// given
// arming grace time has elapsed
simulationTime += systemConfig()->powerOnArmingGraceTime * 1e6;
// when
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
// given
rcData[4] = 1000;
// when
// arm guard time elapses
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(isArmingDisabled());
}
TEST(ArmingPreventionTest, ArmingGuardRadioLeftOnAndArmed)
{
// given
simulationTime = 0;
gyroCalibDone = false;
// and
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
modeActivationConditionsMutable(0)->modeId = BOXARM;
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
useRcControlsConfig(NULL);
// and
rxConfigMutable()->mincheck = 1050;
// and
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(isUsingSticksForArming());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_CALIBRATING, getArmingDisableFlags());
// given
// arm channel takes a safe default value from the RX after power on
rcData[4] = 1500;
// and
// a short time passes while calibration is in progress
simulationTime += 1e6;
// and
// during calibration RF link is established and ARM switch is on
rcData[4] = 1800;
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_CALIBRATING | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
// given
// calibration is done
gyroCalibDone = true;
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
// given
// arm switch is switched off by user
rcData[4] = 1000;
// when
updateActivatedModes();
updateArmingStatus();
// expect
// arming enabled as arm switch has been off for sufficient time
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(isArmingDisabled());
}
TEST(ArmingPreventionTest, Prearm)
{
// given
simulationTime = 0;
// and
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
modeActivationConditionsMutable(0)->modeId = BOXARM;
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
modeActivationConditionsMutable(1)->auxChannelIndex = 1;
modeActivationConditionsMutable(1)->modeId = BOXPREARM;
modeActivationConditionsMutable(1)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(1)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
useRcControlsConfig(NULL);
// and
rxConfigMutable()->mincheck = 1050;
// given
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(isUsingSticksForArming());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_NOPREARM, getArmingDisableFlags());
// given
// prearm is enabled
rcData[5] = 1800;
// when
updateActivatedModes();
updateArmingStatus();
// expect
// arming enabled as arm switch has been off for sufficient time
EXPECT_EQ(0, getArmingDisableFlags());
EXPECT_FALSE(isArmingDisabled());
}
TEST(ArmingPreventionTest, RadioTurnedOnAtAnyTimeArmed)
{
// given
simulationTime = 30e6; // 30 seconds after boot
gyroCalibDone = true;
// and
modeActivationConditionsMutable(0)->auxChannelIndex = 0;
modeActivationConditionsMutable(0)->modeId = BOXARM;
modeActivationConditionsMutable(0)->range.startStep = CHANNEL_VALUE_TO_STEP(1750);
modeActivationConditionsMutable(0)->range.endStep = CHANNEL_VALUE_TO_STEP(CHANNEL_RANGE_MAX);
useRcControlsConfig(NULL);
// and
rxConfigMutable()->mincheck = 1050;
// and
rcData[THROTTLE] = 1000;
ENABLE_STATE(SMALL_ANGLE);
// and
// RX has no link to radio
simulationHaveRx = false;
// and
// arm channel has a safe default value
rcData[4] = 1100;
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(isUsingSticksForArming());
EXPECT_FALSE(isArmingDisabled());
EXPECT_EQ(0, getArmingDisableFlags());
// given
// RF link is established and arm switch is turned on on radio
simulationHaveRx = true;
rcData[4] = 1800;
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(isUsingSticksForArming());
EXPECT_TRUE(isArmingDisabled());
EXPECT_EQ(ARMING_DISABLED_BAD_RX_RECOVERY | ARMING_DISABLED_ARM_SWITCH, getArmingDisableFlags());
// given
// arm switch turned off by user
rcData[4] = 1100;
// when
updateActivatedModes();
updateArmingStatus();
// expect
EXPECT_FALSE(isUsingSticksForArming());
EXPECT_FALSE(isArmingDisabled());
EXPECT_EQ(0, getArmingDisableFlags());
}
// STUBS
extern "C" {
uint32_t micros(void) { return simulationTime; }
uint32_t millis(void) { return micros() / 1000; }
bool rxIsReceivingSignal(void) { return simulationHaveRx; }
bool feature(uint32_t) { return false; }
void warningLedFlash(void) {}
void warningLedDisable(void) {}
void warningLedUpdate(void) {}
void beeper(beeperMode_e) {}
void beeperConfirmationBeeps(uint8_t) {}
void beeperWarningBeeps(uint8_t) {}
void beeperSilence(void) {}
void systemBeep(bool) {}
void saveConfigAndNotify(void) {}
void blackboxFinish(void) {}
bool isAccelerationCalibrationComplete(void) { return true; }
bool isBaroCalibrationComplete(void) { return true; }
bool isGyroCalibrationComplete(void) { return gyroCalibDone; }
void gyroStartCalibration(bool) {}
bool isFirstArmingGyroCalibrationRunning(void) { return false; }
void pidController(const pidProfile_t *, const rollAndPitchTrims_t *, timeUs_t) {}
void pidStabilisationState(pidStabilisationState_e) {}
void mixTable(uint8_t) {};
void writeMotors(void) {};
void writeServos(void) {};
void calculateRxChannelsAndUpdateFailsafe(timeUs_t) {}
bool isMixerUsingServos(void) { return false; }
void gyroUpdate(void) {}
timeDelta_t getTaskDeltaTime(cfTaskId_e) { return 0; }
void updateRSSI(timeUs_t) {}
bool failsafeIsMonitoring(void) { return false; }
void failsafeStartMonitoring(void) {}
void failsafeUpdateState(void) {}
bool failsafeIsActive(void) { return false; }
void pidResetErrorGyroState(void) {}
void updateAdjustmentStates(void) {}
void processRcAdjustments(controlRateConfig_t *) {}
void updateGpsWaypointsAndMode(void) {}
void releaseSharedTelemetryPorts(void) {}
void telemetryCheckState(void) {}
void mspSerialAllocatePorts(void) {}
void gyroReadTemperature(void) {}
void updateRcCommands(void) {}
void applyAltHold(void) {}
void resetYawAxis(void) {}
int16_t calculateThrottleAngleCorrection(uint8_t) { return 0; }
void processRcCommand(void) {}
void updateGpsStateForHomeAndHoldMode(void) {}
void blackboxUpdate(timeUs_t) {}
void transponderUpdate(timeUs_t) {}
void GPS_reset_home_position(void) {}
void accSetCalibrationCycles(uint16_t) {}
void baroSetCalibrationCycles(uint16_t) {}
void changePidProfile(uint8_t) {}
void dashboardEnablePageCycling(void) {}
void dashboardDisablePageCycling(void) {}
}