Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-20 06:45:14 +03:00
Code Issues Wiki Activity

Comment updates; Add channel filtering explanation

Browse source
This commit is contained in:
Konstantin Sharlaimov (DigitalEntity) 2017-09-13 21:57:50 +10:00
parent 9a5fcb1719
commit a115a38892
6 changed files with 69 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

66
src/main/common/maths.c
View file

@ -16,6 +16,7 @@
*/
#include <stdint.h>
#include <string.h>
#include <math.h>
#include "axis.h"
@ -260,55 +261,74 @@ void rotateV(struct fp_vector *v, fp_angles_t *delta)
// Quick median filter implementation
// (c) N. Devillard - 1998
// http://ndevilla.free.fr/median/median.pdf
#define QMF_SORT(a,b) { if ((a)>(b)) QMF_SWAP((a),(b)); }
#define QMF_SWAP(a,b) { int32_t temp=(a);(a)=(b);(b)=temp; }
#define QMF_COPY(p,v,n) { int32_t i; for (i=0; i<n; i++) p[i]=v[i]; }
#define QMF_SORT(type,a,b) { if ((a)>(b)) QMF_SWAP(type, (a),(b)); }
#define QMF_SWAP(type,a,b) { type temp=(a);(a)=(b);(b)=temp; }
int32_t quickMedianFilter3(int32_t * v)
{
int32_t p[3];
QMF_COPY(p, v, 3);
memcpy(p, v, sizeof(p));
QMF_SORT(p[0], p[1]); QMF_SORT(p[1], p[2]); QMF_SORT(p[0], p[1]) ;
QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[0], p[1]) ;
return p[1];
}
int16_t quickMedianFilter3_16(int16_t * v)
{
int16_t p[3];
memcpy(p, v, sizeof(p));
QMF_SORT(int16_t, p[0], p[1]); QMF_SORT(int16_t, p[1], p[2]); QMF_SORT(int16_t, p[0], p[1]) ;
return p[1];
}
int32_t quickMedianFilter5(int32_t * v)
{
int32_t p[5];
QMF_COPY(p, v, 5);
memcpy(p, v, sizeof(p));
QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[0], p[3]);
QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[2]); QMF_SORT(p[2], p[3]);
QMF_SORT(p[1], p[2]);
QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[4]); QMF_SORT(int32_t, p[0], p[3]);
QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[2], p[3]);
QMF_SORT(int32_t, p[1], p[2]);
return p[2];
}
int16_t quickMedianFilter5_16(int16_t * v)
{
int16_t p[5];
memcpy(p, v, sizeof(p));
QMF_SORT(int16_t, p[0], p[1]); QMF_SORT(int16_t, p[3], p[4]); QMF_SORT(int16_t, p[0], p[3]);
QMF_SORT(int16_t, p[1], p[4]); QMF_SORT(int16_t, p[1], p[2]); QMF_SORT(int16_t, p[2], p[3]);
QMF_SORT(int16_t, p[1], p[2]);
return p[2];
}
int32_t quickMedianFilter7(int32_t * v)
{
int32_t p[7];
QMF_COPY(p, v, 7);
memcpy(p, v, sizeof(p));
QMF_SORT(p[0], p[5]); QMF_SORT(p[0], p[3]); QMF_SORT(p[1], p[6]);
QMF_SORT(p[2], p[4]); QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[5]);
QMF_SORT(p[2], p[6]); QMF_SORT(p[2], p[3]); QMF_SORT(p[3], p[6]);
QMF_SORT(p[4], p[5]); QMF_SORT(p[1], p[4]); QMF_SORT(p[1], p[3]);
QMF_SORT(p[3], p[4]);
QMF_SORT(int32_t, p[0], p[5]); QMF_SORT(int32_t, p[0], p[3]); QMF_SORT(int32_t, p[1], p[6]);
QMF_SORT(int32_t, p[2], p[4]); QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[5]);
QMF_SORT(int32_t, p[2], p[6]); QMF_SORT(int32_t, p[2], p[3]); QMF_SORT(int32_t, p[3], p[6]);
QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[1], p[3]);
QMF_SORT(int32_t, p[3], p[4]);
return p[3];
}
int32_t quickMedianFilter9(int32_t * v)
{
int32_t p[9];
QMF_COPY(p, v, 9);
memcpy(p, v, sizeof(p));
QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
QMF_SORT(p[0], p[1]); QMF_SORT(p[3], p[4]); QMF_SORT(p[6], p[7]);
QMF_SORT(p[1], p[2]); QMF_SORT(p[4], p[5]); QMF_SORT(p[7], p[8]);
QMF_SORT(p[0], p[3]); QMF_SORT(p[5], p[8]); QMF_SORT(p[4], p[7]);
QMF_SORT(p[3], p[6]); QMF_SORT(p[1], p[4]); QMF_SORT(p[2], p[5]);
QMF_SORT(p[4], p[7]); QMF_SORT(p[4], p[2]); QMF_SORT(p[6], p[4]);
QMF_SORT(p[4], p[2]);
QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[7], p[8]);
QMF_SORT(int32_t, p[0], p[1]); QMF_SORT(int32_t, p[3], p[4]); QMF_SORT(int32_t, p[6], p[7]);
QMF_SORT(int32_t, p[1], p[2]); QMF_SORT(int32_t, p[4], p[5]); QMF_SORT(int32_t, p[7], p[8]);
QMF_SORT(int32_t, p[0], p[3]); QMF_SORT(int32_t, p[5], p[8]); QMF_SORT(int32_t, p[4], p[7]);
QMF_SORT(int32_t, p[3], p[6]); QMF_SORT(int32_t, p[1], p[4]); QMF_SORT(int32_t, p[2], p[5]);
QMF_SORT(int32_t, p[4], p[7]); QMF_SORT(int32_t, p[4], p[2]); QMF_SORT(int32_t, p[6], p[4]);
QMF_SORT(int32_t, p[4], p[2]);
return p[4];
}

3
src/main/common/maths.h
View file

@ -162,6 +162,9 @@ int32_t quickMedianFilter5(int32_t * v);
int32_t quickMedianFilter7(int32_t * v);
int32_t quickMedianFilter9(int32_t * v);
int16_t quickMedianFilter3_16(int16_t * v);
int16_t quickMedianFilter5_16(int16_t * v);
#if defined(FAST_MATH) || defined(VERY_FAST_MATH)
float sin_approx(float x);
float cos_approx(float x);

2
src/main/flight/failsafe.h
View file

@ -130,7 +130,7 @@ typedef enum {
typedef struct failsafeState_s {
int16_t events;
bool monitoring; // Flag that failsafe is monitoring RC link
bool suspended; // Failsafe is temporary suspended
bool suspended; // Failsafe is temporary suspended. This happens when we temporary suspend RX system due to EEPROM write/read
bool active; // Failsafe is active (on RC link loss)
bool controlling; // Failsafe is driving the sticks instead of pilot
timeMs_t rxDataFailurePeriod;

20
src/main/rx/rx.c
View file

@ -80,7 +80,6 @@ int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; // interval [1000;2000]
uint32_t rcInvalidPulsPeriod[MAX_SUPPORTED_RC_CHANNEL_COUNT];
#define MAX_INVALID_PULS_TIME 300
#define FILTERING_SAMPLE_COUNT 3
#define SKIP_RC_ON_SUSPEND_PERIOD 1500000 // 1.5 second period in usec (call frequency independent)
#define SKIP_RC_SAMPLES_ON_RESUME 2 // flush 2 samples to drop wrong measurements (timing independent)
@ -335,9 +334,10 @@ bool rxUpdateCheck(timeUs_t currentTimeUs, timeDelta_t currentDeltaTime)
return rxDataReceived || ((int32_t)(currentTimeUs - rxLastUpdateTimeUs) >= 0); // data driven or 50Hz
}
#define FILTERING_SAMPLE_COUNT 5
static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample)
{
static int32_t rcSamples[MAX_SUPPORTED_RC_CHANNEL_COUNT][FILTERING_SAMPLE_COUNT];
static int16_t rcSamples[MAX_SUPPORTED_RC_CHANNEL_COUNT][FILTERING_SAMPLE_COUNT];
static bool rxSamplesCollected = false;
// Update the recent samples
@ -351,8 +351,20 @@ static uint16_t applyChannelFiltering(uint8_t chan, uint16_t sample)
rxSamplesCollected = true;
}
// Apply median filtering
return quickMedianFilter3(rcSamples[chan]);
// Assuming a step transition from 1000 -> 2000 different filters will yield the following output:
// No filter: 1000, 2000, 2000, 2000, 2000 - 0 samples delay
// 3-point moving average: 1000, 1333, 1667, 2000, 2000 - 2 samples delay
// 3-point median: 1000, 1000, 2000, 2000, 2000 - 1 sample delay
// 5-point median: 1000, 1000, 1000, 2000, 2000 - 2 sample delay
// For the same filters - noise rejection capabilities (2 out of 5 outliers
// No filter: 1000, 2000, 1000, 2000, 1000, 1000, 1000
// 3-point MA: 1000, 1333, 1333, 1667, 1333, 1333, 1000 - noise has reduced magnitude, but spread over more samples
// 3-point median: 1000, 1000, 1000, 2000, 1000, 1000, 1000 - high density noise is not removed
// 5-point median: 1000, 1000, 1000, 1000, 1000, 1000, 1000 - only 3 out of 5 outlier noise will get through
// Apply 5-point median filtering. This filter has the same delay as 3-point moving average, but better noise rejection
return quickMedianFilter5_16(rcSamples[chan]);
}
void calculateRxChannelsAndUpdateFailsafe(timeUs_t currentTimeUs)

10
src/main/rx/rx.h
View file

@ -46,9 +46,9 @@
#define DELAY_5_HZ (1000000 / 5)
typedef enum {
RX_FRAME_PENDING = 0,
RX_FRAME_COMPLETE = (1 << 0),
RX_FRAME_FAILSAFE = (1 << 1)
RX_FRAME_PENDING = 0, // No new data available from receiver
RX_FRAME_COMPLETE = (1 << 0), // There is new data available
RX_FRAME_FAILSAFE = (1 << 1) // Receiver detected loss of RC link. Only valid when RX_FRAME_COMPLETE is set as well
} rxFrameState_e;
typedef enum {
@ -71,7 +71,7 @@ typedef enum {
SERIALRX_IBUS = 7,
SERIALRX_JETIEXBUS = 8,
SERIALRX_CRSF = 9
} SerialRXType;
} rxSerialReceiverType_e;
#define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16
#define MAX_SUPPORTED_RC_PARALLEL_PWM_CHANNEL_COUNT 8
@ -105,7 +105,7 @@ PG_DECLARE_ARRAY(rxChannelRangeConfig_t, NON_AUX_CHANNEL_COUNT, rxChannelRangeCo
typedef struct rxConfig_s {
uint8_t receiverType;
uint8_t rcmap[MAX_MAPPABLE_RX_INPUTS]; // mapping of radio channels to internal RPYTA+ order
uint8_t serialrx_provider; // type of UART-based receiver (0 = spek 10, 1 = spek 11, 2 = sbus). Only used if recevierType is RX_TYPE_SERIAL
uint8_t serialrx_provider; // Type of UART-based receiver. Only used if receiverType is RX_TYPE_SERIAL
uint8_t sbus_inversion; // default sbus (Futaba, FrSKY) is inverted. Support for uninverted OpenLRS (and modified FrSKY) receivers.
uint8_t halfDuplex; // allow rx to operate in half duplex mode on F4, ignored for F1 and F3.
uint8_t rx_spi_protocol; // type of SPI receiver protocol. Only used if receiverType is RX_TYPE_SPI

2
src/main/target/COLIBRI/config.c
View file

@ -32,7 +32,7 @@
void targetConfiguration(void)
{
mixerConfigMutable()->mixerMode = MIXER_HEX6X;
rxConfigMutable()->serialrx_provider = 2;
rxConfigMutable()->serialrx_provider = SERIALRX_SBUS;
rxConfigMutable()->receiverType = RX_TYPE_SERIAL;
serialConfigMutable()->portConfigs[2].functionMask = FUNCTION_RX_SERIAL;
}