1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

Merge remote-tracking branch 'origin/next' into brian/GPIO_update

This commit is contained in:
Brian Webb 2013-08-02 17:45:06 -07:00
commit 831aa2e213
4 changed files with 30 additions and 18 deletions

View File

@ -32,19 +32,16 @@
#include "inc/stateestimation.h"
#include <revosettings.h>
// Private constants
#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
struct data {
float baroOffset;
float baroGPSOffsetCorrectionAlpha;
float baroAlt;
int16_t first_run;
};
@ -71,6 +68,10 @@ static int32_t init(stateFilter *self)
this->baroOffset = 0.0f;
this->first_run = 100;
RevoSettingsInitialize();
RevoSettingsBaroGPSOffsetCorrectionAlphaGet(&this->baroGPSOffsetCorrectionAlpha);
return 0;
}
@ -90,9 +91,8 @@ static int32_t filter(stateFilter *self, stateEstimation *state)
// Track barometric altitude offset with a low pass filter
// based on GPS altitude if available
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->baroOffset = BARO_OFFSET_LOWPASS_ALPHA * this->baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA)
* (this->baroAlt + state->pos[2]);
this->baroOffset = this->baroOffset * this->baroGPSOffsetCorrectionAlpha +
(1.0f - this->baroGPSOffsetCorrectionAlpha) * (this->baroAlt + state->pos[2]);
}
// calculate bias corrected altitude
if (IS_SET(state->updated, SENSORUPDATES_baro)) {

View File

@ -55,6 +55,8 @@ struct data {
bool useMag;
float currentAccel[3];
float currentMag[3];
float accels_filtered[3];
float grot_filtered[3];
float gyroBias[3];
bool accelUpdated;
bool magUpdated;
@ -266,7 +268,12 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
RPY2Quaternion(&attitudeState.Roll, attitude);
this->first_run = 0;
this->accels_filtered[0] = 0.0f;
this->accels_filtered[1] = 0.0f;
this->accels_filtered[2] = 0.0f;
this->grot_filtered[0] = 0.0f;
this->grot_filtered[1] = 0.0f;
this->grot_filtered[2] = 0.0f;
this->timeval = PIOS_DELAY_GetRaw();
this->starttime = this->timeval;
@ -316,9 +323,8 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
// Get the current attitude estimate
quat_copy(&attitudeState.q1, attitude);
float accels_filtered[3];
// Apply smoothing to accel values, to reduce vibration noise before main calculations.
apply_accel_filter(this, accel, accels_filtered);
apply_accel_filter(this, accel, this->accels_filtered);
// Rotate gravity to body frame and cross with accels
float grot[3];
@ -326,13 +332,13 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
grot[1] = -(2.0f * (attitude[2] * attitude[3] + attitude[0] * attitude[1]));
grot[2] = -(attitude[0] * attitude[0] - attitude[1] * attitude[1] - attitude[2] * attitude[2] + attitude[3] * attitude[3]);
float grot_filtered[3];
float accel_err[3];
apply_accel_filter(this, grot, grot_filtered);
CrossProduct((const float *)accels_filtered, (const float *)grot_filtered, accel_err);
apply_accel_filter(this, grot, this->grot_filtered);
CrossProduct((const float *)this->accels_filtered, (const float *)this->grot_filtered, accel_err);
// Account for accel magnitude
float accel_mag = sqrtf(accels_filtered[0] * accels_filtered[0] + accels_filtered[1] * accels_filtered[1] + accels_filtered[2] * accels_filtered[2]);
float accel_mag = sqrtf(this->accels_filtered[0] * this->accels_filtered[0] + this->accels_filtered[1] * this->accels_filtered[1] + this->accels_filtered[2] * this->accels_filtered[2]);
if (accel_mag < 1.0e-3f) {
return 2; // safety feature copied from CC
}
@ -340,7 +346,7 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
// Account for filtered gravity vector magnitude
float grot_mag;
if (this->accel_filter_enabled) {
grot_mag = sqrtf(grot_filtered[0] * grot_filtered[0] + grot_filtered[1] * grot_filtered[1] + grot_filtered[2] * grot_filtered[2]);
grot_mag = sqrtf(this->grot_filtered[0] * this->grot_filtered[0] + this->grot_filtered[1] * this->grot_filtered[1] + this->grot_filtered[2] * this->grot_filtered[2]);
} else {
grot_mag = 1.0f;
}

View File

@ -1,4 +1,4 @@
html>
<html>
<head>
<title></title>
<meta content="">

View File

@ -2,6 +2,12 @@
<object name="RevoSettings" singleinstance="true" settings="true" category="State">
<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"/>
<!-- 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"/>
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
<telemetryflight acked="true" updatemode="onchange" period="0"/>