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]; s.z = states.mag[2];
switch (states.magStatus) { switch (states.magStatus) {
case MAGSTATUS_OK: case MAGSTATUS_OK:
s.Source = MAGSTATE_SOURCE_OK; s.Source = MAGSTATE_SOURCE_ONBOARD;
break; break;
case MAGSTATUS_AUX: case MAGSTATUS_AUX:
s.Source = MAGSTATE_SOURCE_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) : SixPointCalibrationModel::SixPointCalibrationModel(QObject *parent) :
QObject(parent), QObject(parent),
calibratingMag(false), calibratingMag(false),
externalMagAvailable(false), calibratingAuxMag(false),
calibratingAccel(false), calibratingAccel(false),
calibrationStepsMag(), calibrationStepsMag(),
calibrationStepsAccelOnly(), calibrationStepsAccelOnly(),
@ -334,7 +334,7 @@ void SixPointCalibrationModel::getSample(UAVObject *obj)
aux_mag_accum_x.append(auxMagData.x); aux_mag_accum_x.append(auxMagData.x);
aux_mag_accum_y.append(auxMagData.y); aux_mag_accum_y.append(auxMagData.y);
aux_mag_accum_z.append(auxMagData.z); aux_mag_accum_z.append(auxMagData.z);
externalMagAvailable = true; calibratingAuxMag = true;
#ifndef FITTING_USING_CONTINOUS_ACQUISITION #ifndef FITTING_USING_CONTINOUS_ACQUISITION
aux_mag_fit_x.append(auxMagData.x); aux_mag_fit_x.append(auxMagData.x);
aux_mag_fit_y.append(auxMagData.y); 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_x.append(auxMagData.x);
aux_mag_fit_y.append(auxMagData.y); aux_mag_fit_y.append(auxMagData.y);
aux_mag_fit_z.append(auxMagData.z); aux_mag_fit_z.append(auxMagData.z);
externalMagAvailable = true; calibratingAuxMag = true;
} }
} }
} }
@ -463,10 +463,10 @@ void SixPointCalibrationModel::compute()
qDebug() << "-----------------------------------"; qDebug() << "-----------------------------------";
qDebug() << "Onboard Mag"; qDebug() << "Onboard Mag";
CalcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias); calcCalibration(mag_fit_x, mag_fit_y, mag_fit_z, Be_length, revoCalibrationData.mag_transform, revoCalibrationData.mag_bias);
if (externalMagAvailable) { if (calibratingAuxMag) {
qDebug() << "Aux Mag"; 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 // 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_X]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]); good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Y]);
good_calibration &= !IS_NAN(revoCalibrationData.mag_bias[RevoCalibration::MAG_BIAS_Z]); 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_R0C0]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]); good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C1]);
good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]); good_calibration &= !IS_NAN(auxCalibrationData.mag_transform[RevoCalibration::MAG_TRANSFORM_R0C2]);
@ -537,7 +537,7 @@ void SixPointCalibrationModel::compute()
position = -1; 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(); int vectSize = x.count();
Eigen::VectorXf samples_x(vectSize); Eigen::VectorXf samples_x(vectSize);
@ -589,7 +589,7 @@ void SixPointCalibrationModel::save()
} }
revoCalibration->setData(revoCalibrationData); revoCalibration->setData(revoCalibrationData);
if (externalMagAvailable) { if (calibratingAuxMag) {
AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData(); AuxMagSettings::DataFields auxCalibrationData = auxMagSettings->getData();
// Note that Revo/AuxMag MAG_TRANSFORM_RxCx are interchangeable, an assertion at initialization enforces the structs are equal // 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++) { 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; } Result;
bool calibratingMag; bool calibratingMag;
bool externalMagAvailable; bool calibratingAuxMag;
bool calibratingAccel; bool calibratingAccel;
QList<CalibrationStep> calibrationStepsMag; QList<CalibrationStep> calibrationStepsMag;
@ -125,15 +125,17 @@ public:
QList<double> accel_accum_x; QList<double> accel_accum_x;
QList<double> accel_accum_y; QList<double> accel_accum_y;
QList<double> accel_accum_z; QList<double> accel_accum_z;
QList<double> mag_accum_x; QList<double> mag_accum_x;
QList<double> mag_accum_y; QList<double> mag_accum_y;
QList<double> mag_accum_z; 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_x;
QList<float> mag_fit_y; QList<float> mag_fit_y;
QList<float> mag_fit_z; 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_x;
QList<float> aux_mag_fit_y; QList<float> aux_mag_fit_y;
QList<float> aux_mag_fit_z; QList<float> aux_mag_fit_z;
@ -152,7 +154,7 @@ public:
void compute(); void compute();
void showHelp(QString image); void showHelp(QString image);
UAVObjectManager *getObjectManager(); 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="x" units="mGa" type="float" elements="1"/>
<field name="y" 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="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"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/> <telemetryflight acked="false" updatemode="periodic" period="1000"/>