rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-22 12:54:14 +01:00
Code Issues Releases Activity

Merge remote-tracking branch 'origin/os/baro-offset-lpf' into next

Browse Source
This commit is contained in:
Oleg Semyonov 2013-07-30 23:41:37 +02:00
commit 77a6db3007
2 changed files with 15 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
flight/modules/StateEstimation/filterbaro.c
View File

@ -32,19 +32,16 @@
#include "inc/stateestimation.h" #include "inc/stateestimation.h"
#include <revosettings.h>
// Private constants // Private constants
#define STACK_REQUIRED 64 #define STACK_REQUIRED 64
// low pass filter configuration to calculate offset
// of barometric altitude sensor
// reasoning: updates at: 10 Hz, tau= 300 s settle time
// exp(-(1/f) / tau ) ~=~ 0.9997
#define BARO_OFFSET_LOWPASS_ALPHA 0.9997f
// Private types // Private types
struct data { struct data {
float baroOffset; float baroOffset;
float baroGPSOffsetCorrectionAlpha;
float baroAlt; float baroAlt;
int16_t first_run; int16_t first_run;
}; };
@ -71,6 +68,10 @@ static int32_t init(stateFilter *self)
this->baroOffset = 0.0f; this->baroOffset = 0.0f;
this->first_run = 100; this->first_run = 100;
RevoSettingsInitialize();
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
return 0; return 0;
} }
@ -90,9 +91,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Track barometric altitude offset with a low pass filter // Track barometric altitude offset with a low pass filter
// based on GPS altitude if available // based on GPS altitude if available
if (IS_SET(state->updated, SENSORUPDATES_pos)) { if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset + this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA) (1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
* (this->baroAlt + state->pos[2]);
} }
// calculate bias corrected altitude // calculate bias corrected altitude
if (IS_SET(state->updated, SENSORUPDATES_baro)) { if (IS_SET(state->updated, SENSORUPDATES_baro)) {

6
shared/uavobjectdefinition/revosettings.xml
View File

@ -2,6 +2,12 @@
<object name="RevoSettings" singleinstance="true" settings="true" category="State"> <object name="RevoSettings" singleinstance="true" settings="true" category="State">
<description>Settings for the revo to control the algorithm and what is updated</description> <description>Settings for the revo to control the algorithm and what is updated</description>
<field name="FusionAlgorithm" units="" type="enum" elements="1" options="None,Complementary,Complementary+Mag,INS13Indoor,INS13Outdoor" defaultvalue="Complementary"/> <field name="FusionAlgorithm" units="" type="enum" elements="1" options="None,Complementary,Complementary+Mag,INS13Indoor,INS13Outdoor" defaultvalue="Complementary"/>
<!-- Low pass filter configuration to calculate offset of barometric altitude sensor.
Defaults: updates at 5 Hz, tau = 300s settle time, exp(-(1/f)/tau) ~= 0.9993335555062
Set BaroGPSOffsetCorrectionAlpha = 1.0 to completely disable baro offset updates. -->
<field name="BaroGPSOffsetCorrectionAlpha" units="" type="float" elements="1" defaultvalue="0.9993335555062"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/> <telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/> <telemetryflight acked="true" updatemode="onchange" period="0"/>