mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-21 11:54:15 +01:00
ported cyr's patch to new CF
This commit is contained in:
parent
1353be6cb0
commit
b916df5448
@ -59,6 +59,7 @@ struct data {
|
|||||||
bool magUpdated;
|
bool magUpdated;
|
||||||
float accel_alpha;
|
float accel_alpha;
|
||||||
bool accel_filter_enabled;
|
bool accel_filter_enabled;
|
||||||
|
float rollPitchBiasRate;
|
||||||
int32_t timeval;
|
int32_t timeval;
|
||||||
uint8_t init;
|
uint8_t init;
|
||||||
};
|
};
|
||||||
@ -207,8 +208,6 @@ static inline void apply_accel_filter(const struct data *this, const float *raw,
|
|||||||
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
|
static int32_t complementaryFilter(struct data *this, float gyro[3], float accel[3], float mag[3], float attitude[4])
|
||||||
{
|
{
|
||||||
float dT;
|
float dT;
|
||||||
float magKp = 0.0f; // TODO: make this non hardcoded at some point
|
|
||||||
float magKi = 0.000001f;
|
|
||||||
|
|
||||||
// During initialization and
|
// During initialization and
|
||||||
if (this->first_run) {
|
if (this->first_run) {
|
||||||
@ -261,19 +260,26 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
|
|||||||
if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
if ((this->init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
|
||||||
// For first 7 seconds use accels to get gyro bias
|
// For first 7 seconds use accels to get gyro bias
|
||||||
this->attitudeSettings.AccelKp = 1.0f;
|
this->attitudeSettings.AccelKp = 1.0f;
|
||||||
this->attitudeSettings.AccelKi = 0.9f;
|
this->attitudeSettings.AccelKi = 0.0f;
|
||||||
this->attitudeSettings.YawBiasRate = 0.23f;
|
this->attitudeSettings.YawBiasRate = 0.23f;
|
||||||
magKp = 1.0f;
|
this->accel_filter_enabled = false;
|
||||||
|
this->rollPitchBiasRate = 0.01f;
|
||||||
|
this->attitudeSettings.MagKp = 1.0f;
|
||||||
} 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;
|
||||||
this->attitudeSettings.AccelKi = 0.9f;
|
this->attitudeSettings.AccelKi = 0.0f;
|
||||||
this->attitudeSettings.YawBiasRate = 0.23f;
|
this->attitudeSettings.YawBiasRate = 0.23f;
|
||||||
magKp = 1.0f;
|
this->accel_filter_enabled = false;
|
||||||
|
this->rollPitchBiasRate = 0.01f;
|
||||||
|
this->attitudeSettings.MagKp = 1.0f;
|
||||||
this->init = 0;
|
this->init = 0;
|
||||||
} else if (this->init == 0) {
|
} else if (this->init == 0) {
|
||||||
// Reload settings (all the rates)
|
// Reload settings (all the rates)
|
||||||
AttitudeSettingsGet(&this->attitudeSettings);
|
AttitudeSettingsGet(&this->attitudeSettings);
|
||||||
magKp = 0.01f;
|
this->rollPitchBiasRate = 0.0f;
|
||||||
|
if (this->accel_alpha > 0.0f) {
|
||||||
|
this->accel_filter_enabled = true;
|
||||||
|
}
|
||||||
this->init = 1;
|
this->init = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -356,22 +362,26 @@ static int32_t complementaryFilter(struct data *this, float gyro[3], float accel
|
|||||||
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
|
mag_err[0] = mag_err[1] = mag_err[2] = 0.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
|
||||||
this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi;
|
|
||||||
this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi;
|
|
||||||
this->gyroBias[2] -= mag_err[2] * magKi;
|
|
||||||
|
|
||||||
// Correct rates based on integral coefficient
|
// Correct rates based on integral coefficient
|
||||||
gyro[0] -= this->gyroBias[0];
|
gyro[0] -= this->gyroBias[0];
|
||||||
gyro[1] -= this->gyroBias[1];
|
gyro[1] -= this->gyroBias[1];
|
||||||
gyro[2] -= this->gyroBias[2];
|
gyro[2] -= this->gyroBias[2];
|
||||||
|
|
||||||
|
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
|
||||||
|
this->gyroBias[0] -= accel_err[0] * this->attitudeSettings.AccelKi - gyro[0] * this->rollPitchBiasRate;
|
||||||
|
this->gyroBias[1] -= accel_err[1] * this->attitudeSettings.AccelKi - gyro[1] * this->rollPitchBiasRate;;
|
||||||
|
if (this->useMag) {
|
||||||
|
this->gyroBias[2] -= mag_err[2] * this->attitudeSettings.MagKi - gyro[2] * this->rollPitchBiasRate;
|
||||||
|
} else {
|
||||||
|
this->gyroBias[2] -= -gyro[2] * this->rollPitchBiasRate;
|
||||||
|
}
|
||||||
|
|
||||||
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
|
float gyrotmp[3] = { gyro[0], gyro[1], gyro[2] };
|
||||||
// Correct rates based on proportional coefficient
|
// Correct rates based on proportional coefficient
|
||||||
gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
|
gyrotmp[0] += accel_err[0] * this->attitudeSettings.AccelKp / dT;
|
||||||
gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
|
gyrotmp[1] += accel_err[1] * this->attitudeSettings.AccelKp / dT;
|
||||||
if (this->useMag) {
|
if (this->useMag) {
|
||||||
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
|
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT + mag_err[2] * this->attitudeSettings.MagKp / dT;
|
||||||
} else {
|
} else {
|
||||||
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;
|
gyrotmp[2] += accel_err[2] * this->attitudeSettings.AccelKp / dT;
|
||||||
}
|
}
|
||||||
|
@ -7,6 +7,8 @@
|
|||||||
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
|
<field name="GyroGain" units="(rad/s)/lsb" type="float" elements="1" defaultvalue="0.42"/>
|
||||||
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
|
<field name="AccelKp" units="channel" type="float" elements="1" defaultvalue="0.05"/>
|
||||||
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
|
<field name="AccelKi" units="channel" type="float" elements="1" defaultvalue="0.0001"/>
|
||||||
|
<field name="MagKi" units="" type="float" elements="1" defaultvalue="0"/>
|
||||||
|
<field name="MagKp" units="" type="float" elements="1" defaultvalue="0"/>
|
||||||
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0"/>
|
<field name="AccelTau" units="" type="float" elements="1" defaultvalue="0"/>
|
||||||
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
|
<field name="YawBiasRate" units="channel" type="float" elements="1" defaultvalue="0.000001"/>
|
||||||
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
|
<field name="ZeroDuringArming" units="channel" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="TRUE"/>
|
||||||
|
Loading…
x
Reference in New Issue
Block a user