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 30 Packages Projects Releases Wiki Activity

[ardrone] solved both type1 and type2 spikes: delayed temp versus delayed press

Browse Source
This commit is contained in:
Christophe De Wagter
2013-12-20 21:24:46 +01:00
parent d8f1954bb7
commit e285f0221c
1 changed files with 35 additions and 41 deletions
Download Patch File Download Diff File Expand all files Collapse all files
sw/airborne/boards/ardrone/navdata.c
+34 -40
View File
@@ -316,6 +316,9 @@ static void baro_update_logic(void)
navdata_baro_available = TRUE; navdata_baro_available = TRUE;
} }
lastpressval = navdata.pressure;
lasttempval = navdata.temperature_pressure;
/* /*
* It turns out that a lot of navdata boards have a problem (probably interrupt related) * It turns out that a lot of navdata boards have a problem (probably interrupt related)
* in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz). * in which reading I2C data and writing uart output data is interrupted very long (50% of 200Hz).
@@ -332,61 +335,50 @@ static void baro_update_logic(void)
* 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 #samp press temp.
50925 39284 34501 50925 39284 34501
50926 39287 34501 50926 39287 34501
50927 39287 34501 0 0 50927 39287 34501
50928 39283 34501 0 -4 50928 39283 34501 // *press good -> baro
50929 39283 34501 0 0 50929 39283 34501
50930 39285 34501 0 2 50930 39285 34501 // *press good -> baro
50931 39285 34500 -1 0 50931 39285 34500
50932 34500 34500 0 -4785 50932 34500 34500 // press read too soon from chip (<4.5ms) -> ADC register still previous temp value
50933 34500 36618 2118 0 50933 34500 36618 // press not updated, still wrong. Temp is weird: looks like the average of both
50934 39284 36618 0 4784 50934 39284 36618 // new press read, but temp still outdated
50935 39284 34501 -2117 0 50935 39284 34501
50936 39284 34501 0 0 50936 39284 34501 // *press good -> baro
50937 39284 34500 -1 0 50937 39284 34500
50938 39281 34500 0 -3 50938 39281 34500
50939 39281 34500 0 0 50939 39281 34500
50940 39280 34500 0 -1 50940 39280 34500
50941 39280 34502 2 0 50941 39280 34502
50942 39280 34502 0 0 50942 39280 34502
50943 39280 34501 -1 0 50943 39280 34501
*/ */
// 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((int32_t)navdata.temperature_pressure - (int32_t)lasttempval) > 40))
{ {
// dont use next 3 packets // dont use next 3 packets
spike_detected = 3; spike_detected = 3;
// dont use
navdata_baro_available = FALSE;
spikes++; spikes++;
printf("Spike! # %d\n",spikes); printf("Spike! # %d\n",spikes);
} }
lastpressval = navdata.pressure; if (spike_detected > 0)
lasttempval = navdata.temperature_pressure;
if (spike_detected == 3) // Pressure is wrong
{ {
// override press // disable kalman filter use
navdata.pressure = lastpressval_nospike; navdata_baro_available = FALSE;
lasttempval_nospike = navdata.temperature_pressure; // override both to last good
}
else if (spike_detected == 2) // both are wrong
{
// override both
navdata.pressure = lastpressval_nospike; navdata.pressure = lastpressval_nospike;
navdata.temperature_pressure = lasttempval_nospike; navdata.temperature_pressure = lasttempval_nospike;
}
else if (spike_detected == 1) // temp is still wrong // Countdown
{ spike_detected--;
lastpressval_nospike = navdata.pressure;
navdata.temperature_pressure = lasttempval_nospike;
} }
else // both are good else // both are good
{ {
@@ -394,8 +386,7 @@ static void baro_update_logic(void)
lasttempval_nospike = navdata.temperature_pressure; lasttempval_nospike = navdata.temperature_pressure;
} }
if (spike_detected) // printf(",%d,%d",spike_detected,spikes);
spike_detected--;
} }
void navdata_update() void navdata_update()
@@ -452,10 +443,13 @@ void navdata_update()
p[0] = p[1]; p[0] = p[1];
p[1] = tmp; p[1] = tmp;
// printf("%d %d %d\n",navdata.nu_trame, navdata.pressure, navdata.temperature_pressure); // printf("%d,%d,%d",navdata.nu_trame, navdata.pressure, navdata.temperature_pressure);
baro_update_logic(); baro_update_logic();
// printf(",%d,%d,%d\n", navdata.pressure, navdata.temperature_pressure, (int)navdata_baro_available);
#ifdef USE_SONAR #ifdef USE_SONAR
if (navdata.ultrasound < 10000) if (navdata.ultrasound < 10000)
{ {