Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-23 00:05:28 +03:00
Code Issues Wiki Activity

Refactor arming; Add configurable switch-disarm delay

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2018-04-25 18:27:02 +10:00
parent 108b619a34
commit 8b9d6e092b
7 changed files with 36 additions and 28 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -332,7 +332,7 @@ void annexCode(void)
updateArmingStatus();
}
void mwDisarm(disarmReason_t disarmReason)
void disarm(disarmReason_t disarmReason)
{
if (ARMING_FLAG(ARMED)) {
lastDisarmReason = disarmReason;
@ -365,7 +365,7 @@ void releaseSharedTelemetryPorts(void) {
}
}
void mwArm(void)
void tryArm(void)
{
updateArmingStatus();
@ -427,7 +427,7 @@ void processRx(timeUs_t currentTimeUs)
// in 3D mode, we need to be able to disarm by switch at any time
if (feature(FEATURE_3D)) {
if (!IS_RC_MODE_ACTIVE(BOXARM))
mwDisarm(DISARM_SWITCH_3D);
disarm(DISARM_SWITCH_3D);
}
updateRSSI(currentTimeUs);
@ -454,7 +454,7 @@ void processRx(timeUs_t currentTimeUs)
&& (int32_t)(disarmAt - millis()) < 0
) {
// auto-disarm configured and delay is over
mwDisarm(DISARM_TIMEOUT);
disarm(DISARM_TIMEOUT);
armedBeeperOn = false;
} else {
// still armed; do warning beeps while armed
@ -485,7 +485,7 @@ void processRx(timeUs_t currentTimeUs)
}
}
processRcStickPositions(throttleStatus, armingConfig()->disarm_kill_switch, armingConfig()->fixed_wing_auto_arm);
processRcStickPositions(throttleStatus);
updateActivatedModes();

4
src/main/fc/fc_core.h
View file

@ -31,8 +31,8 @@ typedef enum disarmReason_e {
void handleInflightCalibrationStickPosition(void);
void mwDisarm(disarmReason_t disarmReason);
void mwArm(void);
void disarm(disarmReason_t disarmReason);
void tryArm(void);
disarmReason_t getDisarmReason(void);
bool isCalibrating(void);

39
src/main/fc/rc_controls.c
View file

@ -62,7 +62,8 @@
#include "sensors/acceleration.h"
#define AIRMODE_DEADBAND 25
#define MIN_RC_TICK_INTERVAL_MS 20
#define MIN_RC_TICK_INTERVAL_MS 20
#define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 150 // Wait at least 150ms before disarming via switch
stickPositions_e rcStickPositions;
@ -78,11 +79,13 @@ PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband3d_throttle = 50
);
PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1);
PG_RESET_TEMPLATE(armingConfig_t, armingConfig,
.fixed_wing_auto_arm = 0,
.disarm_kill_switch = 1,
.auto_disarm_delay = 5
.auto_disarm_delay = 5,
.switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS,
);
bool areSticksInApModePosition(uint16_t ap_mode)
@ -151,12 +154,12 @@ static void updateRcStickPositions(void)
rcStickPositions = tmp;
}
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm)
void processRcStickPositions(throttleStatus_e throttleStatus)
{
static timeMs_t lastTickTimeMs = 0;
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
static uint32_t rcSticks; // this hold sticks position for command combos
static uint8_t rcDisarmTicks; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
static timeMs_t rcDisarmTimeMs; // this is an extra guard for disarming through switch to prevent that one frame can disarm it
const timeMs_t currentTimeMs = millis();
updateRcStickPositions();
@ -177,29 +180,29 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
// perform actions
if (!isUsingSticksForArming()) {
if (IS_RC_MODE_ACTIVE(BOXARM)) {
rcDisarmTicks = 0;
mwArm();
rcDisarmTimeMs = currentTimeMs;
tryArm();
} else {
// Disarming via ARM BOX
// Don't disarm via switch if failsafe is active or receiver doesn't receive data - we can't trust receiver
// and can't afford to risk disarming in the air
if (ARMING_FLAG(ARMED) && !IS_RC_MODE_ACTIVE(BOXFAILSAFE) && rxIsReceivingSignal() && !failsafeIsActive()) {
rcDisarmTicks++;
if (rcDisarmTicks > 3) { // Wait for at least 3 RX ticks (60ms @ 50Hz RX)
if (disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
mwDisarm(DISARM_SWITCH);
const timeMs_t disarmDelay = currentTimeMs - rcDisarmTimeMs;
if (disarmDelay > armingConfig()->switchDisarmDelayMs) {
if (armingConfig()->disarm_kill_switch || (throttleStatus == THROTTLE_LOW)) {
disarm(DISARM_SWITCH);
}
}
}
else {
rcDisarmTicks = 0;
rcDisarmTimeMs = currentTimeMs;
}
}
}
// KILLSWITCH disarms instantly
if (IS_RC_MODE_ACTIVE(BOXKILLSWITCH)) {
mwDisarm(DISARM_KILLSWITCH);
disarm(DISARM_KILLSWITCH);
}
if (rcDelayCommand != 20) {
@ -210,11 +213,11 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
// Disarm on throttle down + yaw
if (rcSticks == THR_LO + YAW_LO + PIT_CE + ROL_CE) {
// Dont disarm if fixedwing and motorstop
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
return;
}
else if (ARMING_FLAG(ARMED)) {
mwDisarm(DISARM_STICKS);
disarm(DISARM_STICKS);
}
else {
beeper(BEEPER_DISARM_REPEAT); // sound tone while stick held
@ -274,17 +277,17 @@ void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_s
// Arming by sticks
if (isUsingSticksForArming()) {
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && fixed_wing_auto_arm) {
if (STATE(FIXED_WING) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) {
// Auto arm on throttle when using fixedwing and motorstop
if (throttleStatus != THROTTLE_LOW) {
mwArm();
tryArm();
return;
}
}
else {
if (rcSticks == THR_LO + YAW_HI + PIT_CE + ROL_CE) {
// Arm via YAW
mwArm();
tryArm();
return;
}
}

3
src/main/fc/rc_controls.h
View file

@ -81,6 +81,7 @@ typedef struct armingConfig_s {
uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm
uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value
uint8_t auto_disarm_delay; // allow automatically disarming multicopters after auto_disarm_delay seconds of zero throttle. Disabled when 0
uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm
} armingConfig_t;
PG_DECLARE(armingConfig_t, armingConfig);
@ -92,6 +93,6 @@ bool areSticksInApModePosition(uint16_t ap_mode);
bool areSticksDeflectedMoreThanPosHoldDeadband(void);
throttleStatus_e calculateThrottleStatus(void);
rollPitchStatus_e calculateRollPitchCenterStatus(void);
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm);
void processRcStickPositions(throttleStatus_e throttleStatus);
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);

4
src/main/fc/settings.yaml
View file

@ -710,6 +710,10 @@ groups:
- name: auto_disarm_delay
min: 0
max: 60
- name: switch_disarm_delay
field: switchDisarmDelayMs
min: 0
max: 1000
- name: PG_GPS_CONFIG
type: gpsConfig_t

2
src/main/flight/failsafe.c
View file

@ -498,7 +498,7 @@ void failsafeUpdateState(void)
case FAILSAFE_LANDED:
ENABLE_ARMING_FLAG(ARMING_DISABLED_FAILSAFE_SYSTEM); // To prevent accidently rearming by an intermittent rx link
mwDisarm(DISARM_FAILSAFE);
disarm(DISARM_FAILSAFE);
failsafeState.receivingRxDataPeriod = millis() + failsafeState.receivingRxDataPeriodPreset; // set required period of valid rxData
failsafeState.phase = FAILSAFE_RX_LOSS_MONITORING;
failsafeState.controlling = false; // Failsafe no longer in control of the machine - release control to pilot

2
src/main/navigation/navigation.c
View file

@ -1036,7 +1036,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_FINISHING(navigatio
UNUSED(previousState);
if (navConfig()->general.flags.disarm_on_landing) {
mwDisarm(DISARM_NAVIGATION);
disarm(DISARM_NAVIGATION);
}
return NAV_FSM_EVENT_SUCCESS;