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

LP-482 Changes from review, add defines

Browse Source
This commit is contained in:
Laurent Lalanne 2017-06-19 09:38:05 +02:00
parent ed28311841
commit c2a7582bd9
1 changed files with 17 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -51,8 +51,13 @@
#define CALIBRATION_DELAY_MS 4000 #define CALIBRATION_DELAY_MS 4000
#define CALIBRATION_DURATION_MS 6000 #define CALIBRATION_DURATION_MS 6000
#define RELOADSETTINGS_DELAY_MS 1000 #define RELOADSETTINGS_DELAY_MS 1000
#define CONVERGENCE_MAGKP 20.0f
#define VARIANCE_WINDOW_SIZE 40 #define VARIANCE_WINDOW_SIZE 40
#define UNDONE 0
#define DONE 1
#define RUN 2
// Private types // Private types
struct data { struct data {
AttitudeSettingsData attitudeSettings; AttitudeSettingsData attitudeSettings;
@ -252,7 +257,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
AttitudeStateData attitudeState; // base on previous state AttitudeStateData attitudeState; // base on previous state
AttitudeStateGet(&attitudeState); AttitudeStateGet(&attitudeState);
this->init = 0; this->init = UNDONE;
// Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly, // Set initial attitude. Use accels to determine roll and pitch, rotate magnetic measurement accordingly,
// so pseudo "north" vector can be estimated even if the board is not level // so pseudo "north" vector can be estimated even if the board is not level
@ -292,7 +297,7 @@ 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 // check whether copter is steady
if (this->init == 0 && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) { if (this->init == UNDONE && this->attitudeSettings.InitialZeroWhenBoardSteady == ATTITUDESETTINGS_INITIALZEROWHENBOARDSTEADY_TRUE) {
pseudo_windowed_variance_push_sample(&this->gyro_var[0], gyro[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[1], gyro[1]);
pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]); pseudo_windowed_variance_push_sample(&this->gyro_var[2], gyro[2]);
@ -308,18 +313,18 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
} }
if (this->init == 0 && xTaskGetTickCount() - this->starttime < CALIBRATION_DELAY_MS / portTICK_RATE_MS) { if (this->init == UNDONE && 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
this->timeval = PIOS_DELAY_GetRaw(); this->timeval = PIOS_DELAY_GetRaw();
return FILTERRESULT_ERROR; return FILTERRESULT_ERROR;
} else if (this->init == 0 && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) { } else if (this->init == UNDONE && xTaskGetTickCount() - this->starttime < (CALIBRATION_DELAY_MS + CALIBRATION_DURATION_MS) / portTICK_RATE_MS) {
// For first 6 seconds use accels to get gyro bias // For first 6 seconds use accels to get gyro bias
this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKp = 1.0f;
this->attitudeSettings.AccelKi = 0.0f; this->attitudeSettings.AccelKi = 0.0f;
this->attitudeSettings.YawBiasRate = 0.23f; this->attitudeSettings.YawBiasRate = 0.23f;
this->accel_filter_enabled = false; this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f; this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 20.0f : 0.0f; this->attitudeSettings.MagKp = this->magCalibrated ? CONVERGENCE_MAGKP : 0.0f;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { } else if ((this->attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
this->attitudeSettings.AccelKp = 1.0f; this->attitudeSettings.AccelKp = 1.0f;
@ -327,21 +332,21 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
this->attitudeSettings.YawBiasRate = 0.23f; this->attitudeSettings.YawBiasRate = 0.23f;
this->accel_filter_enabled = false; this->accel_filter_enabled = false;
this->rollPitchBiasRate = 0.01f; this->rollPitchBiasRate = 0.01f;
this->attitudeSettings.MagKp = this->magCalibrated ? 20.0f : 0.0f; this->attitudeSettings.MagKp = this->magCalibrated ? CONVERGENCE_MAGKP : 0.0f;
this->init = 0; this->init = UNDONE;
PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR); PIOS_NOTIFY_StartNotification(NOTIFY_DRAW_ATTENTION, NOTIFY_PRIORITY_REGULAR);
} else if (this->init == 0) { } else if (this->init == UNDONE) {
this->rollPitchBiasRate = 0.0f; this->rollPitchBiasRate = 0.0f;
if (this->accel_alpha > 0.0f) { if (this->accel_alpha > 0.0f) {
this->accel_filter_enabled = true; this->accel_filter_enabled = true;
} }
this->inittime = xTaskGetTickCount(); this->inittime = xTaskGetTickCount();
this->init = 1; this->init = DONE;
// Allow running filter with custom MagKp for some time before reload settings // Allow running filter with custom MagKp for some time before reload settings
} else if (this->init == 1 && (!this->magCalibrated || (xTaskGetTickCount() - this->inittime > RELOADSETTINGS_DELAY_MS / portTICK_RATE_MS))) { } else if (this->init == DONE && (!this->magCalibrated || (xTaskGetTickCount() - this->inittime > RELOADSETTINGS_DELAY_MS / portTICK_RATE_MS))) {
// Reload settings (all the rates) // Reload settings (all the rates)
AttitudeSettingsGet(&this->attitudeSettings); AttitudeSettingsGet(&this->attitudeSettings);
this->init = 2; this->init = RUN;
} }
// Compute the dT using the cpu clock // Compute the dT using the cpu clock
@ -481,7 +486,7 @@ static filterResult complementaryFilter(struct data *this, float gyro[3], float
return FILTERRESULT_WARNING; return FILTERRESULT_WARNING;
} }
if (this->init > 0) { if (this->init != UNDONE) {
return FILTERRESULT_OK; return FILTERRESULT_OK;
} else { } else {
return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later return FILTERRESULT_CRITICAL; // return "critical" for now, so users can see the zeroing period, switch to more graceful notification later