Mirrors_Framework/paparazzi
1
0
Fork 0
mirror of https://github.com/paparazzi/paparazzi.git synced 2026-06-04 13:55:40 +08:00
Code Issues Actions 27 Packages Projects Releases Wiki Activity

[ardrone] updated filter: use more data in case of spikes: only 1 package lost instead of 3

Browse Source
This commit is contained in:
Christophe De Wagter
2013-12-20 14:41:34 +01:00
parent 7c466ff14c
commit d2f8013528
1 changed files with 53 additions and 8 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/ardrone/navdata.c
+53 -8
View File
@@ -267,6 +267,8 @@ static void baro_update_logic(void)
{ {
static int32_t lastpressval = 0; static int32_t lastpressval = 0;
static uint16_t lasttempval = 0; static uint16_t lasttempval = 0;
static int32_t lastpressval_nospike = 0;
static uint16_t lasttempval_nospike = 0;
static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press static uint8_t temp_or_press_was_updated_last = 0; // 0 = press, so we now wait for temp, 1 = temp so we now wait for press
static int sync_errors = 0; static int sync_errors = 0;
@@ -311,11 +313,7 @@ static void baro_update_logic(void)
} }
} }
navdata_baro_available = TRUE;
if (spike_detected == 0)
{
navdata_baro_available = TRUE;
}
} }
/* /*
@@ -333,10 +331,32 @@ static void baro_update_logic(void)
* reading them too quickly (trying to catch up for delay that happened earlier due to still non understood * reading them too quickly (trying to catch up for delay that happened earlier due to still non understood
* reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with * reasons. As pressure is more likely to quickly change, a small (yet unlikely) spike on temperature together with
* press==temp yields very good results as a detector, although theoretically not perfect. * press==temp yields very good results as a detector, although theoretically not perfect.
#sample press temp
50925 39284 34501
50926 39287 34501
50927 39287 34501 0 0
50928 39283 34501 0 -4
50929 39283 34501 0 0
50930 39285 34501 0 2
50931 39285 34500 -1 0
50932 34500 34500 0 -4785
50933 34500 36618 2118 0
50934 39284 36618 0 4784
50935 39284 34501 -2117 0
50936 39284 34501 0 0
50937 39284 34500 -1 0
50938 39281 34500 0 -3
50939 39281 34500 0 0
50940 39280 34500 0 -1
50941 39280 34502 2 0
50942 39280 34502 0 0
50943 39280 34501 -1 0
*/ */
// if press and temp are same and temp has jump: neglect the next frame // if press and temp are same and temp has jump: neglect the next frame
if ((navdata.temperature_pressure == navdata.pressure) && (abs(navdata.temperature_pressure - lasttempval) > 40)) if (navdata.temperature_pressure == navdata.pressure) // && (abs(navdata.temperature_pressure - lasttempval) > 40))
{ {
// dont use next 3 packets // dont use next 3 packets
spike_detected = 3; spike_detected = 3;
@@ -346,11 +366,36 @@ static void baro_update_logic(void)
spikes++; spikes++;
printf("Spike! # %d\n",spikes); printf("Spike! # %d\n",spikes);
} }
if (spike_detected)
spike_detected--;
lastpressval = navdata.pressure; lastpressval = navdata.pressure;
lasttempval = navdata.temperature_pressure; lasttempval = navdata.temperature_pressure;
if (spike_detected == 3) // Pressure is wrong
{
// override press
navdata.pressure = lastpressval_nospike;
lasttempval_nospike = navdata.temperature_pressure;
}
else if (spike_detected == 2) // both are wrong
{
// override both
navdata.pressure = lastpressval_nospike;
navdata.temperature_pressure = lasttempval_nospike;
}
else if (spike_detected == 1) // temp is still wrong
{
lastpressval_nospike = navdata.pressure;
navdata.temperature_pressure = lasttempval_nospike;
}
else // both are good
{
lastpressval_nospike = navdata.pressure;
lasttempval_nospike = navdata.temperature_pressure;
}
if (spike_detected)
spike_detected--;
} }
void navdata_update() void navdata_update()