Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00
Code Issues Wiki Activity

address code review comments

Browse source
This commit is contained in:
Thorsten Laux 2019-01-08 09:50:26 +01:00
parent 8d4ed72e13
commit 9ef95bb84d
4 changed files with 12 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -56,9 +56,7 @@
#include "sensors/gyro.h" #include "sensors/gyro.h"
#include "sensors/acceleration.h" #include "sensors/acceleration.h"
#ifdef USE_RPM_FILTER
#include "sensors/rpm_filter.h" #include "sensors/rpm_filter.h"
#endif
const char pidNames[] = const char pidNames[] =
"ROLL;" "ROLL;"

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

@ -76,9 +76,7 @@
#ifdef USE_GYRO_DATA_ANALYSE #ifdef USE_GYRO_DATA_ANALYSE
#include "sensors/gyroanalyse.h" #include "sensors/gyroanalyse.h"
#endif #endif
#ifdef USE_RPM_FILTER
#include "sensors/rpm_filter.h" #include "sensors/rpm_filter.h"
#endif
#include "sensors/sensors.h" #include "sensors/sensors.h"
#if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500))) #if ((FLASH_SIZE > 128) && (defined(USE_GYRO_SPI_ICM20601) || defined(USE_GYRO_SPI_ICM20689) || defined(USE_GYRO_SPI_MPU6500)))

18
src/main/sensors/rpm_filter.c
View file

@ -19,8 +19,9 @@
*/ */
#include <stdint.h>
#include <math.h> #include <math.h>
#include <stdint.h>
#include "platform.h"
#include "drivers/pwm_output_counts.h" #include "drivers/pwm_output_counts.h"
#include "sensors/rpm_filter.h" #include "sensors/rpm_filter.h"
#include "common/filter.h" #include "common/filter.h"
@ -31,6 +32,9 @@
#define RPM_FILTER_MAXHARMONICS 3 #define RPM_FILTER_MAXHARMONICS 3
#define RPM_MOTOR_FILTER_CUTOFF 150 #define RPM_MOTOR_FILTER_CUTOFF 150
#define SECONDS_PER_MINUTE 60.0f
#define ERPM_PER_LSB 100.0f
#define MIN_UPDATE_T 0.001f
#if defined(USE_RPM_FILTER) #if defined(USE_RPM_FILTER)
@ -106,12 +110,9 @@ void rpmFilterInit(const rpmFilterConfig_t *config)
pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, gyro.targetLooptime)); pt1FilterInit(&rpmFilters[i], pt1FilterGain(RPM_MOTOR_FILTER_CUTOFF, gyro.targetLooptime));
} }
const float secondsPerMinute = 60.0f; erpmToHz = ERPM_PER_LSB / SECONDS_PER_MINUTE / (motorConfig()->motorPoleCount / 2.0f);
const float erpmPerLsb = 100.0f;
erpmToHz = erpmPerLsb / secondsPerMinute / (motorConfig()->motorPoleCount / 2.0f);
const float minUpdateT = 0.001f; const float loopIterationsPerUpdate = MIN_UPDATE_T / (pidLooptime * 1e-6f);
const float loopIterationsPerUpdate = minUpdateT / (pidLooptime * 1e-6f);
numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics); numberFilters = getMotorCount() * (filters[0].harmonics + filters[1].harmonics);
const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate; const float filtersPerLoopIteration = numberFilters / loopIterationsPerUpdate;
filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f); filterUpdatesPerIteration = rintf(filtersPerLoopIteration + 0.49f);
@ -150,7 +151,9 @@ void rpmFilterUpdate()
for (int motor = 0; motor < getMotorCount(); motor++) { for (int motor = 0; motor < getMotorCount(); motor++) {
filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor)); filteredMotorErpm[motor] = pt1FilterApply(&rpmFilters[motor], getDshotTelemetry(motor));
DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]); if (motor < 4) {
DEBUG_SET(DEBUG_RPM_FILTER, motor, motorFrequency[motor]);
}
} }
static uint8_t motor; static uint8_t motor;
@ -171,6 +174,7 @@ void rpmFilterUpdate()
} }
biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic]; biquadFilter_t* template = &currentFilter->notch[0][motor][harmonic];
// uncomment below to debug filter stepping. Need to also comment out motor rpm DEBUG_SET above
/* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 0, harmonic); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 1, motor); */
/* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */ /* DEBUG_SET(DEBUG_RPM_FILTER, 2, currentFilter == &gyroFilter); */

2
src/main/sensors/rpm_filter.h
View file

@ -19,8 +19,8 @@
*/ */
#pragma once #pragma once
#include <common/axis.h>
#include "common/axis.h"
#include "pg/pg.h" #include "pg/pg.h"
#include "pg/pg_ids.h" #include "pg/pg_ids.h"