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

Updates to config gadget for new INS objects

Browse Source 
Make scope gadget check if object exists before using it.  Avoids segfaults
when scoped objects disappear.
This commit is contained in:
James Cotton 2011-09-01 03:32:11 -05:00
parent 612d3336b4
commit 3b20df22d1
4 changed files with 96 additions and 97 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/Modules/AHRSComms/ahrs_comms.c
View File

@ -119,7 +119,7 @@ static void ahrscommsTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING); AlarmsSet(SYSTEMALARMS_ALARM_AHRSCOMMS, SYSTEMALARMS_ALARM_WARNING);
} }
InsStatusData sData; InsStatusData sData;
InsStatusGet(&sData); // InsStatusGet(&sData);
sData.LinkRunning = stat.linkOk; sData.LinkRunning = stat.linkOk;
sData.AhrsKickstarts = stat.remote.kickStarts; sData.AhrsKickstarts = stat.remote.kickStarts;
@ -130,7 +130,7 @@ static void ahrscommsTask(void *parameters)
sData.OpRetries = stat.local.retries; sData.OpRetries = stat.local.retries;
sData.OpInvalidPackets = stat.local.invalidPacket; sData.OpInvalidPackets = stat.local.invalidPacket;
InsStatusSet(&sData); // InsStatusSet(&sData);
/* Wait for the next update interval */ /* Wait for the next update interval */
vTaskDelayUntil(&lastSysTime, 1 / portTICK_RATE_MS); vTaskDelayUntil(&lastSysTime, 1 / portTICK_RATE_MS);

178
ground/openpilotgcs/src/plugins/config/configahrswidget.cpp
View File

@ -39,6 +39,9 @@
#include <Eigen/align-function.h> #include <Eigen/align-function.h>
#include <QDesktopServices> #include <QDesktopServices>
#include <QUrl> #include <QUrl>
#include <inssettings.h>
#include <attituderaw.h>
#include <homelocation.h>
#include "assertions.h" #include "assertions.h"
#include "calibration.h" #include "calibration.h"
@ -202,25 +205,27 @@ ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
position = -1; position = -1;
// Fill the dropdown menus: // Fill the dropdown menus:
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings"))); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVObjectField *field = obj->getField(QString("Algorithm")); Q_ASSERT(insSettings);
UAVObjectField *field = insSettings->getField(QString("Algorithm"));
Q_ASSERT(field);
m_ahrs->algorithm->addItems(field->getOptions()); m_ahrs->algorithm->addItems(field->getOptions());
// Register for Home Location state changes // Register for Home Location state changes
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation"))); HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this , SLOT(enableHomeLocSave(UAVObject*))); Q_ASSERT(homeLocation);
connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this , SLOT(enableHomeLocSave(UAVObject*)));
// Don't enable multi-point calibration until HomeLocation is set. // Don't enable multi-point calibration until HomeLocation is set.
m_ahrs->sixPointsStart->setEnabled(obj->getField("Set")->getValue().toBool()); HomeLocation::DataFields homeLocationData = homeLocation->getData();
m_ahrs->sixPointsStart->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE);
// Connect the signals // Connect the signals
connect(m_ahrs->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(launchAHRSCalibration())); connect(m_ahrs->ahrsCalibStart, SIGNAL(clicked()), this, SLOT(launchAHRSCalibration()));
connect(m_ahrs->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration())); connect(m_ahrs->accelBiasStart, SIGNAL(clicked()), this, SLOT(launchAccelBiasCalibration()));
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings"))); connect(homeLocation, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues())); connect(insSettings, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
obj = getObjectManager()->getObject(QString("HomeLocation"));
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(refreshValues()));
connect(m_ahrs->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM())); connect(m_ahrs->ahrsSettingsSaveRAM, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveRAM()));
connect(m_ahrs->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD())); connect(m_ahrs->ahrsSettingsSaveSD, SIGNAL(clicked()), this, SLOT(ahrsSettingsSaveSD()));
@ -278,35 +283,29 @@ void ConfigAHRSWidget::launchAccelBiasCalibration()
m_ahrs->accelBiasStart->setEnabled(false); m_ahrs->accelBiasStart->setEnabled(false);
m_ahrs->accelBiasProgress->setValue(0); m_ahrs->accelBiasProgress->setValue(0);
// Setup the AHRS to give us the right data at the right rate: InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings"))); Q_ASSERT(insSettings);
UAVObjectField* field = obj->getField(QString("BiasCorrectedRaw")); InsSettings::DataFields insSettingsData = insSettings->getData();
field->setValue("FALSE"); insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_FALSE;
obj->updated(); insSettings->setData(insSettingsData);
insSettings->updated();
accel_accum_x.clear(); accel_accum_x.clear();
accel_accum_y.clear(); accel_accum_y.clear();
accel_accum_z.clear(); accel_accum_z.clear();
// UAVDataObject* ahrsCalib = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration")));
// ahrsCalib->getField("accel_bias")->setDouble(0,0);
// ahrsCalib->getField("accel_bias")->setDouble(0,1);
// ahrsCalib->getField("accel_bias")->setDouble(0,2);
// ahrsCalib->updated();
/* Need to get as many AttitudeRaw updates as possible */ /* Need to get as many AttitudeRaw updates as possible */
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw"))); AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager());
initialMdata = obj->getMetadata(); Q_ASSERT(attitudeRaw);
initialMdata = attitudeRaw->getMetadata();
UAVObject::Metadata mdata = initialMdata; UAVObject::Metadata mdata = initialMdata;
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC; mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mdata.flightTelemetryUpdatePeriod = 100; mdata.flightTelemetryUpdatePeriod = 100;
obj->setMetadata(mdata); attitudeRaw->setMetadata(mdata);
// Now connect to the attituderaw updates, gather for 100 samples // Now connect to the attituderaw updates, gather for 100 samples
collectingData = true; collectingData = true;
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw"))); connect(attitudeRaw, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelBiasattitudeRawUpdated(UAVObject*)));
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelBiasattitudeRawUpdated(UAVObject*)));
} }
/** /**
@ -314,47 +313,42 @@ void ConfigAHRSWidget::launchAccelBiasCalibration()
*/ */
void ConfigAHRSWidget::accelBiasattitudeRawUpdated(UAVObject *obj) void ConfigAHRSWidget::accelBiasattitudeRawUpdated(UAVObject *obj)
{ {
// TODO: THis one gets replaced with the multipoint calibratino below. Q_UNUSED(obj);
UAVObjectField *accel_field = obj->getField(QString("accels"));
Q_ASSERT(accel_field != 0); AttitudeRaw * attitudeRaw = AttitudeRaw::GetInstance(getObjectManager());
Q_ASSERT(attitudeRaw);
AttitudeRaw::DataFields attitudeRawData = attitudeRaw->getData();
// This is necessary to prevent a race condition on disconnect signal and another update // This is necessary to prevent a race condition on disconnect signal and another update
if (collectingData == true) { if (collectingData == true) {
accel_accum_x.append(accel_field->getValue(0).toDouble()); accel_accum_x.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_X]);
accel_accum_y.append(accel_field->getValue(1).toDouble()); accel_accum_y.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Y]);
accel_accum_z.append(accel_field->getValue(2).toDouble()); accel_accum_z.append(attitudeRawData.accels[InsSettings::ACCEL_BIAS_Z]);
} }
m_ahrs->accelBiasProgress->setValue(m_ahrs->accelBiasProgress->value()+1); m_ahrs->accelBiasProgress->setValue(m_ahrs->accelBiasProgress->value()+1);
if(accel_accum_x.size() >= 100 && collectingData == true) { if(accel_accum_x.size() >= 100 && collectingData == true) {
collectingData = false; collectingData = false;
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*))); disconnect(attitudeRaw,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(accelBiasattitudeRawUpdated(UAVObject*)));
m_ahrs->accelBiasStart->setEnabled(true); m_ahrs->accelBiasStart->setEnabled(true);
UAVDataObject* ahrsCalib = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVObjectField* field = ahrsCalib->getField("accel_bias"); Q_ASSERT(insSettings);
double xBias = field->getDouble(0)- listMean(accel_accum_x); InsSettings::DataFields insSettingsData = insSettings->getData();
double yBias = field->getDouble(1) - listMean(accel_accum_y); insSettingsData.BiasCorrectedRaw = InsSettings::BIASCORRECTEDRAW_TRUE;
double zBias = -9.81 + field->getDouble(2) - listMean(accel_accum_z);
field->setDouble(xBias,0); insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_X] -= listMean(accel_accum_x);
field->setDouble(yBias,1); insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_Y] -= listMean(accel_accum_y);
field->setDouble(zBias,2); insSettingsData.accel_bias[InsSettings::ACCEL_BIAS_Z] -= 9.81 + listMean(accel_accum_z);
ahrsCalib->updated(); insSettings->setData(insSettingsData);
insSettings->updated();
getObjectManager()->getObject(QString("AttitudeRaw"))->setMetadata(initialMdata); attitudeRaw->setMetadata(initialMdata);
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
field = obj->getField(QString("BiasCorrectedRaw"));
field->setValue("TRUE");
obj->updated();
saveAHRSCalibration(); saveAHRSCalibration();
} }
} }
@ -1114,37 +1108,33 @@ void ConfigAHRSWidget::displayPlane(QString elementID)
*/ */
void ConfigAHRSWidget::drawVariancesGraph() void ConfigAHRSWidget::drawVariancesGraph()
{ {
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSCalibration"))); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
// Now update size of all the graphs Q_ASSERT(insSettings);
// I have not found a way to do this elegantly... InsSettings::DataFields insSettingsData = insSettings->getData();
UAVObjectField *field = obj->getField(QString("accel_var"));
// The expected range is from 1E-6 to 1E-1 // The expected range is from 1E-6 to 1E-1
double steps = 6; // 6 bars on the graph double steps = 6; // 6 bars on the graph
float accel_x_var = -1/steps*(1+steps+log10(field->getValue(0).toFloat())); float accel_x_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_X]));
accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false); accel_x->setTransform(QTransform::fromScale(1,accel_x_var),false);
float accel_y_var = -1/steps*(1+steps+log10(field->getValue(1).toFloat())); float accel_y_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_Y]));
accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false); accel_y->setTransform(QTransform::fromScale(1,accel_y_var),false);
float accel_z_var = -1/steps*(1+steps+log10(field->getValue(2).toFloat())); float accel_z_var = -1/steps*(1+steps+log10(insSettingsData.accel_var[InsSettings::ACCEL_VAR_Z]));
accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false); accel_z->setTransform(QTransform::fromScale(1,accel_z_var),false);
field = obj->getField(QString("gyro_var")); float gyro_x_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_X]));
float gyro_x_var = -1/steps*(1+steps+log10(field->getValue(0).toFloat()));
gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false); gyro_x->setTransform(QTransform::fromScale(1,gyro_x_var),false);
float gyro_y_var = -1/steps*(1+steps+log10(field->getValue(1).toFloat())); float gyro_y_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_Y]));
gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false); gyro_y->setTransform(QTransform::fromScale(1,gyro_y_var),false);
float gyro_z_var = -1/steps*(1+steps+log10(field->getValue(2).toFloat())); float gyro_z_var = -1/steps*(1+steps+log10(insSettingsData.gyro_var[InsSettings::GYRO_VAR_Z]));
gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false); gyro_z->setTransform(QTransform::fromScale(1,gyro_z_var),false);
// Scale by 1e-3 because mag vars are much higher. // Scale by 1e-3 because mag vars are much higher.
// TODO: Really? This is the scale factor from mG to T float mag_x_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_X]));
field = obj->getField(QString("mag_var"));
float mag_x_var = -1/steps*(1+steps+log10(1e-3*field->getValue(0).toFloat()));
mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false); mag_x->setTransform(QTransform::fromScale(1,mag_x_var),false);
float mag_y_var = -1/steps*(1+steps+log10(1e-3*field->getValue(1).toFloat())); float mag_y_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_Y]));
mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false); mag_y->setTransform(QTransform::fromScale(1,mag_y_var),false);
float mag_z_var = -1/steps*(1+steps+log10(1e-3*field->getValue(2).toFloat())); float mag_z_var = -1/steps*(1+steps+log10(1e-3*insSettingsData.mag_var[InsSettings::MAG_VAR_Z]));
mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false); mag_z->setTransform(QTransform::fromScale(1,mag_z_var),false);
} }
/** /**
@ -1152,17 +1142,16 @@ void ConfigAHRSWidget::drawVariancesGraph()
*/ */
void ConfigAHRSWidget::refreshValues() void ConfigAHRSWidget::refreshValues()
{ {
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVObject *obj = getObjectManager()->getObject(QString("AHRSSettings")); Q_ASSERT(insSettings);
UAVObjectField *field = obj->getField(QString("Algorithm")); InsSettings::DataFields insSettingsData = insSettings->getData();
if (field) m_ahrs->algorithm->setCurrentIndex(insSettingsData.Algorithm);
m_ahrs->algorithm->setCurrentIndex(m_ahrs->algorithm->findText(field->getValue().toString()));
drawVariancesGraph(); drawVariancesGraph();
obj = getObjectManager()->getObject(QString("HomeLocation")); HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
field = obj->getField(QString("Set")); Q_ASSERT(homeLocation);
if (field) HomeLocation::DataFields homeLocationData = homeLocation->getData();
m_ahrs->homeLocationSet->setEnabled(field->getValue().toBool()); m_ahrs->homeLocationSet->setEnabled(homeLocationData.Set == HomeLocation::SET_TRUE);
m_ahrs->ahrsCalibStart->setEnabled(true); m_ahrs->ahrsCalibStart->setEnabled(true);
m_ahrs->sixPointsStart->setEnabled(true); m_ahrs->sixPointsStart->setEnabled(true);
@ -1170,7 +1159,6 @@ void ConfigAHRSWidget::refreshValues()
m_ahrs->startDriftCalib->setEnabled(true); m_ahrs->startDriftCalib->setEnabled(true);
m_ahrs->calibInstructions->setText(QString("Press \"Start\" above to calibrate.")); m_ahrs->calibInstructions->setText(QString("Press \"Start\" above to calibrate."));
} }
/** /**
@ -1192,18 +1180,20 @@ void ConfigAHRSWidget::enableHomeLocSave(UAVObject * obj)
*/ */
void ConfigAHRSWidget::ahrsSettingsSaveRAM() void ConfigAHRSWidget::ahrsSettingsSaveRAM()
{ {
UAVDataObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings"))); InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
UAVObjectField *field = obj->getField(QString("Algorithm")); Q_ASSERT(insSettings);
field->setValue(m_ahrs->algorithm->currentText()); InsSettings::DataFields insSettingsData = insSettings->getData();
obj->updated(); insSettingsData.Algorithm = m_ahrs->algorithm->currentIndex();
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation"))); insSettings->setData(insSettingsData);
field = obj->getField(QString("Set")); insSettings->updated();
if (m_ahrs->homeLocationSet->isChecked())
field->setValue(QString("TRUE"));
else
field->setValue(QString("FALSE"));
obj->updated();
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
HomeLocation::DataFields homeLocationData = homeLocation->getData();
homeLocationData.Set = m_ahrs->homeLocationSet->isChecked() ?
HomeLocation::SET_TRUE : HomeLocation::SET_FALSE;
homeLocation->setData(homeLocationData);
homeLocation->updated();
} }
/** /**
@ -1212,16 +1202,18 @@ Save AHRS Settings and home location to SD
void ConfigAHRSWidget::ahrsSettingsSaveSD() void ConfigAHRSWidget::ahrsSettingsSaveSD()
{ {
ahrsSettingsSaveRAM(); ahrsSettingsSaveRAM();
UAVDataObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("HomeLocation")));
saveObjectToSD(obj);
obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AHRSSettings")));
saveObjectToSD(obj);
InsSettings * insSettings = InsSettings::GetInstance(getObjectManager());
Q_ASSERT(insSettings);
saveObjectToSD(insSettings);
HomeLocation * homeLocation = HomeLocation::GetInstance(getObjectManager());
Q_ASSERT(homeLocation);
saveObjectToSD(homeLocation);
} }
void ConfigAHRSWidget::openHelp() void ConfigAHRSWidget::openHelp()
{ {
QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/INS+Configuration", QUrl::StrictMode) ); QDesktopServices::openUrl( QUrl("http://wiki.openpilot.org/display/Doc/INS+Configuration", QUrl::StrictMode) );
} }

2
ground/openpilotgcs/src/plugins/coreplugin/OpenPilotGCS.xml
View File

@ -2228,7 +2228,7 @@
<plotCurve0> <plotCurve0>
<color>4289374847</color> <color>4289374847</color>
<uavField>RunningTime</uavField> <uavField>RunningTime</uavField>
<uavObject>AhrsStatus</uavObject> <uavObject>InsStatus</uavObject>
<yMaximum>0</yMaximum> <yMaximum>0</yMaximum>
<yMinimum>0</yMinimum> <yMinimum>0</yMinimum>
<yScalePower>0</yScalePower> <yScalePower>0</yScalePower>

9
ground/openpilotgcs/src/plugins/scope/scopegadgetwidget.cpp
View File

@ -385,8 +385,15 @@ void ScopeGadgetWidget::addCurvePlot(QString uavObject, QString uavFieldSubField
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance(); ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>(); UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(objManager->getObject((plotData->uavObject))); UAVDataObject* obj = dynamic_cast<UAVDataObject*>(objManager->getObject((plotData->uavObject)));
if(!obj) {
qDebug() << "Object " << plotData->uavObject << " is missing";
return;
}
UAVObjectField* field = obj->getField(plotData->uavField); UAVObjectField* field = obj->getField(plotData->uavField);
if(!field) {
qDebug() << "Field " << plotData->uavField << " of object " << plotData->uavObject << " is missing";
return;
}
QString units = field->getUnits(); QString units = field->getUnits();
if(units == 0) if(units == 0)