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

Add WIND_ESTIMATOR debug mode

Browse source 
Uses debug[0-2] to log estimated wind in earth frame X, Y, Z.
This commit is contained in:
Alberto García Hierro 2018-05-11 14:03:37 +01:00
parent c21718bcbd
commit eb0fd005c1
3 changed files with 8 additions and 1 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -56,6 +56,7 @@ typedef enum {
DEBUG_FLOW_RAW, DEBUG_FLOW_RAW,
DEBUG_SBUS, DEBUG_SBUS,
DEBUG_FPORT, DEBUG_FPORT,
DEBUG_WIND_ESTIMATOR,
DEBUG_ALWAYS, DEBUG_ALWAYS,
DEBUG_STAGE2, DEBUG_STAGE2,
DEBUG_COUNT DEBUG_COUNT

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

@ -67,7 +67,7 @@ tables:
- name: i2c_speed - name: i2c_speed
values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"] values: ["400KHZ", "800KHZ", "100KHZ", "200KHZ"]
- name: debug_modes - name: debug_modes
values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2"] values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "WIND_ESTIMATOR"]
- name: async_mode - name: async_mode
values: ["NONE", "GYRO", "ALL"] values: ["NONE", "GYRO", "ALL"]
- name: aux_operator - name: aux_operator

6
src/main/flight/wind_estimator.c
View file

@ -18,12 +18,14 @@
#include "platform.h" #include "platform.h"
#if defined(USE_WIND_ESTIMATOR) #if defined(USE_WIND_ESTIMATOR)
#include <stdbool.h> #include <stdbool.h>
#include <stdint.h> #include <stdint.h>
#include <string.h> #include <string.h>
#include <math.h> #include <math.h>
#include "build/build_config.h" #include "build/build_config.h"
#include "build/debug.h"
#include "common/axis.h" #include "common/axis.h"
#include "common/filter.h" #include "common/filter.h"
@ -145,6 +147,10 @@ void updateWindEstimator(timeUs_t currentTimeUs)
estimatedWind[X] = estimatedWind[X] * 0.95f + wind[X] * 0.05f; estimatedWind[X] = estimatedWind[X] * 0.95f + wind[X] * 0.05f;
estimatedWind[Y] = estimatedWind[Y] * 0.95f + wind[Y] * 0.05f; estimatedWind[Y] = estimatedWind[Y] * 0.95f + wind[Y] * 0.05f;
estimatedWind[Z] = estimatedWind[Z] * 0.95f + wind[Z] * 0.05f; estimatedWind[Z] = estimatedWind[Z] * 0.95f + wind[Z] * 0.05f;
DEBUG_SET(DEBUG_WIND_ESTIMATOR, 0, estimatedWind[X] * 100);
DEBUG_SET(DEBUG_WIND_ESTIMATOR, 1, estimatedWind[Y] * 100);
DEBUG_SET(DEBUG_WIND_ESTIMATOR, 2, estimatedWind[Z] * 100);
} }
lastUpdateUs = currentTimeUs; lastUpdateUs = currentTimeUs;