mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-25 01:05:21 +03:00
Merge pull request #77 from iNavFlight/biquad-filter
Biquad filter (kudos @borisbstyle)
This commit is contained in:
Konstantin Sharlaimov
commit
7711fbbec6
19 changed files with 233 additions and 367 deletions
1
Makefile
1
Makefile
|
|
@ -232,7 +232,6 @@ COMMON_SRC = build_config.c \
|
|||
flight/imu.c \
|
||||
flight/hil.c \
|
||||
flight/mixer.c \
|
||||
flight/lowpass.c \
|
||||
drivers/bus_i2c_soft.c \
|
||||
drivers/serial.c \
|
||||
drivers/sound_beeper.c \
|
||||
|
|
|
|||
|
|
@ -24,16 +24,75 @@
|
|||
#include "common/filter.h"
|
||||
#include "common/maths.h"
|
||||
|
||||
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT) {
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#define BIQUAD_BANDWIDTH 1.9f /* bandwidth in octaves */
|
||||
|
||||
/* sets up a biquad Filter */
|
||||
void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate)
|
||||
{
|
||||
float omega, sn, cs, alpha;
|
||||
float a0, a1, a2, b0, b1, b2;
|
||||
|
||||
/* If sampling rate == 0 - use main loop target rate */
|
||||
if (!samplingRate) {
|
||||
samplingRate = 1000000 / targetLooptime;
|
||||
}
|
||||
|
||||
/* setup variables */
|
||||
omega = 2 * M_PIf * (float)filterCutFreq / (float)samplingRate;
|
||||
sn = sin_approx(omega);
|
||||
cs = cos_approx(omega);
|
||||
alpha = sn * sin_approx(M_LN2f / 2 * BIQUAD_BANDWIDTH * omega / sn);
|
||||
|
||||
b0 = (1 - cs) / 2;
|
||||
b1 = 1 - cs;
|
||||
b2 = (1 - cs) / 2;
|
||||
a0 = 1 + alpha;
|
||||
a1 = -2 * cs;
|
||||
a2 = 1 - alpha;
|
||||
|
||||
/* precompute the coefficients */
|
||||
newState->a0 = b0 / a0;
|
||||
newState->a1 = b1 / a0;
|
||||
newState->a2 = b2 / a0;
|
||||
newState->a3 = a1 / a0;
|
||||
newState->a4 = a2 / a0;
|
||||
|
||||
/* zero initial samples */
|
||||
newState->x1 = newState->x2 = 0;
|
||||
newState->y1 = newState->y2 = 0;
|
||||
}
|
||||
|
||||
/* Computes a biquad_t filter on a sample */
|
||||
float filterApplyBiQuad(float sample, biquad_t *state)
|
||||
{
|
||||
float result;
|
||||
|
||||
/* compute result */
|
||||
result = state->a0 * sample + state->a1 * state->x1 + state->a2 * state->x2 -
|
||||
state->a3 * state->y1 - state->a4 * state->y2;
|
||||
|
||||
/* shift x1 to x2, sample to x1 */
|
||||
state->x2 = state->x1;
|
||||
state->x1 = sample;
|
||||
|
||||
/* shift y1 to y2, result to y1 */
|
||||
state->y2 = state->y1;
|
||||
state->y1 = result;
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
// PT1 Low Pass filter (when no dT specified it will be calculated from the cycleTime)
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dT)
|
||||
{
|
||||
// Pre calculate and store RC
|
||||
if (!filter->RC) {
|
||||
filter->RC = 1.0f / ( 2.0f * (float)M_PI * f_cut );
|
||||
}
|
||||
|
||||
filter->state = filter->state + dT / (filter->RC + dT) * (input - filter->state);
|
||||
|
||||
return filter->state;
|
||||
}
|
||||
|
||||
|
|
@ -41,59 +100,3 @@ void filterResetPt1(filterStatePt1_t *filter, float input)
|
|||
{
|
||||
filter->state = input;
|
||||
}
|
||||
|
||||
/**
|
||||
* Typical quadcopter motor noise frequency (at 50% throttle):
|
||||
* 450-sized, 920kv, 9.4x4.3 props, 3S : 4622rpm = 77Hz
|
||||
* 250-sized, 2300kv, 5x4.5 props, 4S : 14139rpm = 235Hz
|
||||
*/
|
||||
static int8_t gyroFIRCoeff_1000[3][9] = { { 0, 0, 12, 23, 40, 51, 52, 40, 38 }, // looptime=1000; group delay 2.5ms; -0.5db = 32Hz ; -1db = 45Hz; -5db = 97Hz; -10db = 132Hz
|
||||
{ 19, 31, 43, 48, 41, 35, 23, 8, 8}, // looptime=1000; group delay 3ms; -0.5db = 18Hz ; -1db = 33Hz; -5db = 81Hz; -10db = 113Hz
|
||||
{ 18, 12, 28, 40, 44, 40, 32, 22, 20} }; // looptime=1000; group delay 4ms; -0.5db = 23Hz ; -1db = 35Hz; -5db = 75Hz; -10db = 103Hz
|
||||
static int8_t gyroFIRCoeff_2000[3][9] = { { 0, 0, 0, 6, 24, 58, 83, 65, 20 }, // looptime=2000, group delay 4ms; -0.5db = 21Hz ; -1db = 31Hz; -5db = 71Hz; -10db = 99Hz
|
||||
{ 0, 0, 14, 21, 45, 59, 55, 39, 23}, // looptime=2000, group delay 5ms; -0.5db = 20Hz ; -1db = 26Hz; -5db = 52Hz; -10db = 71Hz
|
||||
{ 14, 12, 26, 38, 45, 43, 34, 24, 20} }; // looptime=2000, group delay 7ms; -0.5db = 11Hz ; -1db = 18Hz; -5db = 38Hz; -10db = 52Hz
|
||||
static int8_t gyroFIRCoeff_3000[3][9] = { { 0, 0, 0, 0, 4, 35, 87, 87, 43 }, // looptime=3000, group delay 4.5ms; -0.5db = 18Hz ; -1db = 26Hz; -5db = 57Hz; -10db = 78Hz
|
||||
{ 0, 0, 0, 14, 31, 62, 70, 52, 27}, // looptime=3000, group delay 6.5ms; -0.5db = 16Hz ; -1db = 21Hz; -5db = 42Hz; -10db = 57Hz
|
||||
{ 0, 6, 10, 28, 44, 54, 54, 38, 22} }; // looptime=3000, group delay 9ms; -0.5db = 10Hz ; -1db = 13Hz; -5db = 32Hz; -10db = 45Hz
|
||||
|
||||
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime)
|
||||
{
|
||||
if (filter_level == 0) {
|
||||
return NULL;
|
||||
}
|
||||
|
||||
int firIndex = constrain(filter_level, 1, 3) - 1;
|
||||
|
||||
// For looptimes faster than 1499 (and looptime=0) use filter for 1kHz looptime
|
||||
if (targetLooptime < 1500) {
|
||||
return gyroFIRCoeff_1000[firIndex];
|
||||
}
|
||||
// 1500 ... 2499
|
||||
else if (targetLooptime < 2500) {
|
||||
return gyroFIRCoeff_2000[firIndex];
|
||||
}
|
||||
// > 2500
|
||||
else {
|
||||
return gyroFIRCoeff_3000[firIndex];
|
||||
}
|
||||
}
|
||||
|
||||
// 9 Tap FIR filter as described here:
|
||||
// Thanks to Qcopter & BorisB & DigitalEntity
|
||||
void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9])
|
||||
{
|
||||
int32_t FIRsum;
|
||||
int axis, i;
|
||||
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
FIRsum = 0;
|
||||
for (i = 0; i <= 7; i++) {
|
||||
state[axis][i] = state[axis][i + 1];
|
||||
FIRsum += state[axis][i] * (int16_t)coeff[i];
|
||||
}
|
||||
state[axis][8] = data[axis];
|
||||
FIRsum += state[axis][8] * coeff[8];
|
||||
data[axis] = FIRsum / 256;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -23,7 +23,14 @@ typedef struct filterStatePt1_s {
|
|||
float constdT;
|
||||
} filterStatePt1_t;
|
||||
|
||||
/* this holds the data required to update samples thru a filter */
|
||||
typedef struct biquad_s {
|
||||
float a0, a1, a2, a3, a4;
|
||||
float x1, x2, y1, y2;
|
||||
} biquad_t;
|
||||
|
||||
float filterApplyPt1(float input, filterStatePt1_t *filter, uint8_t f_cut, float dt);
|
||||
void filterResetPt1(filterStatePt1_t *filter, float input);
|
||||
int8_t * filterGetFIRCoefficientsTable(uint8_t filter_level, uint16_t targetLooptime);
|
||||
void filterApply9TapFIR(int16_t data[3], int16_t state[3][9], int8_t coeff[9]);
|
||||
|
||||
void filterInitBiQuad(uint8_t filterCutFreq, biquad_t *newState, int16_t samplingRate);
|
||||
float filterApplyBiQuad(float sample, biquad_t *state);
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
|
||||
// Use floating point M_PI instead explicitly.
|
||||
#define M_PIf 3.14159265358979323846f
|
||||
#define M_LN2f 0.69314718055994530942f
|
||||
|
||||
#define RAD (M_PIf / 180.0f)
|
||||
|
||||
|
|
|
|||
|
|
@ -180,12 +180,11 @@ void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->I8[PIDVEL] = 5; // NAV_VEL_Z_I * 100
|
||||
pidProfile->D8[PIDVEL] = 100; // NAV_VEL_Z_D * 1000
|
||||
|
||||
pidProfile->acc_soft_lpf = 2; // MEDIUM filtering by default
|
||||
pidProfile->gyro_soft_lpf = 1; // LOW filtering by default
|
||||
pidProfile->acc_soft_lpf_hz = 15;
|
||||
pidProfile->gyro_soft_lpf_hz = 60;
|
||||
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->yaw_pterm_cut_hz = 0;
|
||||
pidProfile->dterm_cut_hz = 30;
|
||||
pidProfile->dterm_lpf_hz = 30;
|
||||
|
||||
pidProfile->P_f[ROLL] = 1.4f; // new PID with preliminary defaults test carefully
|
||||
pidProfile->I_f[ROLL] = 0.3f;
|
||||
|
|
@ -764,7 +763,7 @@ void activateConfig(void)
|
|||
¤tProfile->pidProfile
|
||||
);
|
||||
|
||||
useGyroConfig(&masterConfig.gyroConfig, filterGetFIRCoefficientsTable(currentProfile->pidProfile.gyro_soft_lpf, masterConfig.looptime));
|
||||
useGyroConfig(&masterConfig.gyroConfig, currentProfile->pidProfile.gyro_soft_lpf_hz);
|
||||
|
||||
#ifdef TELEMETRY
|
||||
telemetryUseConfig(&masterConfig.telemetryConfig);
|
||||
|
|
@ -777,7 +776,7 @@ void activateConfig(void)
|
|||
|
||||
setAccelerationZero(&masterConfig.accZero);
|
||||
setAccelerationGain(&masterConfig.accGain);
|
||||
setAccelerationFilter(filterGetFIRCoefficientsTable(currentProfile->pidProfile.acc_soft_lpf, masterConfig.looptime));
|
||||
setAccelerationFilter(currentProfile->pidProfile.acc_soft_lpf_hz);
|
||||
|
||||
mixerUseConfigs(
|
||||
#ifdef USE_SERVOS
|
||||
|
|
|
|||
|
|
@ -495,17 +495,14 @@ static void imuUpdateMeasuredRotationRate(void)
|
|||
}
|
||||
|
||||
/* Calculate measured acceleration in body frame cm/s/s */
|
||||
#define ACCELERATION_LPF_HZ 10
|
||||
static void imuUpdateMeasuredAcceleration(float dT)
|
||||
static void imuUpdateMeasuredAcceleration(void)
|
||||
{
|
||||
t_fp_vector measuredAccelerationBF;
|
||||
static filterStatePt1_t accFilter[XYZ_AXIS_COUNT];
|
||||
int axis;
|
||||
|
||||
/* Convert acceleration to cm/s/s */
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
measuredAccelerationBF.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
|
||||
imuMeasuredGravityBF.A[axis] = measuredAccelerationBF.A[axis];
|
||||
imuAccelInBodyFrame.A[axis] = accADC[axis] * (GRAVITY_CMSS / acc_1G);
|
||||
imuMeasuredGravityBF.A[axis] = imuAccelInBodyFrame.A[axis];
|
||||
}
|
||||
|
||||
#ifdef GPS
|
||||
|
|
@ -519,11 +516,6 @@ static void imuUpdateMeasuredAcceleration(float dT)
|
|||
imuMeasuredGravityBF.A[Z] += gpsSol.groundSpeed * imuMeasuredRotationBF.A[Y];
|
||||
}
|
||||
#endif
|
||||
|
||||
/* Apply LPF to acceleration readings and publish imuAccelInBodyFrame */
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
imuAccelInBodyFrame.A[axis] = filterApplyPt1(measuredAccelerationBF.A[axis], &accFilter[axis], ACCELERATION_LPF_HZ, dT);
|
||||
}
|
||||
}
|
||||
|
||||
#ifdef HIL
|
||||
|
|
@ -574,16 +566,16 @@ void imuUpdateGyroAndAttitude(void)
|
|||
#ifdef HIL
|
||||
if (!hilActive) {
|
||||
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
|
||||
imuUpdateMeasuredAcceleration(dT); // Calculate accel in body frame in cm/s/s
|
||||
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
|
||||
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
|
||||
}
|
||||
else {
|
||||
imuHILUpdate();
|
||||
imuUpdateMeasuredAcceleration(dT);
|
||||
imuUpdateMeasuredAcceleration();
|
||||
}
|
||||
#else
|
||||
imuUpdateMeasuredRotationRate(); // Calculate gyro rate in body frame in rad/s
|
||||
imuUpdateMeasuredAcceleration(dT); // Calculate accel in body frame in cm/s/s
|
||||
imuUpdateMeasuredAcceleration(); // Calculate accel in body frame in cm/s/s
|
||||
imuCalculateEstimatedAttitude(dT); // Update attitude estimate
|
||||
#endif
|
||||
} else {
|
||||
|
|
|
|||
|
|
@ -1,118 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <stdlib.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "flight/lowpass.h"
|
||||
|
||||
//#define DEBUG_LOWPASS
|
||||
|
||||
void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter)
|
||||
{
|
||||
float fixedScaler;
|
||||
int i;
|
||||
|
||||
// generates coefficients for a 2nd-order butterworth low-pass filter
|
||||
float freqf = (float)freq*0.001f;
|
||||
float omega = tan_approx((float)M_PI*freqf/2.0f);
|
||||
float scaling = 1.0f / (omega*omega + 1.4142136f*omega + 1.0f);
|
||||
|
||||
|
||||
#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
|
||||
printf("lowpass cutoff: %f, omega: %f\n", freqf, omega);
|
||||
#endif
|
||||
|
||||
filter->bf[0] = scaling * omega*omega;
|
||||
filter->bf[1] = 2.0f * filter->bf[0];
|
||||
filter->bf[2] = filter->bf[0];
|
||||
|
||||
filter->af[0] = 1.0f;
|
||||
filter->af[1] = scaling * (2.0f * omega*omega - 2.0f);
|
||||
filter->af[2] = scaling * (omega*omega - 1.4142136f * omega + 1.0f);
|
||||
|
||||
// Scale for fixed-point
|
||||
filter->input_bias = 1500; // Typical servo range is 1500 +/- 500
|
||||
filter->input_shift = 16;
|
||||
filter->coeff_shift = 24;
|
||||
fixedScaler = (float)(1ULL << filter->coeff_shift);
|
||||
for (i = 0; i < LOWPASS_NUM_COEF; i++) {
|
||||
filter->a[i] = LPF_ROUND(filter->af[i] * fixedScaler);
|
||||
filter->b[i] = LPF_ROUND(filter->bf[i] * fixedScaler);
|
||||
#if defined(UNIT_TEST) && defined(DEBUG_LOWPASS)
|
||||
printf("(%d) bf: %f af: %f b: %ld a: %ld\n", i,
|
||||
filter->bf[i], filter->af[i], filter->b[i], filter->a[i]);
|
||||
#endif
|
||||
}
|
||||
|
||||
filter->freq = freq;
|
||||
}
|
||||
|
||||
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq)
|
||||
{
|
||||
int16_t coefIdx;
|
||||
int64_t out;
|
||||
int32_t in_s;
|
||||
|
||||
// Check to see if cutoff frequency changed
|
||||
if (freq != filter->freq) {
|
||||
filter->init = false;
|
||||
}
|
||||
|
||||
// Initialize if needed
|
||||
if (!filter->init) {
|
||||
generateLowpassCoeffs2(freq, filter);
|
||||
for (coefIdx = 0; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
filter->x[coefIdx] = (in - filter->input_bias) << filter->input_shift;
|
||||
filter->y[coefIdx] = (in - filter->input_bias) << filter->input_shift;
|
||||
}
|
||||
filter->init = true;
|
||||
}
|
||||
|
||||
// Unbias input and scale
|
||||
in_s = (in - filter->input_bias) << filter->input_shift;
|
||||
|
||||
// Delays
|
||||
for (coefIdx = LOWPASS_NUM_COEF-1; coefIdx > 0; coefIdx--) {
|
||||
filter->x[coefIdx] = filter->x[coefIdx-1];
|
||||
filter->y[coefIdx] = filter->y[coefIdx-1];
|
||||
}
|
||||
filter->x[0] = in_s;
|
||||
|
||||
// Accumulate result
|
||||
out = filter->x[0] * filter->b[0];
|
||||
for (coefIdx = 1; coefIdx < LOWPASS_NUM_COEF; coefIdx++) {
|
||||
out -= filter->y[coefIdx] * filter->a[coefIdx];
|
||||
out += filter->x[coefIdx] * filter->b[coefIdx];
|
||||
}
|
||||
|
||||
// Scale output by coefficient shift
|
||||
out >>= filter->coeff_shift;
|
||||
filter->y[0] = (int32_t)out;
|
||||
|
||||
// Scale output by input shift and round
|
||||
out = (out + (1 << (filter->input_shift-1))) >> filter->input_shift;
|
||||
|
||||
// Reapply bias
|
||||
out += filter->input_bias;
|
||||
|
||||
return (int32_t)out;
|
||||
}
|
||||
|
||||
|
|
@ -1,41 +0,0 @@
|
|||
/*
|
||||
* This file is part of Cleanflight.
|
||||
*
|
||||
* Cleanflight is free software: you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation, either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* Cleanflight is distributed in the hope that it will be useful,
|
||||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||
* GNU General Public License for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License
|
||||
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
|
||||
#define LOWPASS_NUM_COEF 3
|
||||
#define LPF_ROUND(x) (x < 0 ? (x - 0.5f) : (x + 0.5f))
|
||||
|
||||
typedef struct lowpass_s {
|
||||
bool init;
|
||||
int16_t freq; // Normalized freq in 1/1000ths
|
||||
float bf[LOWPASS_NUM_COEF];
|
||||
float af[LOWPASS_NUM_COEF];
|
||||
int64_t b[LOWPASS_NUM_COEF];
|
||||
int64_t a[LOWPASS_NUM_COEF];
|
||||
int16_t coeff_shift;
|
||||
int16_t input_shift;
|
||||
int32_t input_bias;
|
||||
float xf[LOWPASS_NUM_COEF];
|
||||
float yf[LOWPASS_NUM_COEF];
|
||||
int32_t x[LOWPASS_NUM_COEF];
|
||||
int32_t y[LOWPASS_NUM_COEF];
|
||||
} lowpass_t;
|
||||
|
||||
void generateLowpassCoeffs2(int16_t freq, lowpass_t *filter);
|
||||
int32_t lowpassFixed(lowpass_t *filter, int32_t in, int16_t freq);
|
||||
|
|
@ -27,6 +27,7 @@
|
|||
|
||||
#include "common/axis.h"
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/system.h"
|
||||
#include "drivers/pwm_output.h"
|
||||
|
|
@ -48,7 +49,6 @@
|
|||
#include "flight/failsafe.h"
|
||||
#include "flight/pid.h"
|
||||
#include "flight/imu.h"
|
||||
#include "flight/lowpass.h"
|
||||
#include "flight/navigation_rewrite.h"
|
||||
|
||||
#include "config/runtime_config.h"
|
||||
|
|
@ -78,7 +78,8 @@ int16_t servo[MAX_SUPPORTED_SERVOS];
|
|||
static int useServo;
|
||||
STATIC_UNIT_TESTED uint8_t servoCount;
|
||||
static servoParam_t *servoConf;
|
||||
static lowpass_t lowpassFilters[MAX_SUPPORTED_SERVOS];
|
||||
static biquad_t servoFitlerState[MAX_SUPPORTED_SERVOS];
|
||||
static bool servoFilterIsSet;
|
||||
#endif
|
||||
|
||||
static const motorMixer_t mixerQuadX[] = {
|
||||
|
|
@ -890,10 +891,18 @@ void filterServos(void)
|
|||
#endif
|
||||
|
||||
if (mixerConfig->servo_lowpass_enable) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
servo[servoIdx] = (int16_t)lowpassFixed(&lowpassFilters[servoIdx], servo[servoIdx], mixerConfig->servo_lowpass_freq);
|
||||
// Initialize servo lowpass filter (servos are calculated at looptime rate)
|
||||
if (!servoFilterIsSet) {
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
filterInitBiQuad(mixerConfig->servo_lowpass_freq, &servoFitlerState[servoIdx], 0);
|
||||
}
|
||||
|
||||
// Sanity check
|
||||
servoFilterIsSet = true;
|
||||
}
|
||||
|
||||
for (servoIdx = 0; servoIdx < MAX_SUPPORTED_SERVOS; servoIdx++) {
|
||||
// Apply servo lowpass filter and do sanity cheching
|
||||
servo[servoIdx] = (int16_t) filterApplyBiQuad((float)servo[servoIdx], &servoFitlerState[servoIdx]);
|
||||
servo[servoIdx] = constrain(servo[servoIdx], servoConf[servoIdx].min, servoConf[servoIdx].max);
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@
|
|||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
|
@ -83,7 +84,8 @@ void pidResetErrorGyro(void)
|
|||
|
||||
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
|
||||
|
||||
static filterStatePt1_t yawPTermState, DTermState[3];
|
||||
static biquad_t deltaBiQuadState[3];
|
||||
static bool deltaStateIsSet = false;
|
||||
|
||||
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rxConfig_t *rxConfig)
|
||||
|
|
@ -92,11 +94,16 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
float ITerm,PTerm,DTerm;
|
||||
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
|
||||
static float lastError[3];
|
||||
static float delta1[3], delta2[3];
|
||||
float delta, deltaSum;
|
||||
float delta;
|
||||
int axis;
|
||||
float horizonLevelStrength = 1;
|
||||
|
||||
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
|
||||
deltaStateIsSet = true;
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
|
||||
// Figure out the raw stick positions
|
||||
|
|
@ -156,11 +163,6 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// -----calculate P component
|
||||
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
|
||||
|
||||
// Yaw Pterm low pass
|
||||
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
// -----calculate I component.
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
|
||||
|
||||
|
|
@ -176,23 +178,11 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
|
|||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta *= (1.0f / dT);
|
||||
|
||||
// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
|
||||
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
|
||||
deltaSum = delta;
|
||||
} else {
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
deltaSum /= 3.0f;
|
||||
if (deltaStateIsSet) {
|
||||
delta = filterApplyBiQuad(delta, &deltaBiQuadState[axis]);
|
||||
}
|
||||
|
||||
// Dterm low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = constrainf(deltaSum * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
DTerm = constrainf(delta * pidProfile->D_f[axis] * PIDweight[axis] / 100, -300.0f, 300.0f);
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
||||
|
|
@ -216,36 +206,30 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
{
|
||||
UNUSED(rxConfig);
|
||||
|
||||
int32_t errorAngle;
|
||||
int axis;
|
||||
int32_t delta, deltaSum;
|
||||
static int32_t delta1[3], delta2[3];
|
||||
int32_t delta;
|
||||
int32_t PTerm, ITerm, DTerm;
|
||||
static int32_t lastError[3] = { 0, 0, 0 };
|
||||
int32_t AngleRateTmp, RateError;
|
||||
int32_t AngleRateTmp, RateError, gyroRate;
|
||||
|
||||
int8_t horizonLevelStrength = 100;
|
||||
int32_t stickPosAil, stickPosEle, mostDeflectedPos;
|
||||
|
||||
if (!deltaStateIsSet && pidProfile->dterm_lpf_hz) {
|
||||
for (axis = 0; axis < 3; axis++)
|
||||
filterInitBiQuad(pidProfile->dterm_lpf_hz, &deltaBiQuadState[axis], 0);
|
||||
deltaStateIsSet = true;
|
||||
}
|
||||
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
|
||||
// Figure out the raw stick positions
|
||||
stickPosAil = getRcStickDeflection(FD_ROLL, rxConfig->midrc);
|
||||
stickPosEle = getRcStickDeflection(FD_PITCH, rxConfig->midrc);
|
||||
|
||||
if(ABS(stickPosAil) > ABS(stickPosEle)){
|
||||
mostDeflectedPos = ABS(stickPosAil);
|
||||
}
|
||||
else {
|
||||
mostDeflectedPos = ABS(stickPosEle);
|
||||
}
|
||||
|
||||
const int32_t stickPosAil = ABS(getRcStickDeflection(FD_ROLL, rxConfig->midrc));
|
||||
const int32_t stickPosEle = ABS(getRcStickDeflection(FD_PITCH, rxConfig->midrc));
|
||||
const int32_t mostDeflectedPos = MAX(stickPosAil, stickPosEle);
|
||||
// Progressively turn off the horizon self level strength as the stick is banged over
|
||||
horizonLevelStrength = (500 - mostDeflectedPos) / 5; // 100 at centre stick, 0 = max stick deflection
|
||||
|
||||
// Using Level D as a Sensitivity for Horizon. 0 more level to 255 more rate. Default value of 100 seems to work fine.
|
||||
// For more rate mode increase D and slower flips and rolls will be possible
|
||||
horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
|
||||
horizonLevelStrength = constrain((10 * (horizonLevelStrength - 100) * (10 * pidProfile->D8[PIDLEVEL] / 80) / 100) + 100, 0, 100);
|
||||
}
|
||||
|
||||
// ----------PID controller----------
|
||||
|
|
@ -256,18 +240,21 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
if (axis == FD_YAW) { // YAW is always gyro-controlled (MAG correction is applied to rcCommand)
|
||||
AngleRateTmp = (((int32_t)(rate + 27) * rcCommand[YAW]) >> 5);
|
||||
} else {
|
||||
// calculate error and limit the angle to max configured inclination
|
||||
errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
|
||||
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
|
||||
|
||||
if (!FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
AngleRateTmp = ((int32_t)(rate + 27) * rcCommand[axis]) >> 4;
|
||||
if (FLIGHT_MODE(HORIZON_MODE)) {
|
||||
// mix up angle error to desired AngleRateTmp to add a little auto-level feel. horizonLevelStrength is scaled to the stick input
|
||||
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
|
||||
if (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) {
|
||||
// calculate error and limit the angle to max configured inclination
|
||||
int32_t errorAngle = constrain(2 * rcCommand[axis], -((int) max_angle_inclination),
|
||||
+max_angle_inclination) - attitude.raw[axis]; // 16 bits is ok here
|
||||
|
||||
if (FLIGHT_MODE(ANGLE_MODE)) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
|
||||
// it's the ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
|
||||
} else {
|
||||
// HORIZON mode - mix up angle error to desired AngleRateTmp to add a little auto-level feel,
|
||||
// horizonLevelStrength is scaled to the stick input
|
||||
AngleRateTmp += (errorAngle * pidProfile->I8[PIDLEVEL] * horizonLevelStrength / 100) >> 4;
|
||||
}
|
||||
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
|
||||
AngleRateTmp = (errorAngle * pidProfile->P8[PIDLEVEL]) >> 4;
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -275,22 +262,18 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
// Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes
|
||||
// -----calculate scaled error.AngleRates
|
||||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
RateError = AngleRateTmp - (gyroADC[axis] / 4);
|
||||
gyroRate = gyroADC[axis] / 4;
|
||||
RateError = AngleRateTmp - gyroRate;
|
||||
|
||||
// -----calculate P component
|
||||
PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7;
|
||||
|
||||
// Yaw Pterm low pass
|
||||
if (axis == YAW && pidProfile->yaw_pterm_cut_hz) {
|
||||
PTerm = filterApplyPt1(PTerm, &yawPTermState, pidProfile->yaw_pterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
// -----calculate I component
|
||||
// there should be no division before accumulating the error to integrator, because the precision would be reduced.
|
||||
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
|
||||
// Time correction (to avoid different I scaling for different builds based on average cycle time)
|
||||
// is normalized to cycle time = 2048.
|
||||
errorGyroI[axis] = errorGyroI[axis] + ((RateError * cycleTime) >> 11) * pidProfile->I8[axis];
|
||||
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)cycleTime) >> 11) * pidProfile->I8[axis];
|
||||
|
||||
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
|
||||
// I coefficient (I8) moved before integration to make limiting independent from PID settings
|
||||
|
|
@ -303,24 +286,14 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
|
||||
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
|
||||
// would be scaled by different dt each time. Division by dT fixes that.
|
||||
delta = (delta * ((uint16_t) 0xFFFF / (cycleTime >> 4))) >> 6;
|
||||
delta = (delta * ((uint16_t) 0xFFFF / ((uint16_t)cycleTime >> 4))) >> 6;
|
||||
|
||||
// In case of soft gyro lpf combined with dterm_cut_hz we don't need the old 3 point averaging
|
||||
if (pidProfile->gyro_soft_lpf && pidProfile->dterm_cut_hz) {
|
||||
deltaSum = delta * 3; // Scaling to approximately match the old D values
|
||||
} else {
|
||||
// add moving average here to reduce noise
|
||||
deltaSum = delta1[axis] + delta2[axis] + delta;
|
||||
delta2[axis] = delta1[axis];
|
||||
delta1[axis] = delta;
|
||||
if (deltaStateIsSet) {
|
||||
// Upscale x3 before filtering to avoid rounding errors
|
||||
delta = lrintf(filterApplyBiQuad(3.0f * delta, &deltaBiQuadState[axis]));
|
||||
}
|
||||
|
||||
// Dterm delta low pass
|
||||
if (pidProfile->dterm_cut_hz) {
|
||||
deltaSum = filterApplyPt1(deltaSum, &DTermState[axis], pidProfile->dterm_cut_hz, dT);
|
||||
}
|
||||
|
||||
DTerm = (deltaSum * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||
DTerm = (delta * pidProfile->D8[axis] * PIDweight[axis] / 100) >> 8;
|
||||
|
||||
// -----calculate total PID output
|
||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||
|
|
@ -341,6 +314,9 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co
|
|||
|
||||
void pidSetController(pidControllerType_e type)
|
||||
{
|
||||
// Force filter re-init
|
||||
deltaStateIsSet = false;
|
||||
|
||||
switch (type) {
|
||||
default:
|
||||
case PID_CONTROLLER_MWREWRITE:
|
||||
|
|
|
|||
|
|
@ -58,11 +58,10 @@ typedef struct pidProfile_s {
|
|||
uint8_t H_sensitivity;
|
||||
|
||||
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
|
||||
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
|
||||
uint8_t yaw_pterm_cut_hz; // Used for filering Pterm noise on noisy frames
|
||||
uint8_t dterm_lpf_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
|
||||
|
||||
uint8_t gyro_soft_lpf; // Gyro FIR filtering
|
||||
uint8_t acc_soft_lpf; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
|
||||
uint8_t gyro_soft_lpf_hz; // Gyro FIR filtering
|
||||
uint8_t acc_soft_lpf_hz; // Set the Low Pass Filter factor for ACC. Reducing this value would reduce ACC noise (visible in GUI), but would increase ACC lag time. Zero = no filter
|
||||
|
||||
#ifdef GTUNE
|
||||
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
|
||||
|
|
|
|||
|
|
@ -374,10 +374,6 @@ static const char * const lookupTableGyroLpf[] = {
|
|||
"10HZ"
|
||||
};
|
||||
|
||||
static const char * const lookupTableSoftFilter[] = {
|
||||
"OFF", "LOW", "MEDIUM", "HIGH"
|
||||
};
|
||||
|
||||
static const char * const lookupTableFailsafeProcedure[] = {
|
||||
"SET-THR", "DROP", "RTH"
|
||||
};
|
||||
|
|
@ -413,7 +409,6 @@ typedef enum {
|
|||
TABLE_GIMBAL_MODE,
|
||||
TABLE_PID_CONTROLLER,
|
||||
TABLE_SERIAL_RX,
|
||||
TABLE_SOFT_FILTER,
|
||||
TABLE_GYRO_LPF,
|
||||
TABLE_FAILSAFE_PROCEDURE,
|
||||
#ifdef NAV
|
||||
|
|
@ -438,7 +433,6 @@ static const lookupTableEntry_t lookupTables[] = {
|
|||
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
|
||||
{ lookupTablePidController, sizeof(lookupTablePidController) / sizeof(char *) },
|
||||
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
|
||||
{ lookupTableSoftFilter, sizeof(lookupTableSoftFilter) / sizeof(char *) },
|
||||
{ lookupTableGyroLpf, sizeof(lookupTableGyroLpf) / sizeof(char *) },
|
||||
{ lookupTableFailsafeProcedure, sizeof(lookupTableFailsafeProcedure) / sizeof(char *) },
|
||||
#ifdef NAV
|
||||
|
|
@ -732,10 +726,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "d_level", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.D8[PIDLEVEL], .config.minmax = { 0, 200 }, 0 },
|
||||
|
||||
{ "yaw_p_limit", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_p_limit, .config.minmax = { YAW_P_LIMIT_MIN, YAW_P_LIMIT_MAX } },
|
||||
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_SOFT_FILTER }, 0 },
|
||||
{ "acc_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.acc_soft_lpf, .config.lookup = { TABLE_SOFT_FILTER }, 0 },
|
||||
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
|
||||
{ "yaw_pterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_pterm_cut_hz, .config.minmax = {0, 200 } },
|
||||
{ "gyro_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gyro_soft_lpf_hz, .config.minmax = {0, 200 } },
|
||||
{ "acc_soft_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.acc_soft_lpf_hz, .config.minmax = {0, 200 } },
|
||||
{ "dterm_lpf_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 200 } },
|
||||
|
||||
#ifdef GTUNE
|
||||
{ "gtune_loP_rll", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.gtune_lolimP[FD_ROLL], .config.minmax = { 10, 200 }, 0 },
|
||||
|
|
|
|||
|
|
@ -113,8 +113,6 @@ extern uint32_t currentTime;
|
|||
extern uint8_t dynP8[3], dynI8[3], dynD8[3], PIDweight[3];
|
||||
|
||||
static bool isRXDataNew;
|
||||
static filterStatePt1_t filteredCycleTimeState;
|
||||
uint16_t filteredCycleTime;
|
||||
|
||||
typedef void (*pidControllerFuncPtr)(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, rxConfig_t *rxConfig); // pid controller function prototype
|
||||
|
|
@ -538,15 +536,27 @@ void processRx(void)
|
|||
|
||||
}
|
||||
|
||||
void filterRc(bool isRXDataNew){
|
||||
void filterRc(bool isRXDataNew)
|
||||
{
|
||||
static int16_t lastCommand[4] = { 0, 0, 0, 0 };
|
||||
static int16_t deltaRC[4] = { 0, 0, 0, 0 };
|
||||
static int16_t factor, rcInterpolationFactor;
|
||||
static biquad_t filteredCycleTimeState;
|
||||
static bool filterInitialised;
|
||||
uint16_t filteredCycleTime;
|
||||
uint16_t rxRefreshRate;
|
||||
|
||||
// Set RC refresh rate for sampling and channels to filter
|
||||
initRxRefreshRate(&rxRefreshRate);
|
||||
|
||||
// Calculate average cycle time (1Hz LPF on cycle time)
|
||||
if (!filterInitialised) {
|
||||
filterInitBiQuad(1, &filteredCycleTimeState, 0);
|
||||
filterInitialised = true;
|
||||
}
|
||||
|
||||
filteredCycleTime = filterApplyBiQuad((float) cycleTime, &filteredCycleTimeState);
|
||||
|
||||
rcInterpolationFactor = rxRefreshRate / filteredCycleTime + 1;
|
||||
|
||||
if (isRXDataNew) {
|
||||
|
|
@ -575,9 +585,6 @@ void taskMainPidLoop(void)
|
|||
cycleTime = getTaskDeltaTime(TASK_SELF);
|
||||
dT = (float)cycleTime * 0.000001f;
|
||||
|
||||
// Calculate average cycle time and average jitter
|
||||
filteredCycleTime = filterApplyPt1(cycleTime, &filteredCycleTimeState, 1, dT);
|
||||
|
||||
imuUpdateAccelerometer();
|
||||
imuUpdateGyroAndAttitude();
|
||||
|
||||
|
|
@ -711,8 +718,9 @@ void taskUpdateBattery(void)
|
|||
|
||||
if (feature(FEATURE_VBAT)) {
|
||||
if (cmp32(currentTime, vbatLastServiced) >= VBATINTERVAL) {
|
||||
uint32_t vbatTimeDelta = currentTime - vbatLastServiced;
|
||||
vbatLastServiced = currentTime;
|
||||
updateBattery();
|
||||
updateBattery(vbatTimeDelta);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -27,6 +27,7 @@
|
|||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "sensors/battery.h"
|
||||
#include "sensors/sensors.h"
|
||||
|
|
@ -47,8 +48,10 @@ uint16_t calibratingA = 0; // the calibration is done is the main loop. Cal
|
|||
|
||||
static flightDynamicsTrims_t * accZero;
|
||||
static flightDynamicsTrims_t * accGain;
|
||||
static int8_t * accFIRTable = 0L;
|
||||
static int16_t accFIRState[3][9];
|
||||
|
||||
static int8_t accLpfCutHz = 0;
|
||||
static biquad_t accFilterState[XYZ_AXIS_COUNT];
|
||||
static bool accFilterInitialised = false;
|
||||
|
||||
void accSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
{
|
||||
|
|
@ -173,12 +176,28 @@ void applyAccelerationZero(flightDynamicsTrims_t * accZero, flightDynamicsTrims_
|
|||
|
||||
void updateAccelerationReadings(void)
|
||||
{
|
||||
int axis;
|
||||
|
||||
if (!acc.read(accADC)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (accFIRTable) {
|
||||
filterApply9TapFIR(accADC, accFIRState, accFIRTable);
|
||||
if (accLpfCutHz) {
|
||||
if (!accFilterInitialised) {
|
||||
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
filterInitBiQuad(accLpfCutHz, &accFilterState[axis], 0);
|
||||
}
|
||||
|
||||
accFilterInitialised = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (accFilterInitialised) {
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
accADC[axis] = lrintf(filterApplyBiQuad((float) accADC[axis], &accFilterState[axis]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (!isAccelerationCalibrationComplete()) {
|
||||
|
|
@ -200,7 +219,7 @@ void setAccelerationGain(flightDynamicsTrims_t * accGainToUse)
|
|||
accGain = accGainToUse;
|
||||
}
|
||||
|
||||
void setAccelerationFilter(int8_t * filterTableToUse)
|
||||
void setAccelerationFilter(int8_t initialAccLpfCutHz)
|
||||
{
|
||||
accFIRTable = filterTableToUse;
|
||||
accLpfCutHz = initialAccLpfCutHz;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -44,4 +44,4 @@ void accSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
|||
void updateAccelerationReadings(void);
|
||||
void setAccelerationZero(flightDynamicsTrims_t * accZeroToUse);
|
||||
void setAccelerationGain(flightDynamicsTrims_t * accGainToUse);
|
||||
void setAccelerationFilter(int8_t * filterTableToUse);
|
||||
void setAccelerationFilter(int8_t initialAccLpfCutHz);
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
#include "platform.h"
|
||||
|
||||
#include "common/maths.h"
|
||||
#include "common/filter.h"
|
||||
|
||||
#include "drivers/adc.h"
|
||||
#include "drivers/system.h"
|
||||
|
|
@ -33,11 +34,10 @@
|
|||
#include "rx/rx.h"
|
||||
|
||||
#include "io/rc_controls.h"
|
||||
#include "flight/lowpass.h"
|
||||
#include "io/beeper.h"
|
||||
|
||||
#define VBATT_PRESENT_THRESHOLD_MV 10
|
||||
#define VBATT_LPF_FREQ 10
|
||||
#define VBATT_LPF_FREQ 1
|
||||
|
||||
// Battery monitoring stuff
|
||||
uint8_t batteryCellCount = 3; // cell count
|
||||
|
|
@ -54,7 +54,6 @@ int32_t mAhDrawn = 0; // milliampere hours drawn from the battery
|
|||
batteryConfig_t *batteryConfig;
|
||||
|
||||
static batteryState_e batteryState;
|
||||
static lowpass_t lowpassFilter;
|
||||
|
||||
uint16_t batteryAdcToVoltage(uint16_t src)
|
||||
{
|
||||
|
|
@ -63,24 +62,24 @@ uint16_t batteryAdcToVoltage(uint16_t src)
|
|||
return ((((uint32_t)src * batteryConfig->vbatscale * 33 + (0xFFF * 5)) / (0xFFF * batteryConfig->vbatresdivval))/batteryConfig->vbatresdivmultiplier);
|
||||
}
|
||||
|
||||
static void updateBatteryVoltage(void)
|
||||
static void updateBatteryVoltage(uint32_t vbatTimeDelta)
|
||||
{
|
||||
uint16_t vbatSample;
|
||||
uint16_t vbatFiltered;
|
||||
static filterStatePt1_t vbatFilterState;
|
||||
|
||||
// store the battery voltage with some other recent battery voltage readings
|
||||
vbatSample = vbatLatestADC = adcGetChannel(ADC_BATTERY);
|
||||
vbatFiltered = (uint16_t)lowpassFixed(&lowpassFilter, vbatSample, VBATT_LPF_FREQ);
|
||||
vbat = batteryAdcToVoltage(vbatFiltered);
|
||||
vbatSample = filterApplyPt1(vbatSample, &vbatFilterState, VBATT_LPF_FREQ, vbatTimeDelta * 1e-6f);
|
||||
vbat = batteryAdcToVoltage(vbatSample);
|
||||
}
|
||||
|
||||
#define VBATTERY_STABLE_DELAY 40
|
||||
/* Batt Hysteresis of +/-100mV */
|
||||
#define VBATT_HYSTERESIS 1
|
||||
|
||||
void updateBattery(void)
|
||||
void updateBattery(uint32_t vbatTimeDelta)
|
||||
{
|
||||
updateBatteryVoltage();
|
||||
updateBatteryVoltage(vbatTimeDelta);
|
||||
|
||||
/* battery has just been connected*/
|
||||
if (batteryState == BATTERY_NOT_PRESENT && vbat > VBATT_PRESENT_THRESHOLD_MV)
|
||||
|
|
@ -92,7 +91,7 @@ void updateBattery(void)
|
|||
We only do this on the ground so don't care if we do block, not
|
||||
worse than original code anyway*/
|
||||
delay(VBATTERY_STABLE_DELAY);
|
||||
updateBatteryVoltage();
|
||||
updateBatteryVoltage(vbatTimeDelta);
|
||||
|
||||
unsigned cells = (batteryAdcToVoltage(vbatLatestADC) / batteryConfig->vbatmaxcellvoltage) + 1;
|
||||
if (cells > 8) {
|
||||
|
|
|
|||
|
|
@ -68,7 +68,7 @@ extern int32_t mAhDrawn;
|
|||
uint16_t batteryAdcToVoltage(uint16_t src);
|
||||
batteryState_e getBatteryState(void);
|
||||
const char * getBatteryStateString(void);
|
||||
void updateBattery(void);
|
||||
void updateBattery(uint32_t vbatTimeDelta);
|
||||
void batteryInit(batteryConfig_t *initialBatteryConfig);
|
||||
|
||||
void updateCurrentMeter(int32_t lastUpdateAt, rxConfig_t *rxConfig, uint16_t deadband3d_throttle);
|
||||
|
|
|
|||
|
|
@ -17,6 +17,7 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <math.h>
|
||||
|
||||
#include "platform.h"
|
||||
|
||||
|
|
@ -26,11 +27,13 @@
|
|||
|
||||
#include "drivers/sensor.h"
|
||||
#include "drivers/accgyro.h"
|
||||
#include "sensors/sensors.h"
|
||||
#include "drivers/gyro_sync.h"
|
||||
|
||||
#include "io/beeper.h"
|
||||
#include "io/statusindicator.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
|
||||
#include "sensors/sensors.h"
|
||||
#include "sensors/boardalignment.h"
|
||||
#include "sensors/gyro.h"
|
||||
|
||||
uint16_t calibratingG = 0;
|
||||
|
|
@ -38,16 +41,18 @@ int16_t gyroADC[XYZ_AXIS_COUNT];
|
|||
int16_t gyroZero[FLIGHT_DYNAMICS_INDEX_COUNT] = { 0, 0, 0 };
|
||||
|
||||
static gyroConfig_t *gyroConfig;
|
||||
static int8_t * gyroFIRTable = 0L;
|
||||
static int16_t gyroFIRState[3][9];
|
||||
|
||||
static int8_t gyroLpfCutHz = 0;
|
||||
static biquad_t gyroFilterState[XYZ_AXIS_COUNT];
|
||||
static bool gyroFilterInitialised = false;
|
||||
|
||||
gyro_t gyro; // gyro access functions
|
||||
sensor_align_e gyroAlign = 0;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse)
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz)
|
||||
{
|
||||
gyroConfig = gyroConfigToUse;
|
||||
gyroFIRTable = filterTableToUse;
|
||||
gyroLpfCutHz = initialGyroLpfCutHz;
|
||||
}
|
||||
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired)
|
||||
|
|
@ -120,13 +125,29 @@ static void applyGyroZero(void)
|
|||
|
||||
void gyroUpdate(void)
|
||||
{
|
||||
int8_t axis;
|
||||
|
||||
// range: +/- 8192; +/- 2000 deg/sec
|
||||
if (!gyro.read(gyroADC)) {
|
||||
return;
|
||||
}
|
||||
|
||||
if (gyroFIRTable) {
|
||||
filterApply9TapFIR(gyroADC, gyroFIRState, gyroFIRTable);
|
||||
if (gyroLpfCutHz) {
|
||||
if (!gyroFilterInitialised) {
|
||||
if (targetLooptime) { /* Initialisation needs to happen once sample rate is known */
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
filterInitBiQuad(gyroLpfCutHz, &gyroFilterState[axis], 0);
|
||||
}
|
||||
|
||||
gyroFilterInitialised = true;
|
||||
}
|
||||
}
|
||||
|
||||
if (gyroFilterInitialised) {
|
||||
for (axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
|
||||
gyroADC[axis] = lrintf(filterApplyBiQuad((float) gyroADC[axis], &gyroFilterState[axis]));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
alignSensors(gyroADC, gyroADC, gyroAlign);
|
||||
|
|
|
|||
|
|
@ -39,7 +39,7 @@ typedef struct gyroConfig_s {
|
|||
uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default.
|
||||
} gyroConfig_t;
|
||||
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t * filterTableToUse);
|
||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse, int8_t initialGyroLpfCutHz);
|
||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||
void gyroUpdate(void);
|
||||
bool isGyroCalibrationComplete(void);
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue