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

Merge pull request #2474 from martinbudden/bf_gyro_analysis_hooks 

Added hooks for gyro data analysis
This commit is contained in:
Martin Budden 2017-02-23 19:39:28 +00:00 committed by GitHub
parent 2d5d608095 ea2ec8b069
commit 60f6dca7e0
11 changed files with 143 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
Makefile
View File

@ -643,6 +643,7 @@ COMMON_SRC = \
sensors/boardalignment.c \
sensors/compass.c \
sensors/gyro.c \
sensors/gyroanalyse.c \
sensors/initialisation.c \
blackbox/blackbox.c \
blackbox/blackbox_io.c \
@ -748,6 +749,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \
sensors/acceleration.c \
sensors/boardalignment.c \
sensors/gyro.c \
sensors/gyroanalyse.c \
$(CMSIS_SRC) \
$(DEVICE_STDPERIPH_SRC) \
drivers/display_ug2864hsweg01.c \
@ -873,6 +875,25 @@ else ifeq ($(TARGET),$(filter $(TARGET),$(F1_TARGETS)))
TARGET_SRC := $(STARTUP_SRC) $(STM32F10x_COMMON_SRC) $(TARGET_SRC)
endif
ifneq ($(filter GYROFFT,$(FEATURES)),)
DSPLIB := $(ROOT)/lib/main/DSP_Lib
DEVICE_FLAGS += -DARM_MATH_CM4 -DARM_MATH_MATRIX_CHECK -DARM_MATH_ROUNDING -D__FPU_PRESENT=1 -DUNALIGNED_SUPPORT_DISABLE
INCLUDE_DIRS += $(DSPLIB)/Include
TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_f32.c
TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_f32.c
TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c
TARGET_SRC += $(DSPLIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c
TARGET_SRC += $(DSPLIB)/Source/CommonTables/arm_common_tables.c
TARGET_SRC += $(DSPLIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c
TARGET_SRC += $(DSPLIB)/Source/StatisticsFunctions/arm_max_f32.c
TARGET_SRC += $(wildcard $(DSPLIB)/Source/*/*.S)
endif
ifneq ($(filter ONBOARDFLASH,$(FEATURES)),)
TARGET_SRC += \
drivers/flash_m25p16.c \

3
lib/main/CMSIS/CM4/CoreSupport/arm_math.h
View File

@ -5211,8 +5211,11 @@ void arm_rfft_fast_f32(
/* acc += y[n-1] */
acc += (q31_t) S->state[2] << 15;
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wpedantic"
/* saturate the output */
out = (q15_t) (__SSAT((acc >> 15), 16));
#pragma GCC diagnostic push
/* Update state */
S->state[1] = S->state[0];

1
lib/main/DSP_Lib/Source/CommonTables/arm_common_tables.c
View File

@ -41,6 +41,7 @@
#include "arm_math.h"
#include "arm_common_tables.h"
#pragma GCC diagnostic ignored "-Woverflow"
/**
* @ingroup groupTransforms

4
src/main/drivers/accgyro.h
View File

@ -50,7 +50,9 @@ typedef struct gyroDev_s {
sensorGyroUpdateFuncPtr update;
extiCallbackRec_t exti;
float scale; // scalefactor
volatile int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int16_t gyroADCRaw[XYZ_AXIS_COUNT];
int32_t gyroZero[XYZ_AXIS_COUNT];
int32_t gyroADC[XYZ_AXIS_COUNT]; // gyro data after calibration and alignment
uint8_t lpf;
gyroRateKHz_e gyroRateKHz;
uint8_t mpuDividerDrops;

2
src/main/drivers/exti.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <stdbool.h>
#include "io_types.h"
// old EXTI interface, to be replaced

3
src/main/drivers/sensor.h
View File

@ -17,6 +17,9 @@
#pragma once
#include <stdbool.h>
#include <stdint.h>
typedef enum {
ALIGN_DEFAULT = 0, // driver-provided alignment
CW0_DEG = 1,

19
src/main/fc/fc_tasks.c
View File

@ -72,6 +72,7 @@
#include "sensors/battery.h"
#include "sensors/compass.h"
#include "sensors/gyro.h"
#include "sensors/gyroanalyse.h"
#include "sensors/sonar.h"
#include "sensors/esc_sensor.h"
@ -308,6 +309,9 @@ void fcTasksInit(void)
setTaskEnabled(TASK_VTXCTRL, true);
#endif
#endif
#ifdef USE_GYRO_DATA_ANALYSE
setTaskEnabled(TASK_GYRO_DATA_ANALYSE, true);
#endif
}
cfTask_t cfTasks[TASK_COUNT] = {
@ -409,7 +413,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_SONAR] = {
.taskName = "SONAR",
.taskFunc = sonarUpdate,
.desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfer with each other
.desiredPeriod = TASK_PERIOD_MS(70), // 70ms required so that SONAR pulses do not interfere with each other
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
@ -479,7 +483,7 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_ESC_SENSOR] = {
.taskName = "ESC_SENSOR",
.taskFunc = escSensorProcess,
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz every 10ms
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms
.staticPriority = TASK_PRIORITY_LOW,
},
#endif
@ -506,8 +510,17 @@ cfTask_t cfTasks[TASK_COUNT] = {
[TASK_VTXCTRL] = {
.taskName = "VTXCTRL",
.taskFunc = taskVtxControl,
.desiredPeriod = TASK_PERIOD_HZ(5), // 5Hz @200msec
.desiredPeriod = TASK_PERIOD_HZ(5), // 5 Hz, 200ms
.staticPriority = TASK_PRIORITY_IDLE,
},
#endif
#ifdef USE_GYRO_DATA_ANALYSE
[TASK_GYRO_DATA_ANALYSE] = {
.taskName = "GYROFFT",
.taskFunc = gyroDataAnalyseUpdate,
.desiredPeriod = TASK_PERIOD_HZ(100), // 100 Hz, 10ms
.staticPriority = TASK_PRIORITY_MEDIUM,
},
#endif
};

3
src/main/scheduler/scheduler.h
View File

@ -105,6 +105,9 @@ typedef enum {
#ifdef VTX_CONTROL
TASK_VTXCTRL,
#endif
#ifdef USE_GYRO_DATA_ANALYSE
TASK_GYRO_DATA_ANALYSE,
#endif
/* Count of real tasks */
TASK_COUNT,

56
src/main/sensors/gyro.c
View File

@ -62,6 +62,7 @@
#include "sensors/boardalignment.h"
#include "sensors/gyro.h"
#include "sensors/gyroanalyse.h"
#include "sensors/sensors.h"
#ifdef USE_HARDWARE_REVISION_DETECTION
@ -73,9 +74,6 @@ gyro_t gyro;
STATIC_UNIT_TESTED gyroDev_t gyroDev0;
static int16_t gyroTemperature0;
static int32_t gyroADC[XYZ_AXIS_COUNT];
STATIC_UNIT_TESTED int32_t gyroZero[XYZ_AXIS_COUNT] = { 0, 0, 0 };
static uint16_t calibratingG = 0;
static filterApplyFnPtr softLpfFilterApplyFn;
@ -371,6 +369,9 @@ void gyroInitFilters(void)
biquadFilterInit(notchFilter2[axis], gyroConfig()->gyro_soft_notch_hz_2, gyro.targetLooptime, gyroSoftNotchQ2, FILTER_NOTCH);
}
}
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyseInit();
#endif
}
bool isGyroCalibrationComplete(void)
@ -398,7 +399,7 @@ void gyroSetCalibrationCycles(void)
calibratingG = gyroCalculateCalibratingCycles();
}
STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationThreshold)
STATIC_UNIT_TESTED void performGyroCalibration(gyroDev_t *gyroDev, uint8_t gyroMovementCalibrationThreshold)
{
static int32_t g[3];
static stdev_t var[3];
@ -412,15 +413,15 @@ STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationTh
}
// Sum up CALIBRATING_GYRO_CYCLES readings
g[axis] += gyroADC[axis];
devPush(&var[axis], gyroADC[axis]);
g[axis] += gyroDev->gyroADC[axis];
devPush(&var[axis], gyroDev->gyroADC[axis]);
// Reset global variables to prevent other code from using un-calibrated data
gyroADC[axis] = 0;
gyroZero[axis] = 0;
gyroDev->gyroADC[axis] = 0;
gyroDev->gyroZero[axis] = 0;
if (isOnFinalGyroCalibrationCycle()) {
float dev = devStandardDeviation(&var[axis]);
const float dev = devStandardDeviation(&var[axis]);
DEBUG_SET(DEBUG_GYRO, DEBUG_GYRO_CALIBRATION, lrintf(dev));
@ -429,7 +430,7 @@ STATIC_UNIT_TESTED void performGyroCalibration(uint8_t gyroMovementCalibrationTh
gyroSetCalibrationCycles();
return;
}
gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
gyroDev->gyroZero[axis] = (g[axis] + (gyroCalculateCalibratingCycles() / 2)) / gyroCalculateCalibratingCycles();
}
}
@ -452,16 +453,16 @@ static bool gyroUpdateISR(gyroDev_t* gyroDev)
#endif
gyroDev->dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyroDev->gyroADCRaw[X];
gyroADC[Y] = gyroDev->gyroADCRaw[Y];
gyroADC[Z] = gyroDev->gyroADCRaw[Z];
gyroDev->gyroADC[X] = gyroDev->gyroADCRaw[X];
gyroDev->gyroADC[Y] = gyroDev->gyroADCRaw[Y];
gyroDev->gyroADC[Z] = gyroDev->gyroADCRaw[Z];
alignSensors(gyroADC, gyroDev->gyroAlign);
alignSensors(gyroDev->gyroADC, gyroDev->gyroAlign);
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
gyroDev->gyroADC[axis] -= gyroDev->gyroZero[axis];
// scale gyro output to degrees per second
float gyroADCf = (float)gyroADC[axis] * gyroDev->scale;
float gyroADCf = (float)gyroDev->gyroADC[axis] * gyroDev->scale;
gyroADCf = softLpfFilterApplyFn(softLpfFilter[axis], gyroADCf);
gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf);
gyroADCf = notchFilter2ApplyFn(notchFilter2[axis], gyroADCf);
@ -483,11 +484,11 @@ void gyroUpdate(void)
}
gyroDev0.dataReady = false;
// move gyro data into 32-bit variables to avoid overflows in calculations
gyroADC[X] = gyroDev0.gyroADCRaw[X];
gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
gyroDev0.gyroADC[X] = gyroDev0.gyroADCRaw[X];
gyroDev0.gyroADC[Y] = gyroDev0.gyroADCRaw[Y];
gyroDev0.gyroADC[Z] = gyroDev0.gyroADCRaw[Z];
alignSensors(gyroADC, gyroDev0.gyroAlign);
alignSensors(gyroDev0.gyroADC, gyroDev0.gyroAlign);
const bool calibrationComplete = isGyroCalibrationComplete();
if (calibrationComplete) {
@ -502,13 +503,13 @@ void gyroUpdate(void)
debug[3] = (uint16_t)(micros() & 0xffff);
#endif
} else {
performGyroCalibration(gyroConfig()->gyroMovementCalibrationThreshold);
performGyroCalibration(&gyroDev0, gyroConfig()->gyroMovementCalibrationThreshold);
}
for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
gyroADC[axis] -= gyroZero[axis];
gyroDev0.gyroADC[axis] -= gyroDev0.gyroZero[axis];
// scale gyro output to degrees per second
float gyroADCf = (float)gyroADC[axis] * gyroDev0.scale;
float gyroADCf = (float)gyroDev0.gyroADC[axis] * gyroDev0.scale;
// Apply LPF
DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf));
@ -522,10 +523,13 @@ void gyroUpdate(void)
}
if (!calibrationComplete) {
gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
gyroDev0.gyroADC[X] = lrintf(gyro.gyroADCf[X] / gyroDev0.scale);
gyroDev0.gyroADC[Y] = lrintf(gyro.gyroADCf[Y] / gyroDev0.scale);
gyroDev0.gyroADC[Z] = lrintf(gyro.gyroADCf[Z] / gyroDev0.scale);
}
#ifdef USE_GYRO_DATA_ANALYSE
gyroDataAnalyse(&gyroDev0, &gyro);
#endif
}
void gyroReadTemperature(void)

35
src/main/sensors/gyroanalyse.c Normal file
View File

@ -0,0 +1,35 @@
#include <stdint.h>
#include "platform.h"
#ifdef USE_GYRO_DATA_ANALYSE
#include "arm_math.h"
#include "build/debug.h"
#include "common/maths.h"
#include "common/time.h"
#include "common/utils.h"
#include "drivers/accgyro.h"
#include "sensors/gyro.h"
#include "sensors/gyroanalyse.h"
void gyroDataAnalyseInit(void)
{
}
void gyroDataAnalyse(const gyroDev_t *gyroDev, const gyro_t *gyro)
{
UNUSED(gyroDev);
UNUSED(gyro);
}
void gyroDataAnalyseUpdate(timeUs_t currentTimeUs)
{
UNUSED(currentTimeUs);
}
#endif // USE_GYRO_DATA_ANALYSE

26
src/main/sensors/gyroanalyse.h Normal file
View File

@ -0,0 +1,26 @@
/*
* 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
#include "common/time.h"
void gyroDataAnalyseInit(void);
struct gyroDev_s;
struct gyro_s;
void gyroDataAnalyse(const struct gyroDev_s *gyroDev, const struct gyro_s *gyro);
void gyroDataAnalyseUpdate(timeUs_t currentTimeUs);