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

Merge remote-tracking branch 'origin/amorale/OP-990_revo_bias_with_shaken_startup' into next

Browse Source
This commit is contained in:
Alessio Morale 2013-06-09 09:10:33 +02:00
commit 6d71d92860
3 changed files with 53 additions and 19 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

31
flight/modules/Attitude/revolution/attitude.c
View File

@ -318,6 +318,7 @@ static int32_t updateAttitudeComplementary(bool first_run)
static int32_t timeval;
float dT;
static uint8_t init = 0;
static bool magCalibrated = true;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
@ -347,6 +348,15 @@ static int32_t updateAttitudeComplementary(bool first_run)
magData.y = 0.0f;
magData.z = 0.0f;
#endif
float magBias[3];
RevoCalibrationmag_biasGet(magBias);
// don't trust Mag for initial orientation if it has not been calibrated
if (magBias[0] < 1e-6f && magBias[1] < 1e-6f && magBias[2] < 1e-6f) {
magCalibrated = false;
magData.x = 100.0f;
magData.y = 0.0f;
magData.z = 0.0f;
}
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
@ -382,21 +392,22 @@ static int32_t updateAttitudeComplementary(bool first_run)
return 0;
}
if ((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
if ((xTaskGetTickCount() < 10000) && (xTaskGetTickCount() > 4000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1.0f;
attitudeSettings.AccelKi = 0.0f;
attitudeSettings.YawBiasRate = 0.23f;
accel_filter_enabled = false;
rollPitchBiasRate = 0.01f;
attitudeSettings.MagKp = 1.0f;
attitudeSettings.MagKp = magCalibrated ? 1.0f : 0.0f;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
@ -498,22 +509,16 @@ static int32_t updateAttitudeComplementary(bool first_run)
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate;
gyrosBias.z -= -gyrosData.z * rollPitchBiasRate;
} else {
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - (gyrosData.x - gyrosBias.x) * rollPitchBiasRate;;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - (gyrosData.y - gyrosBias.y) * rollPitchBiasRate;
gyrosBias.z -= -(gyrosData.z - gyrosBias.z) * rollPitchBiasRate;
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_FALSE) {
// if the raw values are not adjusted, we need to adjust here.
gyrosData.x -= gyrosBias.x;
gyrosData.y -= gyrosBias.y;
gyrosData.z -= gyrosBias.z;
}
gyrosBias.z -= mag_err[2] * attitudeSettings.MagKi;
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi - gyrosData.x * rollPitchBiasRate;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi - gyrosData.y * rollPitchBiasRate;
gyrosBias.z -= -mag_err[2] * attitudeSettings.MagKi - gyrosData.z * rollPitchBiasRate;
GyrosBiasSet(&gyrosBias);
// Correct rates based on error, integral component dealt with in updateSensors

39
ground/openpilotgcs/src/plugins/setupwizard/biascalibrationutil.cpp
View File

@ -32,6 +32,7 @@
#include "accels.h"
#include "gyros.h"
#include "qdebug.h"
#include "revocalibration.h"
BiasCalibrationUtil::BiasCalibrationUtil(long measurementCount, long measurementRate) : QObject(),
@ -133,15 +134,37 @@ void BiasCalibrationUtil::startMeasurement()
m_gyroY = 0;
m_gyroZ = 0;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager);
RevoCalibration *revolutionCalibration = RevoCalibration::GetInstance(uavObjectManager);
Q_ASSERT(revolutionCalibration);
RevoCalibration::DataFields revoCalibrationData = revolutionCalibration->getData();
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_X] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Y] = 0;
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_X] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Y] = 0;
revoCalibrationData.gyro_bias[RevoCalibration::GYRO_BIAS_Z] = 0;
revoCalibrationData.BiasCorrectedRaw = RevoCalibration::BIASCORRECTEDRAW_FALSE;
int i;
for (i = 0; i < 5; i++) {
RevoCalibration::GetInstance(uavObjectManager)->setData(revoCalibrationData);
}
// Disable gyro bias correction to see raw data
AttitudeSettings::DataFields attitudeSettingsData = AttitudeSettings::GetInstance(uavObjectManager)->getData();
attitudeSettingsData.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_FALSE;
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_X] = 0;
attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_Y] = 0;
attitudeSettingsData.AccelBias[AttitudeSettings::ACCELBIAS_Z] = 0;
attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_X] = 0;
attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_Y] = 0;
attitudeSettingsData.GyroBias[AttitudeSettings::GYROBIAS_Z] = 0;
for (i = 0; i < 5; i++) {
AttitudeSettings::GetInstance(uavObjectManager)->setData(attitudeSettingsData);
}
// Set up to receive updates for accels
UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(accelMeasurementsUpdated(UAVObject *)));
@ -151,8 +174,10 @@ void BiasCalibrationUtil::startMeasurement()
UAVObject::Metadata newMetaData = m_previousAccelMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate;
uavObject->setMetadata(newMetaData);
for (i = 0; i < 5; i++) {
uavObject->setMetadata(newMetaData);
}
// Set up to receive updates from gyros
uavObject = Gyros::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject *)), this, SLOT(gyroMeasurementsUpdated(UAVObject *)));
@ -162,7 +187,9 @@ void BiasCalibrationUtil::startMeasurement()
newMetaData = m_previousGyroMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate;
uavObject->setMetadata(newMetaData);
for (i = 0; i < 5; i++) {
uavObject->setMetadata(newMetaData);
}
}
void BiasCalibrationUtil::stopMeasurement()

2
ground/openpilotgcs/src/plugins/setupwizard/vehicleconfigurationhelper.cpp
View File

@ -357,6 +357,8 @@ void VehicleConfigurationHelper::applySensorBiasConfiguration()
data.GyroBias[AttitudeSettings::GYROBIAS_Y] = -(bias.m_gyroYBias * GYRO_SCALE);
data.GyroBias[AttitudeSettings::GYROBIAS_Z] = -(bias.m_gyroZBias * GYRO_SCALE);
data.BiasCorrectGyro = AttitudeSettings::BIASCORRECTGYRO_TRUE;
copterControlCalibration->setData(data);
addModifiedObject(copterControlCalibration, tr("Writing gyro and accelerometer bias settings"));
break;