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

Merge remote-tracking branch 'origin/development' into dzikuvx-yaw-handling-cleanup

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2019-11-30 09:39:17 +01:00
commit f8717e19d5
18 changed files with 245 additions and 332 deletions
Download patch file Download diff file Expand all files Collapse all files

10
src/main/build/debug.h
View file

@ -51,29 +51,19 @@ extern timeUs_t sectionTimes[2][4];
typedef enum {
DEBUG_NONE,
DEBUG_GYRO,
DEBUG_NOTCH,
DEBUG_NAV_LANDING_DETECTOR,
DEBUG_FW_CLIMB_RATE_TO_ALTITUDE,
DEBUG_AGL,
DEBUG_FLOW_RAW,
DEBUG_FLOW,
DEBUG_SBUS,
DEBUG_FPORT,
DEBUG_ALWAYS,
DEBUG_STAGE2,
DEBUG_SAG_COMP_VOLTAGE,
DEBUG_VIBE,
DEBUG_CRUISE,
DEBUG_REM_FLIGHT_TIME,
DEBUG_SMARTAUDIO,
DEBUG_ACC,
DEBUG_GENERIC,
DEBUG_ITERM_RELAX,
DEBUG_D_BOOST,
DEBUG_ANTIGRAVITY,
DEBUG_FFT,
DEBUG_FFT_TIME,
DEBUG_FFT_FREQ,
DEBUG_ERPM,
DEBUG_COUNT
} debugType_e;

3
src/main/common/maths.c
View file

@ -23,6 +23,7 @@
#include "maths.h"
#include "vector.h"
#include "quaternion.h"
#include "platform.h"
// http://lolengine.net/blog/2011/12/21/better-function-approximations
// Chebyshev http://stackoverflow.com/questions/345085/how-do-trigonometric-functions-work/345117#345117
@ -158,7 +159,7 @@ int constrain(int amt, int low, int high)
return amt;
}
float constrainf(float amt, float low, float high)
float FAST_CODE NOINLINE constrainf(float amt, float low, float high)
{
if (amt < low)
return low;

4
src/main/fc/cli.c
View file

@ -1612,8 +1612,8 @@ static void cliServo(char *cmdline)
servo = servoParamsMutable(i);
if (
arguments[MIN] < PWM_PULSE_MIN || arguments[MIN] > PWM_PULSE_MAX ||
arguments[MAX] < PWM_PULSE_MIN || arguments[MAX] > PWM_PULSE_MAX ||
arguments[MIN] < SERVO_OUTPUT_MIN || arguments[MIN] > SERVO_OUTPUT_MAX ||
arguments[MAX] < SERVO_OUTPUT_MIN || arguments[MAX] > SERVO_OUTPUT_MAX ||
arguments[MIDDLE] < arguments[MIN] || arguments[MIDDLE] > arguments[MAX] ||
arguments[MIN] > arguments[MAX] || arguments[MAX] < arguments[MIN] ||
arguments[RATE] < -125 || arguments[RATE] > 125

9
src/main/fc/config.c
View file

@ -167,27 +167,18 @@ uint32_t getLooptime(void) {
void validateAndFixConfig(void)
{
#ifdef USE_GYRO_NOTCH_1
if (gyroConfig()->gyro_soft_notch_cutoff_1 >= gyroConfig()->gyro_soft_notch_hz_1) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = 0;
}
#endif
#ifdef USE_GYRO_NOTCH_2
if (gyroConfig()->gyro_soft_notch_cutoff_2 >= gyroConfig()->gyro_soft_notch_hz_2) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = 0;
}
#endif
#ifdef USE_DTERM_NOTCH
if (pidProfile()->dterm_soft_notch_cutoff >= pidProfile()->dterm_soft_notch_hz) {
pidProfileMutable()->dterm_soft_notch_hz = 0;
}
#endif
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_cutoff >= accelerometerConfig()->acc_notch_hz) {
accelerometerConfigMutable()->acc_notch_hz = 0;
}
#endif
// Disable unused features
featureClear(FEATURE_UNUSED_3 | FEATURE_UNUSED_4 | FEATURE_UNUSED_5 | FEATURE_UNUSED_6 | FEATURE_UNUSED_7 | FEATURE_UNUSED_8 | FEATURE_UNUSED_9 | FEATURE_UNUSED_10);

49
src/main/fc/fc_msp.c
View file

@ -1101,43 +1101,19 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, gyroConfig()->gyro_soft_lpf_hz);
sbufWriteU16(dst, pidProfile()->dterm_lpf_hz);
sbufWriteU16(dst, pidProfile()->yaw_lpf_hz);
#ifdef USE_GYRO_NOTCH_1
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_1); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_1); //BF: masterConfig.gyro_soft_notch_cutoff_1
#else
sbufWriteU16(dst, 0); //masterConfig.gyro_soft_notch_hz_1
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_1
#endif
#ifdef USE_DTERM_NOTCH
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_hz); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, pidProfile()->dterm_soft_notch_cutoff); //pidProfile()->dterm_notch_cutoff
#else
sbufWriteU16(dst, 0); //BF: pidProfile()->dterm_notch_hz
sbufWriteU16(dst, 1); //pidProfile()->dterm_notch_cutoff
#endif
#ifdef USE_GYRO_NOTCH_2
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_hz_2); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, gyroConfig()->gyro_soft_notch_cutoff_2); //BF: masterConfig.gyro_soft_notch_cutoff_2
#else
sbufWriteU16(dst, 0); //BF: masterConfig.gyro_soft_notch_hz_2
sbufWriteU16(dst, 1); //BF: masterConfig.gyro_soft_notch_cutoff_2
#endif
#ifdef USE_ACC_NOTCH
sbufWriteU16(dst, accelerometerConfig()->acc_notch_hz);
sbufWriteU16(dst, accelerometerConfig()->acc_notch_cutoff);
#else
sbufWriteU16(dst, 0);
sbufWriteU16(dst, 1);
#endif
#ifdef USE_GYRO_BIQUAD_RC_FIR2
sbufWriteU16(dst, gyroConfig()->gyro_stage2_lowpass_hz);
#else
sbufWriteU16(dst, 0);
#endif
break;
case MSP_PID_ADVANCED:
@ -2017,43 +1993,38 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
gyroConfigMutable()->gyro_soft_lpf_hz = sbufReadU8(src);
pidProfileMutable()->dterm_lpf_hz = constrain(sbufReadU16(src), 0, 255);
pidProfileMutable()->yaw_lpf_hz = constrain(sbufReadU16(src), 0, 255);
#ifdef USE_GYRO_NOTCH_1
if (dataSize >= 9) {
gyroConfigMutable()->gyro_soft_notch_hz_1 = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_soft_notch_cutoff_1 = constrain(sbufReadU16(src), 1, 500);
} else
} else {
return MSP_RESULT_ERROR;
#endif
#ifdef USE_DTERM_NOTCH
}
if (dataSize >= 13) {
pidProfileMutable()->dterm_soft_notch_hz = constrain(sbufReadU16(src), 0, 500);
pidProfileMutable()->dterm_soft_notch_cutoff = constrain(sbufReadU16(src), 1, 500);
pidInitFilters();
} else
} else {
return MSP_RESULT_ERROR;
#endif
#ifdef USE_GYRO_NOTCH_2
}
if (dataSize >= 17) {
gyroConfigMutable()->gyro_soft_notch_hz_2 = constrain(sbufReadU16(src), 0, 500);
gyroConfigMutable()->gyro_soft_notch_cutoff_2 = constrain(sbufReadU16(src), 1, 500);
} else
} else {
return MSP_RESULT_ERROR;
#endif
}
#ifdef USE_ACC_NOTCH
if (dataSize >= 21) {
accelerometerConfigMutable()->acc_notch_hz = constrain(sbufReadU16(src), 0, 255);
accelerometerConfigMutable()->acc_notch_cutoff = constrain(sbufReadU16(src), 1, 255);
} else
} else {
return MSP_RESULT_ERROR;
#endif
}
#ifdef USE_GYRO_BIQUAD_RC_FIR2
if (dataSize >= 22) {
gyroConfigMutable()->gyro_stage2_lowpass_hz = constrain(sbufReadU16(src), 0, 500);
} else
} else {
return MSP_RESULT_ERROR;
#endif
}
} else
return MSP_RESULT_ERROR;
break;

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

@ -78,10 +78,10 @@ tables:
- name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX",
"D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "ERPM"]
values: ["NONE", "GYRO", "AGL", "FLOW_RAW",
"FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE",
"VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ITERM_RELAX",
"ERPM"]
- name: async_mode
values: ["NONE", "GYRO", "ALL"]
- name: aux_operator
@ -135,6 +135,9 @@ tables:
- name: dynamicFilterRangeTable
values: ["HIGH", "MEDIUM", "LOW"]
enum: dynamicFilterRange_e
- name: pidTypeTable
values: ["NONE", "PID", "PIFF", "AUTO"]
enum: pidType_e
groups:
- name: PG_GYRO_CONFIG
@ -164,25 +167,20 @@ groups:
max: 128
- name: gyro_notch1_hz
field: gyro_soft_notch_hz_1
condition: USE_GYRO_NOTCH_1
max: 500
- name: gyro_notch1_cutoff
field: gyro_soft_notch_cutoff_1
condition: USE_GYRO_NOTCH_1
min: 1
max: 500
- name: gyro_notch2_hz
field: gyro_soft_notch_hz_2
condition: USE_GYRO_NOTCH_2
max: 500
- name: gyro_notch2_cutoff
field: gyro_soft_notch_cutoff_2
condition: USE_GYRO_NOTCH_2
min: 1
max: 500
- name: gyro_stage2_lowpass_hz
field: gyro_stage2_lowpass_hz
condition: USE_GYRO_BIQUAD_RC_FIR2
min: 0
max: 500
- name: dyn_notch_width_percent
@ -236,11 +234,9 @@ groups:
headers: ["sensors/acceleration.h"]
members:
- name: acc_notch_hz
condition: USE_ACC_NOTCH
min: 0
max: 255
- name: acc_notch_cutoff
condition: USE_ACC_NOTCH
min: 1
max: 255
- name: align_acc
@ -1073,12 +1069,10 @@ groups:
max: 1
- name: dterm_notch_hz
field: dterm_soft_notch_hz
condition: USE_DTERM_NOTCH
min: 0
max: 500
- name: dterm_notch_cutoff
field: dterm_soft_notch_cutoff
condition: USE_DTERM_NOTCH
min: 1
max: 500
- name: pidsum_limit
@ -1232,6 +1226,9 @@ groups:
condition: USE_ANTIGRAVITY
min: 1
max: 30
- name: pid_type
field: pidControllerType
table: pidTypeTable
- name: PG_PID_AUTOTUNE_CONFIG
type: pidAutotuneConfig_t

26
src/main/flight/gyroanalyse.c
View file

@ -157,9 +157,6 @@ void gyroDataAnalyse(gyroAnalyseState_t *state, biquadFilter_t *notchFilterDyn,
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
float sample = state->oversampledGyroAccumulator[axis] * state->maxSampleCountRcp;
state->downsampledGyroData[axis][state->circularBufferIdx] = sample;
if (axis == 0) {
DEBUG_SET(DEBUG_FFT, 2, lrintf(sample));
}
state->oversampledGyroAccumulator[axis] = 0;
}
@ -201,12 +198,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
arm_cfft_instance_f32 *Sint = &(state->fftInstance.Sint);
uint32_t startTime = 0;
if (debugMode == (DEBUG_FFT_TIME)) {
startTime = micros();
}
DEBUG_SET(DEBUG_FFT_TIME, 0, state->updateStep);
switch (state->updateStep) {
case STEP_ARM_CFFT_F32:
{
@ -224,14 +215,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
arm_radix8_butterfly_f32(state->fftData, FFT_BIN_COUNT, Sint->pTwiddle, 1);
break;
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_BITREVERSAL:
{
// 6us
arm_bitreversal_32((uint32_t*) state->fftData, Sint->bitRevLength, Sint->pBitRevTable);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateStep++;
FALLTHROUGH;
}
@ -240,14 +229,12 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// 14us
// this does not work in place => fftData AND rfftData needed
stage_rfft_f32(&state->fftInstance, state->fftData, state->rfftData);
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_ARM_CMPLX_MAG_F32:
{
// 8us
arm_cmplx_mag_f32(state->rfftData, state->fftData, FFT_BIN_COUNT);
DEBUG_SET(DEBUG_FFT_TIME, 2, micros() - startTime);
state->updateStep++;
FALLTHROUGH;
}
@ -312,15 +299,7 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
dynNotchMaxFFT = MAX(dynNotchMaxFFT, state->centerFreq[state->updateAxis]);
if (state->updateAxis == 0) {
DEBUG_SET(DEBUG_FFT, 3, lrintf(fftMeanIndex * 100));
DEBUG_SET(DEBUG_FFT_FREQ, 0, state->centerFreq[state->updateAxis]);
}
if (state->updateAxis == 1) {
DEBUG_SET(DEBUG_FFT_FREQ, 1, state->centerFreq[state->updateAxis]);
}
// Debug FFT_Freq carries raw gyro, gyro after first filter set, FFT centre for roll and for pitch
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
break;
}
case STEP_UPDATE_FILTERS:
@ -329,8 +308,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
// calculate cutoffFreq and notch Q, update notch filter =1.8+((A2-150)*0.004)
if (state->prevCenterFreq[state->updateAxis] != state->centerFreq[state->updateAxis]) {
DEBUG_SET(DEBUG_FFT_FREQ, state->updateAxis + 5, state->centerFreq[state->updateAxis]);
if (dualNotch) {
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch1Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
biquadFilterUpdate(&notchFilterDyn2[state->updateAxis], state->centerFreq[state->updateAxis] * dynNotch2Ctr, getLooptime(), dynNotchQ, FILTER_NOTCH);
@ -338,7 +315,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
biquadFilterUpdate(&notchFilterDyn[state->updateAxis], state->centerFreq[state->updateAxis], getLooptime(), dynNotchQ, FILTER_NOTCH);
}
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
state->updateAxis = (state->updateAxis + 1) % XYZ_AXIS_COUNT;
state->updateStep++;
@ -354,8 +330,6 @@ static NOINLINE void gyroDataAnalyseUpdate(gyroAnalyseState_t *state, biquadFilt
if (state->circularBufferIdx > 0) {
arm_mult_f32(&state->downsampledGyroData[state->updateAxis][0], &hanningWindow[ringBufIdx], &state->fftData[ringBufIdx], state->circularBufferIdx);
}
DEBUG_SET(DEBUG_FFT_TIME, 1, micros() - startTime);
}
}

92
src/main/flight/mixer.c
View file

@ -109,6 +109,9 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
typedef void (*motorRateLimitingApplyFnPtr)(const float dT);
static EXTENDED_FASTRAM motorRateLimitingApplyFnPtr motorRateLimitingApplyFn;
static void computeMotorCount(void)
{
motorCount = 0;
@ -153,6 +156,49 @@ void mixerUpdateStateFlags(void)
}
}
void nullMotorRateLimiting(const float dT)
{
UNUSED(dT);
}
void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
}
}
else {
// Calculate max motor step
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
for (int i = 0; i < motorCount; i++) {
// Apply motor rate limiting
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
// Handle throttle below min_throttle (motor start/stop)
if (motorPrevious[i] < motorConfig()->minthrottle) {
if (motor[i] < motorConfig()->minthrottle) {
motorPrevious[i] = motor[i];
}
else {
motorPrevious[i] = motorConfig()->minthrottle;
}
}
}
}
// Update motor values
for (int i = 0; i < motorCount; i++) {
motor[i] = motorPrevious[i];
}
}
void mixerInit(void)
{
computeMotorCount();
@ -163,6 +209,12 @@ void mixerInit(void)
}
mixerResetDisarmedMotors();
if (motorConfig()->motorAccelTimeMs || motorConfig()->motorDecelTimeMs) {
motorRateLimitingApplyFn = applyMotorRateLimiting;
} else {
motorRateLimitingApplyFn = nullMotorRateLimiting;
}
}
void mixerResetDisarmedMotors(void)
@ -239,44 +291,6 @@ void stopPwmAllMotors(void)
pwmShutdownPulsesForAllMotors(motorCount);
}
static void applyMotorRateLimiting(const float dT)
{
static float motorPrevious[MAX_SUPPORTED_MOTORS] = { 0 };
if (feature(FEATURE_3D)) {
// FIXME: Don't apply rate limiting in 3D mode
for (int i = 0; i < motorCount; i++) {
motorPrevious[i] = motor[i];
}
}
else {
// Calculate max motor step
const uint16_t motorRange = motorConfig()->maxthrottle - motorConfig()->minthrottle;
const float motorMaxInc = (motorConfig()->motorAccelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorAccelTimeMs * 1e-3f);
const float motorMaxDec = (motorConfig()->motorDecelTimeMs == 0) ? 2000 : motorRange * dT / (motorConfig()->motorDecelTimeMs * 1e-3f);
for (int i = 0; i < motorCount; i++) {
// Apply motor rate limiting
motorPrevious[i] = constrainf(motor[i], motorPrevious[i] - motorMaxDec, motorPrevious[i] + motorMaxInc);
// Handle throttle below min_throttle (motor start/stop)
if (motorPrevious[i] < motorConfig()->minthrottle) {
if (motor[i] < motorConfig()->minthrottle) {
motorPrevious[i] = motor[i];
}
else {
motorPrevious[i] = motorConfig()->minthrottle;
}
}
}
}
// Update motor values
for (int i = 0; i < motorCount; i++) {
motor[i] = motorPrevious[i];
}
}
void FAST_CODE NOINLINE mixTable(const float dT)
{
int16_t input[3]; // RPY, range [-500:+500]
@ -408,7 +422,7 @@ void FAST_CODE NOINLINE mixTable(const float dT)
}
/* Apply motor acceleration/deceleration limit */
applyMotorRateLimiting(dT);
motorRateLimitingApplyFn(dT);
}
motorStatus_e getMotorStatus(void)

249
src/main/flight/pid.c
View file

@ -81,11 +81,8 @@ typedef struct {
rateLimitFilter_t axisAccelFilter;
pt1Filter_t ptermLpfState;
biquadFilter_t deltaLpfState;
// Dterm notch filtering
#ifdef USE_DTERM_NOTCH
biquadFilter_t deltaNotchFilter;
#endif
float stickPosition;
#ifdef USE_D_BOOST
@ -96,17 +93,15 @@ typedef struct {
#endif
uint16_t pidSumLimit;
filterApply4FnPtr ptermFilterApplyFn;
bool itermLimitActive;
} pidState_t;
#ifdef USE_DTERM_NOTCH
STATIC_FASTRAM filterApplyFnPtr notchFilterApplyFn;
#endif
STATIC_FASTRAM bool pidFiltersConfigured = false;
FASTRAM float headingHoldCosZLimit;
FASTRAM int16_t headingHoldTarget;
STATIC_FASTRAM pt1Filter_t headingHoldRateFilter;
STATIC_FASTRAM pt1Filter_t fixedWingTpaFilter;
static EXTENDED_FASTRAM float headingHoldCosZLimit;
static EXTENDED_FASTRAM int16_t headingHoldTarget;
static EXTENDED_FASTRAM pt1Filter_t headingHoldRateFilter;
static EXTENDED_FASTRAM pt1Filter_t fixedWingTpaFilter;
// Thrust PID Attenuation factor. 0.0f means fully attenuated, 1.0f no attenuation is applied
STATIC_FASTRAM bool pidGainsUpdateRequired;
@ -139,6 +134,16 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration;
#endif
static EXTENDED_FASTRAM uint8_t yawLpfHz;
static EXTENDED_FASTRAM float motorItermWindupPoint;
static EXTENDED_FASTRAM float antiWindupScaler;
#ifdef USE_ANTIGRAVITY
static EXTENDED_FASTRAM float iTermAntigravityGain;
#endif
static EXTENDED_FASTRAM uint8_t usedPidControllerType;
typedef void (*pidControllerFnPtr)(pidState_t *pidState, flight_dynamics_index_t axis, float dT);
static EXTENDED_FASTRAM pidControllerFnPtr pidControllerApplyFn;
static EXTENDED_FASTRAM filterApplyFnPtr dTermLpfFilterApplyFn;
PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE, 11);
@ -244,49 +249,9 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile,
.antigravityGain = 1.0f,
.antigravityAccelerator = 1.0f,
.antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF,
.pidControllerType = PID_TYPE_AUTO,
);
void pidInit(void)
{
pidResetTPAFilter();
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
yawLpfHz = pidProfile()->yaw_lpf_hz;
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
#endif
#ifdef USE_ANTIGRAVITY
antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (axis == FD_YAW) {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
if (yawLpfHz) {
pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
} else {
pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
}
} else {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimit;
pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
}
}
}
bool pidInitFilters(void)
{
const uint32_t refreshRate = getLooptime();
@ -319,7 +284,6 @@ bool pidInitFilters(void)
firFilterInit(&pidState[axis].gyroRateFilter, pidState[axis].gyroRateBuf, PID_GYRO_RATE_BUF_LENGTH, dtermCoeffs);
}
#ifdef USE_DTERM_NOTCH
notchFilterApplyFn = nullFilterApply;
if (pidProfile()->dterm_soft_notch_hz != 0) {
notchFilterApplyFn = (filterApplyFnPtr)biquadFilterApply;
@ -327,7 +291,6 @@ bool pidInitFilters(void)
biquadFilterInitNotch(&pidState[axis].deltaNotchFilter, refreshRate, pidProfile()->dterm_soft_notch_hz, pidProfile()->dterm_soft_notch_cutoff);
}
}
#endif
// Init other filters
for (int axis = 0; axis < 3; ++ axis) {
@ -355,7 +318,7 @@ bool pidInitFilters(void)
void pidResetTPAFilter(void)
{
if (STATE(FIXED_WING) && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
if (usedPidControllerType == PID_TYPE_PIFF && currentControlRateProfile->throttle.fixedWingTauMs > 0) {
pt1FilterInitRC(&fixedWingTpaFilter, currentControlRateProfile->throttle.fixedWingTauMs * 1e-3f, getLooptime() * 1e-6f);
pt1FilterReset(&fixedWingTpaFilter, motorConfig()->minthrottle);
}
@ -453,7 +416,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
STATIC_FASTRAM uint16_t prevThrottle = 0;
// Check if throttle changed. Different logic for fixed wing vs multirotor
if (STATE(FIXED_WING) && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
if (usedPidControllerType == PID_TYPE_PIFF && (currentControlRateProfile->throttle.fixedWingTauMs > 0)) {
uint16_t filteredThrottle = pt1FilterApply3(&fixedWingTpaFilter, rcCommand[THROTTLE], dT);
if (filteredThrottle != prevThrottle) {
prevThrottle = filteredThrottle;
@ -468,7 +431,7 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
}
#ifdef USE_ANTIGRAVITY
if (!STATE(FIXED_WING)) {
if (usedPidControllerType == PID_TYPE_PID) {
antigravityThrottleHpf = rcCommand[THROTTLE] - pt1FilterApply(&antigravityThrottleLpf, rcCommand[THROTTLE]);
}
#endif
@ -485,12 +448,12 @@ void FAST_CODE NOINLINE updatePIDCoefficients(float dT)
return;
}
const float tpaFactor = STATE(FIXED_WING) ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
const float tpaFactor = usedPidControllerType == PID_TYPE_PIFF ? calculateFixedWingTPAFactor(prevThrottle) : calculateMultirotorTPAFactor();
// PID coefficients can be update only with THROTTLE and TPA or inflight PID adjustments
//TODO: Next step would be to update those only at THROTTLE or inflight adjustments change
for (int axis = 0; axis < 3; axis++) {
if (STATE(FIXED_WING)) {
if (usedPidControllerType == PID_TYPE_PIFF) {
// Airplanes - scale all PIDs according to TPA
pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * tpaFactor;
pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER * tpaFactor;
@ -604,6 +567,20 @@ static FAST_CODE NOINLINE float pTermProcess(pidState_t *pidState, float rateErr
return pidState->ptermFilterApplyFn(&pidState->ptermLpfState, newPTerm, yawLpfHz, dT);
}
static void applyItermLimiting(pidState_t *pidState) {
if (pidState->itermLimitActive) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
}
static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
}
static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
@ -613,11 +590,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh
// Calculate integral
pidState->errorGyroIf += rateError * pidState->kI * dT;
if (STATE(ANTI_WINDUP) || isFixedWingItermLimitActive(pidState->stickPosition)) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
applyItermLimiting(pidState);
if (pidProfile()->fixedWingItermThrowLimit != 0) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit);
@ -666,7 +639,7 @@ static void FAST_CODE applyItermRelax(const int axis, const float gyroRate, floa
}
}
#ifdef USE_D_BOOST
static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
static float FAST_CODE applyDBoost(pidState_t *pidState, float dT) {
float dBoost = 1.0f;
@ -680,14 +653,6 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
dBoost = pt1FilterApply4(&pidState->dBoostLpf, dBoost, D_BOOST_LPF_HZ, dT);
dBoost = constrainf(dBoost, 1.0f, dBoostFactor);
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_D_BOOST, 0, dBoostGyroAcceleration);
DEBUG_SET(DEBUG_D_BOOST, 1, dBoostRateAcceleration);
DEBUG_SET(DEBUG_D_BOOST, 2, dBoost * 100);
} else if (axis == FD_PITCH) {
DEBUG_SET(DEBUG_D_BOOST, 3, dBoost * 100);
}
pidState->previousRateTarget = pidState->rateTarget;
pidState->previousRateGyro = pidState->gyroRate;
}
@ -695,43 +660,38 @@ static float FAST_CODE applyDBoost(pidState_t *pidState, flight_dynamics_index_t
return dBoost;
}
#else
static float applyDBoost(pidState_t *pidState, flight_dynamics_index_t axis, float dT) {
static float applyDBoost(pidState_t *pidState, float dT) {
UNUSED(pidState);
UNUSED(axis);
UNUSED(dT);
return 1.0f;
}
#endif
static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT)
{
const float rateError = pidState->rateTarget - pidState->gyroRate;
const float newPTerm = pTermProcess(pidState, rateError, dT);
// Calculate new D-term
float newDTerm;
if (pidBank()->pid[axis].D == 0) {
if (pidState->kD == 0) {
// optimisation for when D8 is zero, often used by YAW axis
newDTerm = 0;
} else {
// Calculate delta for Dterm calculation. Apply filters before derivative to minimize effects of dterm kick
float deltaFiltered = pidProfile()->dterm_setpoint_weight * pidState->rateTarget - pidState->gyroRate;
#ifdef USE_DTERM_NOTCH
// Apply D-term notch
deltaFiltered = notchFilterApplyFn(&pidState->deltaNotchFilter, deltaFiltered);
#endif
// Apply additional lowpass
if (pidProfile()->dterm_lpf_hz) {
deltaFiltered = biquadFilterApply(&pidState->deltaLpfState, deltaFiltered);
}
deltaFiltered = dTermLpfFilterApplyFn(&pidState->deltaLpfState, deltaFiltered);
firFilterUpdate(&pidState->gyroRateFilter, deltaFiltered);
newDTerm = firFilterApply(&pidState->gyroRateFilter);
// Calculate derivative
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, axis, dT);
newDTerm = newDTerm * (pidState->kD / dT) * applyDBoost(pidState, dT);
// Additionally constrain D
newDTerm = constrainf(newDTerm, -300.0f, 300.0f);
@ -741,17 +701,10 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf;
const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit);
// Prevent strong Iterm accumulation during stick inputs
const float motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
const float antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
float itermErrorRate = rateError;
applyItermRelax(axis, pidState->gyroRate, pidState->rateTarget, &itermErrorRate);
#ifdef USE_ANTIGRAVITY
const float iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
DEBUG_SET(DEBUG_ANTIGRAVITY, 0, iTermAntigravityGain * 100);
DEBUG_SET(DEBUG_ANTIGRAVITY, 1, antigravityThrottleHpf);
itermErrorRate *= iTermAntigravityGain;
#endif
@ -759,11 +712,7 @@ static void FAST_CODE pidApplyMulticopterRateController(pidState_t *pidState, fl
+ ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT);
// Don't grow I-term if motors are at their limit
if (STATE(ANTI_WINDUP) || mixerIsOutputSaturated()) {
pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit);
} else {
pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf);
}
applyItermLimiting(pidState);
axisPID[axis] = newOutputLimited;
@ -955,7 +904,20 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng
pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT);
}
void FAST_CODE pidController(float dT)
void FAST_CODE checkItermLimitingActive(pidState_t *pidState)
{
bool shouldActivate;
if (usedPidControllerType == PID_TYPE_PIFF) {
shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition);
} else
{
shouldActivate = mixerIsOutputSaturated();
}
pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate;
}
void FAST_CODE NOINLINE pidController(float dT)
{
if (!pidFiltersConfigured) {
return;
@ -1006,30 +968,29 @@ void FAST_CODE pidController(float dT)
pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees);
}
// Apply setpoint rate of change limits
for (int axis = 0; axis < 3; axis++) {
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
}
// Prevent strong Iterm accumulation during stick inputs
antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f);
#ifdef USE_ANTIGRAVITY
iTermAntigravityGain = scaleRangef(fabsf(antigravityThrottleHpf) * antigravityAccelerator, 0.0f, 1000.0f, 1.0f, antigravityGain);
#endif
// Step 4: Run gyro-driven control
for (int axis = 0; axis < 3; axis++) {
// Apply PID setpoint controller
if (STATE(FIXED_WING)) {
pidApplyFixedWingRateController(&pidState[axis], axis, dT);
}
else {
pidApplyMulticopterRateController(&pidState[axis], axis, dT);
}
// Apply setpoint rate of change limits
pidApplySetpointRateLimiting(&pidState[axis], axis, dT);
// Step 4: Run gyro-driven control
checkItermLimitingActive(&pidState[axis]);
pidControllerApplyFn(&pidState[axis], axis, dT);
}
}
pidType_e pidIndexGetType(pidIndex_e pidIndex)
{
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return usedPidControllerType;
}
if (STATE(FIXED_WING)) {
// FW specific
if (pidIndex == PID_ROLL || pidIndex == PID_PITCH || pidIndex == PID_YAW) {
return PID_TYPE_PIFF;
}
if (pidIndex == PID_VEL_XY || pidIndex == PID_VEL_Z) {
return PID_TYPE_NONE;
}
@ -1040,3 +1001,69 @@ pidType_e pidIndexGetType(pidIndex_e pidIndex)
}
return PID_TYPE_PID;
}
void pidInit(void)
{
pidResetTPAFilter();
// Calculate max overall tilt (max pitch + max roll combined) as a limit to heading hold
headingHoldCosZLimit = cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_ROLL])) *
cos_approx(DECIDEGREES_TO_RADIANS(pidProfile()->max_angle_inclination[FD_PITCH]));
pidGainsUpdateRequired = false;
itermRelax = pidProfile()->iterm_relax;
itermRelaxType = pidProfile()->iterm_relax_type;
itermRelaxSetpointThreshold = MC_ITERM_RELAX_SETPOINT_THRESHOLD * MC_ITERM_RELAX_CUTOFF_DEFAULT / pidProfile()->iterm_relax_cutoff;
yawLpfHz = pidProfile()->yaw_lpf_hz;
motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f);
#ifdef USE_D_BOOST
dBoostFactor = pidProfile()->dBoostFactor;
dBoostMaxAtAlleceleration = pidProfile()->dBoostMaxAtAlleceleration;
#endif
#ifdef USE_ANTIGRAVITY
antigravityGain = pidProfile()->antigravityGain;
antigravityAccelerator = pidProfile()->antigravityAccelerator;
#endif
for (uint8_t axis = FD_ROLL; axis <= FD_YAW; axis++) {
if (axis == FD_YAW) {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimitYaw;
if (yawLpfHz) {
pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) pt1FilterApply4;
} else {
pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
}
} else {
pidState[axis].pidSumLimit = pidProfile()->pidSumLimit;
pidState[axis].ptermFilterApplyFn = (filterApply4FnPtr) nullFilterApply4;
}
}
if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
if (STATE(FIXED_WING)) {
usedPidControllerType = PID_TYPE_PIFF;
} else {
usedPidControllerType = PID_TYPE_PID;
}
} else {
usedPidControllerType = pidProfile()->pidControllerType;
}
if (pidProfile()->dterm_lpf_hz) {
dTermLpfFilterApplyFn = (filterApplyFnPtr) biquadFilterApply;
} else {
dTermLpfFilterApplyFn = nullFilterApply;
}
if (usedPidControllerType == PID_TYPE_PIFF) {
pidControllerApplyFn = pidApplyFixedWingRateController;
} else if (usedPidControllerType == PID_TYPE_PID) {
pidControllerApplyFn = pidApplyMulticopterRateController;
} else {
pidControllerApplyFn = nullRateController;
}
}

14
src/main/flight/pid.h
View file

@ -70,9 +70,10 @@ typedef enum {
// TODO(agh): PIDFF
typedef enum {
PID_TYPE_NONE, // Not used in the current platform/mixer/configuration
PID_TYPE_NONE = 0, // Not used in the current platform/mixer/configuration
PID_TYPE_PID, // Uses P, I and D terms
PID_TYPE_PIFF, // Uses P, I and FF, ignoring D
PID_TYPE_AUTO, // Autodetect
} pidType_e;
typedef struct pid8_s {
@ -98,6 +99,7 @@ typedef enum {
} itermRelaxType_e;
typedef struct pidProfile_s {
uint8_t pidControllerType;
pidBank_t bank_fw;
pidBank_t bank_mc;
@ -153,18 +155,16 @@ typedef struct pidAutotuneConfig_s {
PG_DECLARE_PROFILE(pidProfile_t, pidProfile);
PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig);
static inline const pidBank_t * pidBank(void) { return STATE(FIXED_WING) ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
static inline pidBank_t * pidBankMutable(void) { return STATE(FIXED_WING) ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
static uint8_t usedPidControllerType;
static inline const pidBank_t * pidBank(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfile()->bank_fw : &pidProfile()->bank_mc; }
static inline pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; }
extern int16_t axisPID[];
extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[];
void pidInit(void);
#ifdef USE_DTERM_NOTCH
bool pidInitFilters(void);
#endif
void pidResetErrorAccumulators(void);
void pidResetTPAFilter(void);

5
src/main/flight/servos.h
View file

@ -108,9 +108,8 @@ typedef struct servoMixer_s {
#define MAX_SERVO_RULES (2 * MAX_SUPPORTED_SERVOS)
#define MAX_SERVO_SPEED UINT8_MAX
#define MAX_SERVO_BOXES 3
#define SERVO_MIXER_INPUT_WIDTH 1000
#define SERVO_OUTPUT_MAX 2500
#define SERVO_OUTPUT_MIN 500
PG_DECLARE_ARRAY(servoMixer_t, MAX_SERVO_RULES, customServoMixers);

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

@ -1368,6 +1368,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_PRE_ACTION(nav
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
case NAV_WP_ACTION_WAYPOINT:
calculateAndSetActiveWaypoint(&posControl.waypointList[posControl.activeWaypointIndex]);
posControl.wpInitialDistance = calculateDistanceToDestination(&posControl.activeWaypoint.pos);
posControl.wpInitialAltitude = posControl.actualState.abs.pos.z;
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_IN_PROGRESS
case NAV_WP_ACTION_RTH:
@ -1392,7 +1394,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_REACHED
}
else {
setDesiredPosition(&posControl.activeWaypoint.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_BEARING);
fpVector3_t tmpWaypoint;
tmpWaypoint.x = posControl.activeWaypoint.pos.x;
tmpWaypoint.y = posControl.activeWaypoint.pos.y;
tmpWaypoint.z = scaleRange(constrainf(posControl.wpDistance, posControl.wpInitialDistance / 10, posControl.wpInitialDistance),
posControl.wpInitialDistance, posControl.wpInitialDistance / 10,
posControl.wpInitialAltitude, posControl.activeWaypoint.pos.z);
setDesiredPosition(&tmpWaypoint, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
return NAV_FSM_EVENT_NONE; // will re-process state in >10ms
}
break;
@ -2040,15 +2048,15 @@ bool isWaypointMissed(const navWaypointPosition_t * waypoint)
static bool isWaypointPositionReached(const fpVector3_t * pos, const bool isWaypointHome)
{
// We consider waypoint reached if within specified radius
const uint32_t wpDistance = calculateDistanceToDestination(pos);
posControl.wpDistance = calculateDistanceToDestination(pos);
if (STATE(FIXED_WING) && isWaypointHome) {
// Airplane will do a circular loiter over home and might never approach it closer than waypoint_radius - need extra check
return (wpDistance <= navConfig()->general.waypoint_radius)
|| (wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
return (posControl.wpDistance <= navConfig()->general.waypoint_radius)
|| (posControl.wpDistance <= (navConfig()->fw.loiter_radius * 1.10f)); // 10% margin of desired circular loiter radius
}
else {
return (wpDistance <= navConfig()->general.waypoint_radius);
return (posControl.wpDistance <= navConfig()->general.waypoint_radius);
}
}
@ -2391,9 +2399,6 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti
// Fixed wing climb rate controller is open-loop. We simply move the known altitude target
float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs);
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 0, desiredClimbRate);
DEBUG_SET(DEBUG_FW_CLIMB_RATE_TO_ALTITUDE, 1, timeDelta * 1000);
if (timeDelta <= HZ2S(MIN_POSITION_UPDATE_RATE_HZ)) {
posControl.desiredState.pos.z += desiredClimbRate * timeDelta;
posControl.desiredState.pos.z = constrainf(posControl.desiredState.pos.z, altitudeToUse - 500, altitudeToUse + 500); // FIXME: calculate sanity limits in a smarter way

4
src/main/navigation/navigation_multicopter.c
View file

@ -660,10 +660,6 @@ bool isMulticopterLandingDetected(void)
bool possibleLandingDetected = isAtMinimalThrust && !verticalMovement && !horizontalMovement;
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 0, isAtMinimalThrust * 100 + !verticalMovement * 10 + !horizontalMovement * 1);
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 1, (landingThrSamples == 0) ? 0 : (rcCommandAdjustedThrottle - (landingThrSum / landingThrSamples)));
DEBUG_SET(DEBUG_NAV_LANDING_DETECTOR, 2, (currentTimeUs - landingTimer) / 1000);
// If we have surface sensor (for example sonar) - use it to detect touchdown
if ((posControl.flags.estAglStatus == EST_TRUSTED) && (posControl.actualState.agl.pos.z >= 0)) {
// TODO: Come up with a clever way to let sonar increase detection performance, not just add extra safety.

3
src/main/navigation/navigation_private.h
View file

@ -354,6 +354,9 @@ typedef struct {
navWaypointPosition_t activeWaypoint; // Local position and initial bearing, filled on waypoint activation
int8_t activeWaypointIndex;
float wpInitialAltitude; // Altitude at start of WP
float wpInitialDistance; // Distance when starting flight to WP
float wpDistance; // Distance to active WP
/* Internals & statistics */
int16_t rcAdjustment[4];

6
src/main/sensors/acceleration.c
View file

@ -81,10 +81,8 @@ STATIC_FASTRAM void *accSoftLpfFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t accVibeFloorFilter[XYZ_AXIS_COUNT];
STATIC_FASTRAM pt1Filter_t accVibeFilter[XYZ_AXIS_COUNT];
#ifdef USE_ACC_NOTCH
STATIC_FASTRAM filterApplyFnPtr accNotchFilterApplyFn;
STATIC_FASTRAM void *accNotchFilter[XYZ_AXIS_COUNT];
#endif
PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 3);
@ -578,13 +576,11 @@ void accUpdate(void)
acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
}
#ifdef USE_ACC_NOTCH
if (accelerometerConfig()->acc_notch_hz) {
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
}
}
#endif
}
@ -665,7 +661,6 @@ void accInitFilters(void)
pt1FilterInit(&accVibeFilter[axis], ACC_VIBE_FILT_HZ, accDt);
}
#ifdef USE_ACC_NOTCH
STATIC_FASTRAM biquadFilter_t accFilterNotch[XYZ_AXIS_COUNT];
accNotchFilterApplyFn = nullFilterApply;
@ -676,7 +671,6 @@ void accInitFilters(void)
biquadFilterInitNotch(accNotchFilter[axis], acc.accTargetLooptime, accelerometerConfig()->acc_notch_hz, accelerometerConfig()->acc_notch_cutoff);
}
}
#endif
}

50
src/main/sensors/gyro.c
View file

@ -84,20 +84,13 @@ STATIC_FASTRAM int32_t gyroADC[XYZ_AXIS_COUNT];
STATIC_FASTRAM filterApplyFnPtr softLpfFilterApplyFn;
STATIC_FASTRAM void *softLpfFilter[XYZ_AXIS_COUNT];
#ifdef USE_GYRO_NOTCH_1
STATIC_FASTRAM filterApplyFnPtr notchFilter1ApplyFn;
STATIC_FASTRAM void *notchFilter1[XYZ_AXIS_COUNT];
#endif
#ifdef USE_GYRO_NOTCH_2
STATIC_FASTRAM filterApplyFnPtr notchFilter2ApplyFn;
STATIC_FASTRAM void *notchFilter2[XYZ_AXIS_COUNT];
#endif
#if defined(USE_GYRO_BIQUAD_RC_FIR2)
// gyro biquad RC FIR2 filter
STATIC_FASTRAM filterApplyFnPtr gyroFilterStage2ApplyFn;
STATIC_FASTRAM void *stage2Filter[XYZ_AXIS_COUNT];
#endif
#ifdef USE_DYNAMIC_FILTERS
@ -337,16 +330,13 @@ void gyroInitFilters(void)
{
STATIC_FASTRAM filter_t gyroFilterLPF[XYZ_AXIS_COUNT];
softLpfFilterApplyFn = nullFilterApply;
#ifdef USE_GYRO_NOTCH_1
STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT];
notchFilter1ApplyFn = nullFilterApply;
#endif
#ifdef USE_GYRO_NOTCH_2
STATIC_FASTRAM biquadFilter_t gyroFilterNotch_2[XYZ_AXIS_COUNT];
notchFilter2ApplyFn = nullFilterApply;
#endif
#ifdef USE_GYRO_BIQUAD_RC_FIR2
STATIC_FASTRAM biquadFilter_t gyroFilterStage2[XYZ_AXIS_COUNT];
gyroFilterStage2ApplyFn = nullFilterApply;
@ -357,7 +347,6 @@ void gyroInitFilters(void)
biquadRCFIR2FilterInit(stage2Filter[axis], gyroConfig()->gyro_stage2_lowpass_hz, getLooptime());
}
}
#endif
if (gyroConfig()->gyro_soft_lpf_hz) {
@ -380,7 +369,6 @@ void gyroInitFilters(void)
}
}
#ifdef USE_GYRO_NOTCH_1
if (gyroConfig()->gyro_soft_notch_hz_1) {
notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
@ -388,9 +376,7 @@ void gyroInitFilters(void)
biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_1, gyroConfig()->gyro_soft_notch_cutoff_1);
}
}
#endif
#ifdef USE_GYRO_NOTCH_2
if (gyroConfig()->gyro_soft_notch_hz_2) {
notchFilter2ApplyFn = (filterApplyFnPtr)biquadFilterApply;
for (int axis = 0; axis < 3; axis++) {
@ -398,7 +384,6 @@ void gyroInitFilters(void)
biquadFilterInitNotch(notchFilter2[axis], getLooptime(), gyroConfig()->gyro_soft_notch_hz_2, gyroConfig()->gyro_soft_notch_cutoff_2);
}
}
#endif
}
void gyroStartCalibration(void)
@ -479,44 +464,13 @@ void FAST_CODE NOINLINE gyroUpdate()
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
#ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) {
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FFT, 0, lrintf(gyroADCf));
DEBUG_SET(DEBUG_FFT_FREQ, 3, lrintf(gyroADCf));
}
}
#endif
if (axis < 2) {
DEBUG_SET(DEBUG_STAGE2, axis, lrintf(gyroADCf));
}
#ifdef USE_GYRO_BIQUAD_RC_FIR2
gyroADCf = gyroFilterStage2ApplyFn(stage2Filter[axis], gyroADCf);
#endif
if (axis < 2) {
DEBUG_SET(DEBUG_STAGE2, axis + 2, lrintf(gyroADCf));
}
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
DEBUG_SET(DEBUG_NOTCH, axis, lrintf(gyroADCf));
#ifdef USE_GYRO_NOTCH_1
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
#endif
#ifdef USE_GYRO_NOTCH_2
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
#endif
#ifdef USE_DYNAMIC_FILTERS
if (isDynamicFilterActive()) {
if (axis == FD_ROLL) {
DEBUG_SET(DEBUG_FFT, 1, lrintf(gyroADCf));
DEBUG_SET(DEBUG_FFT_FREQ, 2, lrintf(gyroADCf));
}
gyroDataAnalysePush(&gyroAnalyseState, axis, gyroADCf);
gyroADCf = notchFilterDynApplyFn((filter_t *)&notchFilterDyn[axis], gyroADCf);
gyroADCf = notchFilterDynApplyFn2((filter_t *)&notchFilterDyn2[axis], gyroADCf);

2
src/main/target/OMNIBUS/target.h
View file

@ -154,3 +154,5 @@
#define TARGET_IO_PORTB 0xffff
#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#undef USE_PWM_SERVO_DRIVER

5
src/main/target/common.h
View file

@ -54,7 +54,6 @@
#define USE_TELEMETRY_LTM
#define USE_TELEMETRY_FRSKY
#define USE_GYRO_BIQUAD_RC_FIR2
#define USE_MR_BRAKING_MODE
#if defined(STM_FAST_TARGET)
@ -121,10 +120,6 @@
#define USE_AUTOTUNE_FIXED_WING
#define USE_LOG
#define USE_STATS
#define USE_GYRO_NOTCH_1
#define USE_GYRO_NOTCH_2
#define USE_DTERM_NOTCH
#define USE_ACC_NOTCH
#define USE_CMS
#define CMS_MENU_OSD
#define USE_GPS_PROTO_NMEA