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

Mag calibration, does this make any sense?

Browse Source
This commit is contained in:
Sambas 2012-05-27 22:15:31 +03:00
parent f75134c6d5
commit 75bc00c534
1 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ground/openpilotgcs/src/plugins/config/configrevowidget.cpp
View File

@ -40,6 +40,7 @@
#include <QDesktopServices> #include <QDesktopServices>
#include <QUrl> #include <QUrl>
#include <revocalibration.h> #include <revocalibration.h>
#include <homelocation.h>
#include <accels.h> #include <accels.h>
#include <gyros.h> #include <gyros.h>
#include <magnetometer.h> #include <magnetometer.h>
@ -571,12 +572,16 @@ int SixPointInConstFieldCal( double ConstMag, double x[6], double y[6], double z
void ConfigRevoWidget::computeScaleBias() void ConfigRevoWidget::computeScaleBias()
{ {
double S[3], b[3]; double S[3], b[3];
double Be_lenght;
RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager()); RevoCalibration * revoCalibration = RevoCalibration::GetInstance(getObjectManager());
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(revoCalibration); Q_ASSERT(revoCalibration);
Q_ASSERT(homeLocation);
RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData(); RevoCalibration::DataFields revoCalibrationData = revoCalibration->getData();
HomeLocation::DataFields homeLocationData = homeLocation->getData();
// Calibration accel // Calibration accel
SixPointInConstFieldCal( GRAVITY, accel_data_x, accel_data_y, accel_data_z, S, b); SixPointInConstFieldCal( homeLocationData.g_e, accel_data_x, accel_data_y, accel_data_z, S, b);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]); revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_X] = fabs(S[0]);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]); revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Y] = fabs(S[1]);
revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]); revoCalibrationData.accel_scale[RevoCalibration::ACCEL_SCALE_Z] = fabs(S[2]);
@ -586,7 +591,8 @@ void ConfigRevoWidget::computeScaleBias()
revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = -sign(S[2]) * b[2]; revoCalibrationData.accel_bias[RevoCalibration::ACCEL_BIAS_Z] = -sign(S[2]) * b[2];
// Calibration mag // Calibration mag
SixPointInConstFieldCal( 1000, mag_data_x, mag_data_y, mag_data_z, S, b); Be_lenght = sqrt(pow(homeLocationData.Be[0],2)+pow(homeLocationData.Be[1],2)+pow(homeLocationData.Be[2],2));
SixPointInConstFieldCal( Be_lenght, mag_data_x, mag_data_y, mag_data_z, S, b);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]); revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_X] = fabs(S[0]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]); revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Y] = fabs(S[1]);
revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]); revoCalibrationData.mag_scale[RevoCalibration::MAG_SCALE_Z] = fabs(S[2]);