Skip to content

Commit

Permalink
on MR allow airmode to be sticky and activated by throttle threshold
Browse files Browse the repository at this point in the history
  • Loading branch information
DzikuVx committed Apr 23, 2019
1 parent 7af86a9 commit af6f1fa
Show file tree
Hide file tree
Showing 6 changed files with 65 additions and 5 deletions.
10 changes: 8 additions & 2 deletions src/main/fc/fc_core.c
Original file line number Diff line number Diff line change
Expand Up @@ -485,7 +485,7 @@ void processRx(timeUs_t currentTimeUs)
}

processRcStickPositions(throttleStatus);

processAirmode();
updateActivatedModes();

#ifdef USE_PINIOBOX
Expand Down Expand Up @@ -589,7 +589,7 @@ void processRx(timeUs_t currentTimeUs)
/* In MANUAL mode we reset integrators prevent I-term wind-up (PID output is not used in MANUAL) */
pidResetErrorAccumulators();
}
else {
else if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
if (throttleStatus == THROTTLE_LOW) {
if (isAirmodeActive() && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus();
Expand All @@ -610,6 +610,12 @@ void processRx(timeUs_t currentTimeUs)
else {
DISABLE_STATE(ANTI_WINDUP);
}
} else if (rcControlsConfig()->airmodeHandlingType == AIRMODE_THROTTLE_THRESHOLD) {
DISABLE_STATE(ANTI_WINDUP);
//This case applies only to MR when Airmode management is throttle threshold activated
if (throttleStatus == THROTTLE_LOW && !isAirmodeActive()) {
pidResetErrorAccumulators();
}
}

if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
Expand Down
5 changes: 3 additions & 2 deletions src/main/fc/rc_controls.c
Original file line number Diff line number Diff line change
Expand Up @@ -69,14 +69,15 @@ stickPositions_e rcStickPositions;

FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW

PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 0);
PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1);

PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig,
.deadband = 5,
.yaw_deadband = 5,
.pos_hold_deadband = 20,
.alt_hold_deadband = 50,
.deadband3d_throttle = 50
.deadband3d_throttle = 50,
.airmodeHandlingType = STICK_CENTER
);

PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 1);
Expand Down
8 changes: 8 additions & 0 deletions src/main/fc/rc_controls.h
Original file line number Diff line number Diff line change
Expand Up @@ -22,6 +22,8 @@
#define AUTO_DISARM_DELAY_MIN 0
#define AUTO_DISARM_DELAY_MAX 60

#define AIRMODE_THROTTLE_THRESHOLD 1250

typedef enum rc_alias {
ROLL = 0,
PITCH,
Expand Down Expand Up @@ -51,6 +53,11 @@ typedef enum {
CENTERED
} rollPitchStatus_e;

typedef enum {
STICK_CENTER = 0,
THROTTLE_THRESHOLD
} airmodeAndAntiWindupHandlingType_e;

typedef enum {
ROL_LO = (1 << (2 * ROLL)),
ROL_CE = (3 << (2 * ROLL)),
Expand All @@ -77,6 +84,7 @@ typedef struct rcControlsConfig_s {
uint8_t pos_hold_deadband; // Adds ability to adjust the Hold-position when moving the sticks (assisted mode)
uint8_t alt_hold_deadband; // Defines the neutral zone of throttle stick during altitude hold
uint16_t deadband3d_throttle; // default throttle deadband from MIDRC
uint8_t airmodeHandlingType; // Defaults to ANTI_WINDUP triggered at sticks centered
} rcControlsConfig_t;

PG_DECLARE(rcControlsConfig_t, rcControlsConfig);
Expand Down
41 changes: 40 additions & 1 deletion src/main/fc/rc_modes.c
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@

#include "fc/config.h"
#include "fc/rc_controls.h"
#include "fc/runtime_config.h"

#include "rx/rx.h"

Expand All @@ -40,6 +41,8 @@ static bool isUsingSticksToArm = true;
static bool isUsingNAVModes = false;
#endif

static EXTENDED_FASTRAM uint8_t airmodeActivationFlag = false;

boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e

// TODO(alberto): It looks like we can now safely remove this assert, since everything
Expand All @@ -59,9 +62,45 @@ bool isUsingSticksForArming(void)
return isUsingSticksToArm;
}

void processAirmode(void) {
if (STATE(FIXED_WING) || rcControlsConfig()->airmodeHandlingType == STICK_CENTER) {
airmodeActivationFlag = feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE);
} else if (rcControlsConfig()->airmodeHandlingType == THROTTLE_THRESHOLD) {

if (!ARMING_FLAG(ARMED)) {
/*
* Disarm disables airmode immediately
*/
airmodeActivationFlag = false;
} else if (
!airmodeActivationFlag &&
rcCommand[THROTTLE] > AIRMODE_THROTTLE_THRESHOLD &&
(feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE))
) {
/*
* Airmode is allowed to be active only after ARMED and then THROTTLE goes above
* activation threshold
*/
airmodeActivationFlag = true;
} else if (
airmodeActivationFlag &&
!feature(FEATURE_AIRMODE) &&
!IS_RC_MODE_ACTIVE(BOXAIRMODE)
) {
/*
* When user disables BOXAIRMODE, turn airmode off as well
*/
airmodeActivationFlag = false;
}

} else {
airmodeActivationFlag = false;
}
}

bool isAirmodeActive(void)
{
return feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE);
return airmodeActivationFlag;
}

#if defined(USE_NAV)
Expand Down
1 change: 1 addition & 0 deletions src/main/fc/rc_modes.h
Original file line number Diff line number Diff line change
Expand Up @@ -121,6 +121,7 @@ void rcModeUpdate(boxBitmask_t *newState);
bool isModeActivationConditionPresent(boxId_e modeId);

bool isUsingSticksForArming(void);
void processAirmode(void);
bool isAirmodeActive(void);
bool isUsingNavigationModes(void);
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
Expand Down
5 changes: 5 additions & 0 deletions src/main/fc/settings.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -114,6 +114,8 @@ tables:
values: ["PT1", "BIQUAD"]
- name: log_level
values: ["ERROR", "WARNING", "INFO", "VERBOSE", "DEBUG"]
- name: airmodeHandlingType
values: ["STICK_CENTER", "THROTTLE_THRESHOLD"]

groups:
- name: PG_GYRO_CONFIG
Expand Down Expand Up @@ -845,6 +847,9 @@ groups:
field: deadband3d_throttle
min: 0
max: 200
- name: mc_airmode_type
field: airmodeHandlingType
table: airmodeHandlingType

- name: PG_PID_PROFILE
type: pidProfile_t
Expand Down

0 comments on commit af6f1fa

Please sign in to comment.