Skip to content
Permalink
Browse files

rpm telemetry based notch filter

  • Loading branch information...
joelucid committed Jan 3, 2019
1 parent 00e0248 commit 8d4ed72e135689a82fb762a5c574cf5df53d78bb
@@ -113,6 +113,7 @@ COMMON_SRC = \
sensors/compass.c \
sensors/gyro.c \
sensors/gyroanalyse.c \
sensors/rpm_filter.c \
sensors/initialisation.c \
blackbox/blackbox.c \
blackbox/blackbox_encoding.c \
@@ -239,6 +240,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
sensors/boardalignment.c \
sensors/gyro.c \
sensors/gyroanalyse.c \
sensors/rpm_filter.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) \

@@ -79,4 +79,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"DYN_LPF",
"RX_SPEKTRUM_SPI",
"DSHOT_TELEMETRY",
"RPM_FILTER",
};
@@ -97,6 +97,7 @@ typedef enum {
DEBUG_DYN_LPF,
DEBUG_RX_SPEKTRUM_SPI,
DEBUG_RPM_TELEMETRY,
DEBUG_RPM_FILTER,
DEBUG_COUNT
} debugType_e;

@@ -51,7 +51,6 @@ static motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
static motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];

#ifdef USE_DSHOT_TELEMETRY
//FAST_RAM_ZERO_INIT bool dshotTelemetryEnabled;
uint32_t readDoneCount;

// TODO remove once debugging no longer needed
@@ -150,14 +149,14 @@ static uint16_t decodeProshotPacket(uint32_t buffer[])
for (int i = 1; i < PROSHOT_TELEMETRY_INPUT_LEN; i += 2) {
const int proshotModulo = MOTOR_NIBBLE_LENGTH_PROSHOT;
int diff = ((buffer[i] + proshotModulo - buffer[i-1]) % proshotModulo) - PROSHOT_BASE_SYMBOL;
int nible;
int nibble;
if (diff < 0) {
nible = 0;
nibble = 0;
} else {
nible = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH;
nibble = (diff + PROSHOT_BIT_WIDTH / 2) / PROSHOT_BIT_WIDTH;
}
value <<= 4;
value |= nible;
value |= (nibble & 0xf);
}

uint32_t csum = value;
@@ -263,10 +262,8 @@ void pwmStartDshotMotorUpdate(uint8_t motorCount)
for (int i = 0; i < motorCount; i++) {
if (dmaMotors[i].hasTelemetry) {
uint16_t value = dmaMotors[i].useProshot ?
decodeProshotPacket(
dmaMotors[i].dmaBuffer ) :
decodeDshotPacket(
dmaMotors[i].dmaBuffer );
decodeProshotPacket(dmaMotors[i].dmaBuffer) :
decodeDshotPacket(dmaMotors[i].dmaBuffer);
if (value != 0xffff) {
dmaMotors[i].dshotTelemetryValue = value;
if (i < 4) {
@@ -150,6 +150,7 @@
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/initialisation.h"
#include "sensors/rpm_filter.h"
#include "sensors/sensors.h"

#include "telemetry/telemetry.h"
@@ -56,6 +56,9 @@
#include "sensors/gyro.h"
#include "sensors/acceleration.h"

#ifdef USE_RPM_FILTER
#include "sensors/rpm_filter.h"
#endif

const char pidNames[] =
"ROLL;"
@@ -604,6 +607,9 @@ void pidInit(const pidProfile_t *pidProfile)
pidSetTargetLooptime(gyro.targetLooptime * pidConfig()->pid_process_denom); // Initialize pid looptime
pidInitFilters(pidProfile);
pidInitConfig(pidProfile);
#ifdef USE_RPM_FILTER
rpmFilterInit(rpmFilterConfig());
#endif
}

#ifdef USE_ACRO_TRAINER
@@ -1103,12 +1109,20 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
// Precalculate gyro deta for D-term here, this allows loop unrolling
float gyroRateDterm[XYZ_AXIS_COUNT];
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyro.gyroADCf[axis]);
gyroRateDterm[axis] = gyro.gyroADCf[axis];
#ifdef USE_RPM_FILTER
gyroRateDterm[axis] = rpmFilterDterm(axis,gyroRateDterm[axis]);
#endif
gyroRateDterm[axis] = dtermNotchApplyFn((filter_t *) &dtermNotch[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateDterm[axis]);
gyroRateDterm[axis] = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateDterm[axis]);
}

rotateItermAndAxisError();
#ifdef USE_RPM_FILTER
rpmFilterUpdate();
#endif


// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@@ -102,6 +102,7 @@
#include "sensors/esc_sensor.h"
#include "sensors/gyro.h"
#include "sensors/rangefinder.h"
#include "sensors/rpm_filter.h"

#include "telemetry/frsky_hub.h"
#include "telemetry/ibus_shared.h"
@@ -1299,6 +1300,15 @@ const clivalue_t valueTable[] = {
#ifdef USE_RTC_TIME
{ "timezone_offset_minutes", VAR_INT16 | MASTER_VALUE, .config.minmax = { TIMEZONE_OFFSET_MINUTES_MIN, TIMEZONE_OFFSET_MINUTES_MAX }, PG_TIME_CONFIG, offsetof(timeConfig_t, tz_offsetMinutes) },
#endif

#ifdef USE_RPM_FILTER
{ "gyro_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_harmonics) },
{ "gyro_rpm_notch_q", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 100 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_q) },
{ "gyro_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, gyro_rpm_notch_min) },
{ "dterm_rpm_notch_harmonics", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 0, 3 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_harmonics) },
{ "dterm_rpm_notch_q", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 1, 100 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_q) },
{ "dterm_rpm_notch_min", VAR_UINT8 | MASTER_VALUE, .config.minmax = { 50, 200 }, PG_RPM_FILTER_CONFIG, offsetof(rpmFilterConfig_t, dterm_rpm_notch_min) },
#endif
};

const uint16_t valueTableEntryCount = ARRAYLEN(valueTable);
@@ -138,7 +138,8 @@
#define PG_MCO_CONFIG 541
#define PG_RX_SPEKTRUM_SPI_CONFIG 542
#define PG_SERIAL_UART_CONFIG 543
#define PG_BETAFLIGHT_END 543
#define PG_RPM_FILTER_CONFIG 544
#define PG_BETAFLIGHT_END 544


// OSD configuration (subject to change)
@@ -76,6 +76,9 @@
#ifdef USE_GYRO_DATA_ANALYSE
#include "sensors/gyroanalyse.h"
#endif
#ifdef USE_RPM_FILTER
#include "sensors/rpm_filter.h"
#endif
#include "sensors/sensors.h"

#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))
@@ -17,6 +17,11 @@ static FAST_CODE void GYRO_FILTER_FUNCTION_NAME(gyroSensor_t *gyroSensor)
}
#endif

#ifdef USE_RPM_FILTER
gyroADCf = rpmFilterGyro(axis, gyroADCf);
#endif


// apply static notch filters and software lowpass filters
gyroADCf = gyroSensor->notchFilter1ApplyFn((filter_t *)&gyroSensor->notchFilter1[axis], gyroADCf);
gyroADCf = gyroSensor->notchFilter2ApplyFn((filter_t *)&gyroSensor->notchFilter2[axis], gyroADCf);
@@ -0,0 +1,204 @@
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software 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 and Betaflight are distributed in the hope that they
* 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 this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/


#include <stdint.h>
#include <math.h>
#include "drivers/pwm_output_counts.h"
#include "sensors/rpm_filter.h"
#include "common/filter.h"
#include "sensors/gyro.h"
#include "build/debug.h"
#include "flight/mixer.h"
#include "flight/pid.h"

#define RPM_FILTER_MAXHARMONICS 3
#define RPM_MOTOR_FILTER_CUTOFF 150

#if defined(USE_RPM_FILTER)

static pt1Filter_t rpmFilters[MAX_SUPPORTED_MOTORS];

typedef struct rpmNotchFilter_s
{
uint8_t harmonics;
uint8_t minHz;
float q;
float loopTime;

biquadFilter_t notch[XYZ_AXIS_COUNT][MAX_SUPPORTED_MOTORS][RPM_FILTER_MAXHARMONICS];
} rpmNotchFilter_t;

static float erpmToHz;
static float filteredMotorErpm[MAX_SUPPORTED_MOTORS];
static uint8_t numberFilters;
static uint8_t numberRpmNotchFilters;
static uint8_t filterUpdatesPerIteration;
static float pidLooptime;
static rpmNotchFilter_t filters[2];
static rpmNotchFilter_t* gyroFilter;
static rpmNotchFilter_t* dtermFilter;


PG_REGISTER_WITH_RESET_FN(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1);

void pgResetFn_rpmFilterConfig(rpmFilterConfig_t *config)
{
config->gyro_rpm_notch_harmonics = 3;
config->gyro_rpm_notch_min = 100;
config->gyro_rpm_notch_q = 50;

config->dterm_rpm_notch_harmonics = 1;
config->dterm_rpm_notch_min = 100;
config->dterm_rpm_notch_q = 50;
}

static void rpmNotchFilterInit(rpmNotchFilter_t* filter, int harmonics, int minHz, int q, float looptime)
{
filter->harmonics = harmonics;
filter->minHz = minHz;
filter->q = q / 10.0f;
filter->loopTime = looptime;

for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < harmonics; i++) {
biquadFilterInit(
&filter->notch[axis][motor][i], minHz * i, looptime, filter->q, FILTER_NOTCH);
}
}
}
}

void rpmFilterInit(const rpmFilterConfig_t *config)
{
pidLooptime = gyro.targetLooptime * pidConfig()->pid_process_denom;
numberRpmNotchFilters = 0;
if (config->gyro_rpm_notch_harmonics) {
gyroFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(gyroFilter, config->gyro_rpm_notch_harmonics,
config->gyro_rpm_notch_min, config->gyro_rpm_notch_q, gyro.targetLooptime);
}
if (config->dterm_rpm_notch_harmonics) {
dtermFilter = &filters[numberRpmNotchFilters++];
rpmNotchFilterInit(dtermFilter, config->dterm_rpm_notch_harmonics,
config->dterm_rpm_notch_min, config->dterm_rpm_notch_q, pidLooptime);
}

for (int i = 0; i < getMotorCount(); i++) {
pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, gyro.targetLooptime));
}

const float secondsPerMinute = 60.0f;
const float erpmPerLsb = 100.0f;
erpmToHz = erpmPerLsb / secondsPerMinute / (motorConfig()->motorPoleCount / 2.0f);

const float minUpdateT = 0.001f;
const float loopIterationsPerUpdate = minUpdateT / (pidLooptime * 1e-6f);
numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
}

static float applyFilter(rpmNotchFilter_t* filter, int axis, float value)
{
if (filter == NULL) {
return value;
}
for (int motor = 0; motor < getMotorCount(); motor++) {
for (int i = 0; i < filter->harmonics; i++) {
value = biquadFilterApplyDF1(&filter->notch[axis][motor][i], value);
}
}
return value;
}

float rpmFilterGyro(int axis, float value)
{
return applyFilter(gyroFilter, axis, value);
}

float rpmFilterDterm(int axis, float value)
{
return applyFilter(dtermFilter, axis, value);
}

static float motorFrequency[MAX_SUPPORTED_MOTORS];

void rpmFilterUpdate()
{
if (gyroFilter == NULL && dtermFilter == NULL) {
return;
}

for (int motor = 0; motor < getMotorCount(); motor++) {
filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]);
}

static uint8_t motor;
static uint8_t harmonic;
static uint8_t filter;
static rpmNotchFilter_t* currentFilter = &filters[0];

for (int i = 0; i < filterUpdatesPerIteration; i++) {

float frequency = (harmonic + 1) * motorFrequency[motor];
const float deactivateFreq = 1000.0f;
if (frequency < currentFilter->minHz) {
if (frequency < 0.5f * currentFilter->minHz) {
frequency = deactivateFreq;
} else {
frequency = currentFilter->minHz;
}
}

biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
/* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 3, frequency) */
biquadFilterUpdate(
template, frequency, currentFilter->loopTime, currentFilter->q, FILTER_NOTCH);
for (int axis = 1; axis < XYZ_AXIS_COUNT; axis++) {
biquadFilter_t* clone = &currentFilter->notch[axis][motor][harmonic];
clone->b0 = template->b0;
clone->b1 = template->b1;
clone->b2 = template->b2;
clone->a1 = template->a1;
clone->a2 = template->a2;
}

if (++harmonic == currentFilter->harmonics) {
harmonic = 0;
if (++filter == numberRpmNotchFilters) {
filter = 0;
if (++motor == getMotorCount()) {
motor = 0;
}
motorFrequency[motor] = erpmToHz * filteredMotorErpm[motor];
}
currentFilter = &filters[filter];
}

}
}

#endif
Oops, something went wrong.

0 comments on commit 8d4ed72

Please sign in to comment.
You can’t perform that action at this time.