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:
commit
831aa2e213
@ -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)) {
|
||||
|
@ -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;
|
||||
}
|
||||
|
@ -1,4 +1,4 @@
|
||||
html>
|
||||
<html>
|
||||
<head>
|
||||
<title></title>
|
||||
<meta content="">
|
||||
|
@ -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"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user