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

OP-1406 - Fixes from review comments

Browse Source
This commit is contained in:
Alessio Morale 2014-09-08 23:24:18 +02:00
parent 157ab9c0d2
commit c777f9e194
4 changed files with 18 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/modules/StateEstimation/stateestimation.c
View File

@ -483,7 +483,7 @@ static void StateEstimationCb(void)
s.z = states.mag[2];
switch (states.magStatus) {
case MAGSTATUS_OK:
s.Source = MAGSTATE_SOURCE_OK;
s.Source = MAGSTATE_SOURCE_ONBOARD;
break;
case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_AUX;

18
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.cpp
View File

@ -45,7 +45,7 @@ namespace OpenPilot {
SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent),
calibratingMag(false),
externalMagAvailable(false),
calibratingAuxMag(false),
calibratingAccel(false),
calibrationStepsMag(),
calibrationStepsAccelOnly(),
@ -334,7 +334,7 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
aux_mag_accum_x.append(auxMagData.x);
aux_mag_accum_y.append(auxMagData.y);
aux_mag_accum_z.append(auxMagData.z);
externalMagAvailable = true;
calibratingAuxMag = true;
#ifndef FITTING_USING_CONTINOUS_ACQUISITION
aux_mag_fit_x.append(auxMagData.x);
aux_mag_fit_y.append(auxMagData.y);
@ -425,7 +425,7 @@ void SixPointCalibrationModel::continouslyGetMagSamples(UAVObject *obj)
aux_mag_fit_x.append(auxMagData.x);
aux_mag_fit_y.append(auxMagData.y);
aux_mag_fit_z.append(auxMagData.z);
externalMagAvailable = true;
calibratingAuxMag = true;
}
}
}
@ -463,10 +463,10 @@ void SixPointCalibrationModel::compute()
qDebug() << "-----------------------------------";
qDebug() << "Onboard Mag";
CalcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
if (externalMagAvailable) {
calcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
if (calibratingAuxMag) {
qDebug() << "Aux Mag";
CalcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias);
calcCalibration(aux_mag_fit_x, aux_mag_fit_y, aux_mag_fit_z, Be_length, auxCalibrationData.mag_transform, auxCalibrationData.mag_bias);
}
}
// Restore the previous setting
@ -491,7 +491,7 @@ void SixPointCalibrationModel::compute()
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_X]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]);
if (externalMagAvailable) {
if (calibratingAuxMag) {
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C0]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]);
@ -537,7 +537,7 @@ void SixPointCalibrationModel::compute()
position = -1;
}
void SixPointCalibrationModel::CalcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[])
void SixPointCalibrationModel::calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[])
{
int vectSize = x.count();
Eigen::VectorXf samples_x(vectSize);
@ -589,7 +589,7 @@ void SixPointCalibrationModel::save()
}
revoCalibration->setData(revoCalibrationData);
if (externalMagAvailable) {
if (calibratingAuxMag) {
AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
// Note that Revo/AuxMag MAG_TRANSFORM_RxCx are interchangeable, an assertion at initialization enforces the structs are equal
for (int i = 0; i < RevoCalibration::MAG_TRANSFORM_NUMELEM; i++) {

12
ground/openpilotgcs/src/plugins/config/calibration/sixpointcalibrationmodel.h
View File

@ -104,7 +104,7 @@ public:
} Result;
bool calibratingMag;
bool externalMagAvailable;
bool calibratingAuxMag;
bool calibratingAccel;
QList<CalibrationStep> calibrationStepsMag;
@ -125,15 +125,17 @@ public:
QList<double> accel_accum_x;
QList<double> accel_accum_y;
QList<double> accel_accum_z;
QList<double> mag_accum_x;
QList<double> mag_accum_y;
QList<double> mag_accum_z;
QList<double> aux_mag_accum_x;
QList<double> aux_mag_accum_y;
QList<double> aux_mag_accum_z;
QList<float> mag_fit_x;
QList<float> mag_fit_y;
QList<float> mag_fit_z;
QList<double> aux_mag_accum_x;
QList<double> aux_mag_accum_y;
QList<double> aux_mag_accum_z;
QList<float> aux_mag_fit_x;
QList<float> aux_mag_fit_y;
QList<float> aux_mag_fit_z;
@ -152,7 +154,7 @@ public:
void compute();
void showHelp(QString image);
UAVObjectManager *getObjectManager();
void CalcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[]);
void calcCalibration(QList<float> x, QList<float> y, QList<float> z, double Be_length, float calibrationMatrix[], float bias[]);
};
}

2
shared/uavobjectdefinition/magstate.xml
View File

@ -4,7 +4,7 @@
<field name="x" units="mGa" type="float" elements="1"/>
<field name="y" units="mGa" type="float" elements="1"/>
<field name="z" units="mGa" type="float" elements="1"/>
<field name="Source" units="" type="enum" elements="1" options="Invalid,Ok,Aux" defaultvalue="Invalid"/>
<field name="Source" units="" type="enum" elements="1" options="Invalid,OnBoard,Aux" defaultvalue="Invalid"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>