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

Add anti gravity as feature and mode 

3.1.7 mergebacks
This commit is contained in:
borisbstyle 2017-03-31 15:55:31 +02:00
parent d8f03693f7
commit 6899c66a05
10 changed files with 46 additions and 51 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -147,7 +147,7 @@ static const char * const featureNames[] = {
"SONAR", "TELEMETRY", "CURRENT_METER", "3D", "RX_PARALLEL_PWM",
"RX_MSP", "RSSI_ADC", "LED_STRIP", "DISPLAY", "OSD",
"UNUSED", "CHANNEL_FORWARDING", "TRANSPONDER", "AIRMODE",
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", NULL
"SDCARD", "VTX", "RX_SPI", "SOFTSPI", "ESC_SENSOR", "ANTI_GRAVITY", NULL
};
// sync this with rxFailsafeChannelMode_e

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

@ -61,6 +61,7 @@ typedef enum {
FEATURE_RX_SPI = 1 << 25,
FEATURE_SOFTSPI = 1 << 26,
FEATURE_ESC_SENSOR = 1 << 27,
FEATURE_ANTI_GRAVITY = 1 << 28,
} features_e;
typedef struct systemConfig_s {

7
src/main/fc/fc_msp.c
View File

@ -126,7 +126,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
{ BOXANGLE, "ANGLE;", 1 },
{ BOXHORIZON, "HORIZON;", 2 },
{ BOXBARO, "BARO;", 3 },
//{ BOXVARIO, "VARIO;", 4 },
{ BOXANTIGRAVITY, "ANTI GRAVITY;", 4 },
{ BOXMAG, "MAG;", 5 },
{ BOXHEADFREE, "HEADFREE;", 6 },
{ BOXHEADADJ, "HEADADJ;", 7 },
@ -322,6 +322,10 @@ void initActiveBoxIds(void)
activeBoxIds[activeBoxIdCount++] = BOXAIRMODE;
}
if (!feature(FEATURE_ANTI_GRAVITY)) {
activeBoxIds[activeBoxIdCount++] = BOXANTIGRAVITY;
}
if (sensors(SENSOR_ACC)) {
activeBoxIds[activeBoxIdCount++] = BOXANGLE;
activeBoxIds[activeBoxIdCount++] = BOXHORIZON;
@ -441,6 +445,7 @@ static uint32_t packFlightModeFlags(void)
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXBLACKBOXERASE)) << BOXBLACKBOXERASE |
IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)) << BOXFAILSAFE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)) << BOXAIRMODE |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY)) << BOXANTIGRAVITY |
IS_ENABLED(IS_RC_MODE_ACTIVE(BOXFPVANGLEMIX)) << BOXFPVANGLEMIX;
uint32_t ret = 0;

3
src/main/fc/fc_rc.c
View File

@ -185,8 +185,9 @@ void processRcCommand(void)
if (isRXDataNew) {
currentRxRefreshRate = constrain(getTaskDeltaTime(TASK_RX),1000,20000);
if (currentPidProfile->itermAcceleratorGain > 1.0f)
if (isAntiGravityModeActive()) {
checkForThrottleErrorResetState(currentRxRefreshRate);
}
}
if (rxConfig()->rcInterpolation || flightModeFlags) {

4
src/main/fc/rc_controls.c
View File

@ -94,6 +94,10 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isAntiGravityModeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
}
bool isUsingSticksForArming(void)
{
return isUsingSticksToArm;

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

@ -26,7 +26,7 @@ typedef enum {
BOXANGLE,
BOXHORIZON,
BOXBARO,
// BOXVARIO,
BOXANTIGRAVITY,
BOXMAG,
BOXHEADFREE,
BOXHEADADJ,
@ -190,6 +190,7 @@ bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);
bool isAirmodeActive(void);
bool isAntiGravityModeActive(void);
bool isUsingSticksForArming(void);

16
src/main/flight/mixer.c
View File

@ -539,13 +539,19 @@ void mixTable(pidProfile_t *pidProfile)
const float motorOutputRange = motorOutputMax - motorOutputMin;
float scaledAxisPIDf[3];
// Limit the PIDsum
scaledAxisPIDf[FD_ROLL] = constrainf(axisPIDf[FD_ROLL] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
scaledAxisPIDf[FD_PITCH] = constrainf(axisPIDf[FD_PITCH] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimit);
scaledAxisPIDf[FD_YAW] = constrainf(axisPIDf[FD_YAW] / PID_MIXER_SCALING, -pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
// Calculate and Limit the PIDsum
scaledAxisPIDf[FD_ROLL] =
constrainf((axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimit);
scaledAxisPIDf[FD_PITCH] =
constrainf((axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimit);
scaledAxisPIDf[FD_YAW] =
constrainf((axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) / PID_MIXER_SCALING,
-pidProfile->pidSumLimit, pidProfile->pidSumLimitYaw);
// Calculate voltage compensation
const float vbatCompensationFactor = pidProfile->vbatPidCompensation ? calculateVbatPidCompensation() : 1.0f;
const float vbatCompensationFactor = (pidProfile->vbatPidCompensation) ? calculateVbatPidCompensation() : 1.0f;
// Find roll/pitch/yaw desired output
float motorMix[MAX_SUPPORTED_MOTORS];

52
src/main/flight/pid.c
View File

@ -51,13 +51,7 @@
uint32_t targetPidLooptime;
static bool pidStabilisationEnabled;
float axisPIDf[3];
#ifdef BLACKBOX
int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
#endif
static float previousGyroIf[3];
float axisPID_P[3], axisPID_I[3], axisPID_D[3];
static float dT;
@ -145,7 +139,7 @@ void pidSetTargetLooptime(uint32_t pidLooptime)
void pidResetErrorGyroState(void)
{
for (int axis = 0; axis < 3; axis++) {
previousGyroIf[axis] = 0.0f;
axisPID_I[axis] = 0.0f;
}
}
@ -335,56 +329,40 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
const float errorRate = currentPidSetpoint - gyroRate; // r - y
// -----calculate P component and add Dynamic Part based on stick input
float PTerm = Kp[axis] * errorRate * tpaFactor;
axisPID_P[axis] = Kp[axis] * errorRate * tpaFactor;
if (axis == FD_YAW) {
PTerm = ptermYawFilterApplyFn(ptermYawFilter, PTerm);
axisPID_P[axis] = ptermYawFilterApplyFn(ptermYawFilter, axisPID_P[axis]);
}
// -----calculate I component
float ITerm = previousGyroIf[axis];
if (motorMixRange < 1.0f) {
// Only increase ITerm if motor output is not saturated
ITerm += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
previousGyroIf[axis] = ITerm;
axisPID_I[axis] += Ki[axis] * errorRate * dT * dynKi * itermAccelerator;
}
// -----calculate D component
if (axis == FD_YAW) {
// no DTerm for yaw axis
// -----calculate total PID output
axisPIDf[FD_YAW] = PTerm + ITerm;
#ifdef BLACKBOX
axisPID_P[FD_YAW] = PTerm;
axisPID_I[FD_YAW] = ITerm;
axisPID_D[FD_YAW] = 0;
#endif
} else {
if (axis != FD_YAW) {
float dynC = dtermSetpointWeight;
if (pidProfile->setpointRelaxRatio < 100) {
dynC *= MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f);
}
const float rD = dynC * currentPidSetpoint - gyroRate; // cr - y
// Divide rate change by dT to get differential (ie dr/dt)
const float delta = (rD - previousRateError[axis]) / dT;
float delta = (rD - previousRateError[axis]) / dT;
previousRateError[axis] = rD;
float DTerm = Kd[axis] * delta * tpaFactor;
DEBUG_SET(DEBUG_DTERM_FILTER, axis, DTerm);
// apply filters
DTerm = dtermNotchFilterApplyFn(dtermFilterNotch[axis], DTerm);
DTerm = dtermLpfApplyFn(dtermFilterLpf[axis], DTerm);
delta = dtermNotchFilterApplyFn(dtermFilterNotch[axis], delta);
delta = dtermLpfApplyFn(dtermFilterLpf[axis], delta);
// -----calculate total PID output
axisPIDf[axis] = PTerm + ITerm + DTerm;
#ifdef BLACKBOX
axisPID_P[axis] = PTerm;
axisPID_I[axis] = ITerm;
axisPID_D[axis] = DTerm;
#endif
axisPID_D[axis] = Kd[axis] * delta * tpaFactor;
}
// Disable PID control at zero throttle
if (!pidStabilisationEnabled) axisPIDf[axis] = 0;
if (!pidStabilisationEnabled) {
axisPID_P[axis] = 0;
axisPID_I[axis] = 0;
axisPID_D[axis] = 0;
}
}
}

3
src/main/flight/pid.h
View File

@ -101,8 +101,7 @@ PG_DECLARE(pidConfig_t, pidConfig);
union rollAndPitchTrims_u;
void pidController(const pidProfile_t *pidProfile, const union rollAndPitchTrims_u *angleTrim);
extern float axisPIDf[3];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
extern float axisPID_P[3], axisPID_I[3], axisPID_D[3];
bool airmodeWasActivated;
extern uint32_t targetPidLooptime;

6
src/main/flight/servos.c
View File

@ -379,9 +379,9 @@ STATIC_UNIT_TESTED void servoMixer(void)
input[INPUT_STABILIZED_YAW] = rcCommand[YAW];
} else {
// Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui
input[INPUT_STABILIZED_ROLL] = axisPIDf[ROLL] * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_PITCH] = axisPIDf[PITCH] * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_YAW] = axisPIDf[YAW] * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_ROLL] = (axisPID_P[FD_ROLL] + axisPID_I[FD_ROLL] + axisPID_D[FD_ROLL]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_PITCH] = (axisPID_P[FD_PITCH] + axisPID_I[FD_PITCH] + axisPID_D[FD_PITCH]) * PID_SERVO_MIXER_SCALING;
input[INPUT_STABILIZED_YAW] = (axisPID_P[FD_YAW] + axisPID_I[FD_YAW]) * PID_SERVO_MIXER_SCALING;
// Reverse yaw servo when inverted in 3D mode
if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig()->midrc)) {