rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-19 04:52:12 +01:00
Code Issues Releases Activity

LP-327 - Determine if copter is steady checking gyro variance before calibrating its bias

Browse Source
This commit is contained in:
Alessio Morale 2016-06-28 00:01:46 +02:00
parent 9774045699
commit 2ce258e365
1 changed files with 24 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
flight/modules/StateEstimation/filtercf.c
View File

@ -40,7 +40,7 @@
#include <flightstatus.h> #include <flightstatus.h>
#include <homelocation.h> #include <homelocation.h>
#include <revocalibration.h> #include <revocalibration.h>
#include <mathmisc.h>
#include <CoordinateConversions.h> #include <CoordinateConversions.h>
#include <pios_notify.h> #include <pios_notify.h>
// Private constants // Private constants
@ -49,7 +49,7 @@
#define CALIBRATION_DELAY_MS 4000 #define CALIBRATION_DELAY_MS 4000
#define CALIBRATION_DURATION_MS 6000 #define CALIBRATION_DURATION_MS 6000
#define VARIANCE_WINDOW_SIZE 40
// Private types // Private types
struct data { struct data {
AttitudeSettingsData attitudeSettings; AttitudeSettingsData attitudeSettings;
@ -70,6 +70,7 @@ struct data {
int32_t starttime; int32_t starttime;
uint8_t init; uint8_t init;
bool magCalibrated; bool magCalibrated;
pw_variance_t gyro_var[3];
}; };
// Private variables // Private variables
@ -158,7 +159,6 @@ static int32_t maininit(stateFilter *self)
this->gyroBias[0] = 0.0f; this->gyroBias[0] = 0.0f;
this->gyroBias[1] = 0.0f; this->gyroBias[1] = 0.0f;
this->gyroBias[2] = 0.0f; this->gyroBias[2] = 0.0f;
return 0; return 0;
} }
@ -231,6 +231,11 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
mag[1] = 0.0f; mag[1] = 0.0f;
mag[2] = 0.0f; mag[2] = 0.0f;
#endif #endif
pseudo_windowed_variance_init(&this->gyro_var[0], VARIANCE_WINDOW_SIZE);
pseudo_windowed_variance_init(&this->gyro_var[1], VARIANCE_WINDOW_SIZE);
pseudo_windowed_variance_init(&this->gyro_var[2], VARIANCE_WINDOW_SIZE);
float magBias[3]; float magBias[3];
RevoCalibrationmag_biasArrayGet(magBias); RevoCalibrationmag_biasArrayGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated // don't trust Mag for initial orientation if it has not been calibrated
@ -282,6 +287,22 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion return FILTERRESULT_OK; // must return OK on initial initialization, so attitude will init with a valid quaternion
} }
// check whether copter is steady
if (this->init == 0) {
pseudo_windowed_variance_push_sample(&this->gyro_var[0], gyro[0]);
pseudo_windowed_variance_push_sample(&this->gyro_var[1], gyro[1]);
pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]);
float const gyrovarx = pseudo_windowed_variance_get(&this->gyro_var[0]);
float const gyrovary = pseudo_windowed_variance_get(&this->gyro_var[1]);
float const gyrovarz = pseudo_windowed_variance_get(&this->gyro_var[2]);
if ((fabsf(gyrovarx) + fabsf(gyrovary) + fabsf(gyrovarz)) > 1.0f) {
this->starttime = xTaskGetTickCount();
this->first_run = 1;
return FILTERRESULT_WARNING;
}
}
if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) {
// wait 4 seconds for the user to get his hands off in case the board was just powered // wait 4 seconds for the user to get his hands off in case the board was just powered