Rework Iterm Shrink for AIR Mode
Rework Iterm Shrink for AIR Mode part 2
This commit is contained in:
parent
66fb254d62
commit
6a4682908f
|
@ -321,7 +321,7 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
|
|||
if (CALC_OFFSET(errorGyroI[axis]) < CALC_OFFSET(previousErrorGyroI[axis])) {
|
||||
previousErrorGyroI[axis] = errorGyroI[axis];
|
||||
} else {
|
||||
errorGyroIf[axis] = constrain(errorGyroIf[axis], -CALC_OFFSET(previousErrorGyroI[axis]), CALC_OFFSET(previousErrorGyroI[axis]));
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis], -CALC_OFFSET(previousErrorGyroI[axis]), CALC_OFFSET(previousErrorGyroI[axis]));
|
||||
}
|
||||
} else {
|
||||
previousErrorGyroI[axis] = errorGyroI[axis];
|
||||
|
|
|
@ -59,13 +59,11 @@
|
|||
|
||||
#include "mw.h"
|
||||
|
||||
|
||||
static escAndServoConfig_t *escAndServoConfig;
|
||||
static pidProfile_t *pidProfile;
|
||||
|
||||
// true if arming is done via the sticks (as opposed to a switch)
|
||||
static bool isUsingSticksToArm = true;
|
||||
static bool rollPitchCentered = true; // Roll and pitch are centered, AIR Mode condition
|
||||
|
||||
int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW
|
||||
|
||||
|
@ -107,11 +105,6 @@ bool isUsingSticksForArming(void)
|
|||
return isUsingSticksToArm;
|
||||
}
|
||||
|
||||
bool isRollPitchCentered(void)
|
||||
{
|
||||
return rollPitchCentered;
|
||||
}
|
||||
|
||||
bool areSticksInApModePosition(uint16_t ap_mode)
|
||||
{
|
||||
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode;
|
||||
|
@ -127,6 +120,18 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
|
|||
return THROTTLE_HIGH;
|
||||
}
|
||||
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
|
||||
{
|
||||
if (feature(FEATURE_3D)) // TODO
|
||||
return CENTERED;
|
||||
else if (!feature(FEATURE_3D)
|
||||
&& ((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND)))
|
||||
&& ((rcData[ROLL] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODEDEADBAND))))
|
||||
return CENTERED;
|
||||
|
||||
return NOT_CENTERED;
|
||||
}
|
||||
|
||||
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool retarded_arm, bool disarm_kill_switch)
|
||||
{
|
||||
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
|
||||
|
@ -151,12 +156,6 @@ void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStat
|
|||
rcDelayCommand = 0;
|
||||
rcSticks = stTmp;
|
||||
|
||||
if (rcSticks == PIT_CE + ROL_CE) {
|
||||
rollPitchCentered = true;
|
||||
} else {
|
||||
rollPitchCentered = false;
|
||||
}
|
||||
|
||||
// perform actions
|
||||
if (!isUsingSticksToArm) {
|
||||
|
||||
|
|
|
@ -77,6 +77,13 @@ typedef enum {
|
|||
THROTTLE_HIGH
|
||||
} throttleStatus_e;
|
||||
|
||||
#define AIRMODEDEADBAND 10
|
||||
|
||||
typedef enum {
|
||||
NOT_CENTERED = 0,
|
||||
CENTERED
|
||||
} rollPitchStatus_e;
|
||||
|
||||
#define ROL_LO (1 << (2 * ROLL))
|
||||
#define ROL_CE (3 << (2 * ROLL))
|
||||
#define ROL_HI (2 << (2 * ROLL))
|
||||
|
@ -243,4 +250,4 @@ bool isUsingSticksForArming(void);
|
|||
|
||||
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
|
||||
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
|
||||
bool isRollPitchCentered(void);
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig);
|
||||
|
|
|
@ -20,7 +20,6 @@
|
|||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "debug.h"
|
||||
#include "platform.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
|
@ -575,26 +574,30 @@ void processRx(void)
|
|||
if (throttleStatus == THROTTLE_LOW) {
|
||||
// When in AIR Mode LOW Throttle and reset was already disabled we will only prevent further growing
|
||||
if ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && !airModeErrorResetIsEnabled) {
|
||||
if (isRollPitchCentered()) {
|
||||
if (calculateRollPitchCenterStatus(&masterConfig.rxConfig) == CENTERED) {
|
||||
allowITermShrinkOnly = true; // Iterm is now only allowed to shrink
|
||||
} else {
|
||||
allowITermShrinkOnly = false; // Iterm should considered safe to increase
|
||||
}
|
||||
}
|
||||
|
||||
// Conditions to reset Error
|
||||
if (!IS_RC_MODE_ACTIVE(BOXARM) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled)) {
|
||||
if (!ARMING_FLAG(ARMED) || feature(FEATURE_MOTOR_STOP) || ((IS_RC_MODE_ACTIVE(BOXAIRMODE)) && airModeErrorResetIsEnabled) || !IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
||||
pidResetErrorGyro();
|
||||
airModeErrorResetTimeout = millis() + ERROR_RESET_DEACTIVATE_DELAY; // Reset de-activate timer
|
||||
airModeErrorResetIsEnabled = true; // Enable Reset again especially after Disarm
|
||||
allowITermShrinkOnly = false; // Disable shrink especially after Disarm
|
||||
}
|
||||
} else {
|
||||
if (!(feature(FEATURE_MOTOR_STOP)) && IS_RC_MODE_ACTIVE(BOXARM) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
||||
if (!(feature(FEATURE_MOTOR_STOP)) && ARMING_FLAG(ARMED) && IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
||||
if (airModeErrorResetIsEnabled) {
|
||||
if ((millis() > airModeErrorResetTimeout) && !isRollPitchCentered()) { // Only disable error reset when roll and pitch not centered
|
||||
if (millis() > airModeErrorResetTimeout && calculateRollPitchCenterStatus(&masterConfig.rxConfig) == NOT_CENTERED) { // Only disable error reset when roll and pitch not centered
|
||||
airModeErrorResetIsEnabled = false;
|
||||
allowITermShrinkOnly = false; // Reset shrinking for Iterm
|
||||
}
|
||||
}
|
||||
} else {
|
||||
airModeErrorResetIsEnabled = true; // This is needed to reset procedure when switch is toggled in air
|
||||
allowITermShrinkOnly = false; // Reset shrinking for Iterm
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in New Issue