/**
Enables and runs feed forward testing
*/
void ConfigVehicleTypeWidget::enableFFTest()
{
// Role:
// - Check if all three checkboxes are checked
// - Every other timer event: toggle engine from 45% to 55%
// - Every other time event: send FF settings to flight FW
if (m_aircraft->ffTestBox1->isChecked() && m_aircraft->ffTestBox2->isChecked()
&& m_aircraft->ffTestBox3->isChecked()) {
if (!ffTuningInProgress) {
// Initiate tuning:
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(
QString("ManualControlCommand")));
UAVObject::Metadata mdata = obj->getMetadata();
accInitialData = mdata;
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
obj->setMetadata(mdata);
}
// Depending on phase, either move actuator or send FF settings:
if (ffTuningPhase) {
// Send FF settings to the board
UAVDataObject *mixer = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(QString("MixerSettings")));
Q_ASSERT(mixer);
QPointer<VehicleConfig> vconfig = new VehicleConfig();
// Update feed forward settings
vconfig->setMixerValue(mixer, "FeedForward", m_aircraft->feedForwardSlider->value() / 100.0);
vconfig->setMixerValue(mixer, "AccelTime", m_aircraft->accelTime->value());
vconfig->setMixerValue(mixer, "DecelTime", m_aircraft->decelTime->value());
vconfig->setMixerValue(mixer, "MaxAccel", m_aircraft->maxAccelSlider->value());
mixer->updated();
} else {
// Toggle motor state
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(
QString("ManualControlCommand")));
double value = obj->getField("Throttle")->getDouble();
double target = (value < 0.5) ? 0.55 : 0.45;
obj->getField("Throttle")->setValue(target);
obj->updated();
}
ffTuningPhase = !ffTuningPhase;
ffTuningInProgress = true;
QTimer::singleShot(1000, this, SLOT(enableFFTest()));
} else {
// - If no: disarm timer, restore actuatorcommand metadata
// Disarm!
if (ffTuningInProgress) {
ffTuningInProgress = false;
UAVDataObject *obj = dynamic_cast<UAVDataObject *>(getObjectManager()->getObject(
QString("ManualControlCommand")));
UAVObject::Metadata mdata = obj->getMetadata();
mdata = accInitialData; // Restore metadata
obj->setMetadata(mdata);
}
}
}
void LevellingUtil::startMeasurement()
{
QMutexLocker locker(&m_measurementMutex);
m_isMeasuring = true;
// Reset variables
m_receivedAccelUpdates = 0;
m_accelerometerX = 0;
m_accelerometerY = 0;
m_accelerometerZ = 0;
m_receivedGyroUpdates = 0;
m_gyroX = 0;
m_gyroY = 0;
m_gyroZ = 0;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager * uavObjectManager = pm->getObject<UAVObjectManager>();
Q_ASSERT(uavObjectManager);
// 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);
// Set up to receive updates for accels
UAVDataObject *uavObject = Accels::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(accelMeasurementsUpdated(UAVObject*)));
// Set update period for accels
m_previousAccelMetaData = uavObject->getMetadata();
UAVObject::Metadata newMetaData = m_previousAccelMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
newMetaData.flightTelemetryUpdatePeriod = m_accelMeasurementRate;
uavObject->setMetadata(newMetaData);
// Set up to receive updates from gyros
uavObject = Gyros::GetInstance(uavObjectManager);
connect(uavObject, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(gyroMeasurementsUpdated(UAVObject*)));
// Set update period for gyros
m_previousGyroMetaData = uavObject->getMetadata();
newMetaData = m_previousGyroMetaData;
UAVObject::SetFlightTelemetryUpdateMode(newMetaData, UAVObject::UPDATEMODE_PERIODIC);
newMetaData.flightTelemetryUpdatePeriod = m_gyroMeasurementRate;
uavObject->setMetadata(newMetaData);
}
/*!
\brief Called when the gcs control is toggled and enabled or disables flight write access to manual control command
*/
void GCSControlGadgetWidget::toggleControl(int state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
UAVObject::Metadata mdata = obj->getMetadata();
if (state)
{
mccInitialData = mdata;
UAVObject::SetFlightAccess(mdata, UAVObject::ACCESS_READONLY);
UAVObject::SetFlightTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
UAVObject::SetGcsTelemetryAcked(mdata, false);
UAVObject::SetGcsTelemetryUpdateMode(mdata, UAVObject::UPDATEMODE_ONCHANGE);
mdata.gcsTelemetryUpdatePeriod = 100;
m_gcscontrol->checkBoxUDPControl->setEnabled(true);
}
else
{
mdata = mccInitialData;
toggleUDPControl(false);
m_gcscontrol->checkBoxUDPControl->setEnabled(false);
}
obj->setMetadata(mdata);
}
GCSControlGadgetWidget::GCSControlGadgetWidget(QWidget *parent) : QLabel(parent)
{
m_gcscontrol = new Ui_GCSControl();
m_gcscontrol->setupUi(this);
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("ManualControlCommand")) );
UAVObject::Metadata mdata = obj->getMetadata();
m_gcscontrol->checkBoxGcsControl->setChecked(UAVObject::GetFlightAccess(mdata) == UAVObject::ACCESS_READONLY);
// Set up the drop down box for the flightmode
UAVDataObject* flightStatus = dynamic_cast<UAVDataObject*>( objManager->getObject(QString("FlightStatus")) );
m_gcscontrol->comboBoxFlightMode->addItems(flightStatus->getField("FlightMode")->getOptions());
// Set up slots and signals for joysticks
connect(m_gcscontrol->widgetLeftStick,SIGNAL(positionClicked(double,double)),this,SLOT(leftStickClicked(double,double)));
connect(m_gcscontrol->widgetRightStick,SIGNAL(positionClicked(double,double)),this,SLOT(rightStickClicked(double,double)));
// Connect misc controls
connect(m_gcscontrol->checkBoxGcsControl, SIGNAL(stateChanged(int)), this, SLOT(toggleControl(int)));
connect(m_gcscontrol->checkBoxArmed, SIGNAL(stateChanged(int)), this, SLOT(toggleArmed(int)));
connect(m_gcscontrol->comboBoxFlightMode, SIGNAL(currentIndexChanged(int)), this, SLOT(selectFlightMode(int)));
connect(m_gcscontrol->checkBoxUDPControl, SIGNAL(stateChanged(int)),this,SLOT(toggleUDPControl(int))); //UDP control checkbox
// Connect object updated event from UAVObject to also update check boxes and dropdown
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(mccChanged(UAVObject*)));
leftX = 0;
leftY = 0;
rightX = 0;
rightY = 0;
// No point enabling OpenGL for the joysticks, and causes
// issues on some computers:
// m_gcscontrol->widgetLeftStick->enableOpenGL(true);
// m_gcscontrol->widgetRightStick->enableOpenGL(true);
}
/**
Toggles the channel testing mode by making the GCS take over
the ActuatorCommand objects
*/
void ConfigOutputWidget::runChannelTests(bool state)
{
// Confirm this is definitely what they want
if(state) {
QMessageBox mbox;
mbox.setText(QString(tr("This option will requires you to be in the armed state and will start your motors by the amount selected on the sliders. It is recommended to remove any blades from motors. Are you sure you want to do this?")));
mbox.setStandardButtons(QMessageBox::Yes | QMessageBox::No);
int retval = mbox.exec();
if(retval != QMessageBox::Yes) {
state = false;
qDebug() << "Cancelled";
m_config->channelOutTest->setChecked(false);
return;
}
}
qDebug() << "Running with state " << state;
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(objManager->getObject(QString("ActuatorCommand")));
UAVObject::Metadata mdata = obj->getMetadata();
if (state)
{
accInitialData = mdata;
mdata.flightAccess = UAVObject::ACCESS_READONLY;
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
mdata.gcsTelemetryAcked = false;
mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
mdata.gcsTelemetryUpdatePeriod = 100;
}
else
{
mdata = accInitialData; // Restore metadata
}
obj->setMetadata(mdata);
}
/**
Toggles the channel testing mode by making the GCS take over
the ActuatorCommand objects
*/
void ConfigServoWidget::runChannelTests(bool state)
{
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
UAVObjectManager *objManager = pm->getObject<UAVObjectManager>();
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(objManager->getObject(QString("ActuatorCommand")));
UAVObject::Metadata mdata = obj->getMetadata();
if (state)
{
accInitialData = mdata;
mdata.flightAccess = UAVObject::ACCESS_READONLY;
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
mdata.gcsTelemetryAcked = false;
mdata.gcsTelemetryUpdateMode = UAVObject::UPDATEMODE_ONCHANGE;
mdata.gcsTelemetryUpdatePeriod = 100;
}
else
{
mdata = accInitialData; // Restore metadata
}
obj->setMetadata(mdata);
}
bool VehicleConfigurationHelper::saveChangesToController(bool save)
{
qDebug() << "Saving modified objects to controller. " << m_modifiedObjects.count() << " objects in found.";
const int OUTER_TIMEOUT = 3000 * 20; // 10 seconds timeout for saving all objects
const int INNER_TIMEOUT = 2000; // 1 second timeout on every save attempt
ExtensionSystem::PluginManager *pm = ExtensionSystem::PluginManager::instance();
Q_ASSERT(pm);
UAVObjectUtilManager *utilMngr = pm->getObject<UAVObjectUtilManager>();
Q_ASSERT(utilMngr);
QTimer outerTimeoutTimer;
outerTimeoutTimer.setSingleShot(true);
QTimer innerTimeoutTimer;
innerTimeoutTimer.setSingleShot(true);
connect(utilMngr, SIGNAL(saveCompleted(int, bool)), this, SLOT(uAVOTransactionCompleted(int, bool)));
connect(&innerTimeoutTimer, SIGNAL(timeout()), &m_eventLoop, SLOT(quit()));
connect(&outerTimeoutTimer, SIGNAL(timeout()), this, SLOT(saveChangesTimeout()));
outerTimeoutTimer.start(OUTER_TIMEOUT);
for (int i = 0; i < m_modifiedObjects.count(); i++) {
QPair<UAVDataObject *, QString> *objPair = m_modifiedObjects.at(i);
m_transactionOK = false;
UAVDataObject *obj = objPair->first;
QString objDescription = objPair->second;
if (UAVObject::GetGcsAccess(obj->getMetadata()) != UAVObject::ACCESS_READONLY && obj->isSettings()) {
emit saveProgress(m_modifiedObjects.count() + 1, ++m_progress, objDescription);
m_currentTransactionObjectID = obj->getObjID();
connect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(uAVOTransactionCompleted(UAVObject *, bool)));
while (!m_transactionOK && !m_transactionTimeout) {
// Allow the transaction to take some time
innerTimeoutTimer.start(INNER_TIMEOUT);
// Set object updated
obj->updated();
if (!m_transactionOK) {
m_eventLoop.exec();
}
innerTimeoutTimer.stop();
}
disconnect(obj, SIGNAL(transactionCompleted(UAVObject *, bool)), this, SLOT(uAVOTransactionCompleted(UAVObject *, bool)));
if (m_transactionOK) {
qDebug() << "Object " << obj->getName() << " was successfully updated.";
if (save) {
m_transactionOK = false;
m_currentTransactionObjectID = obj->getObjID();
// Try to save until success or timeout
while (!m_transactionOK && !m_transactionTimeout) {
// Allow the transaction to take some time
innerTimeoutTimer.start(INNER_TIMEOUT);
// Persist object in controller
utilMngr->saveObjectToFlash(obj);
if (!m_transactionOK) {
m_eventLoop.exec();
}
innerTimeoutTimer.stop();
}
m_currentTransactionObjectID = -1;
}
}
if (!m_transactionOK) {
qDebug() << "Transaction timed out when trying to save: " << obj->getName();
} else {
qDebug() << "Object " << obj->getName() << " was successfully saved.";
}
} else {
/**
* Updates the slider positions and min/max values
*
*/
void ConfigServoWidget::updateChannels(UAVObject* controlCommand)
{
QString fieldName = QString("Connected");
UAVObjectField *field = controlCommand->getField(fieldName);
if (field->getValue().toBool())
m_config->RCInputConnected->setText("RC Receiver Connected");
else
m_config->RCInputConnected->setText("RC Receiver Not Connected");
if (m_config->doRCInputCalibration->isChecked())
{
if (firstUpdate)
{
// Increase the data rate from the board so that the sliders
// move faster
UAVObject::Metadata mdata = controlCommand->getMetadata();
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mccDataRate = mdata.flightTelemetryUpdatePeriod;
mdata.flightTelemetryUpdatePeriod = 150;
controlCommand->setMetadata(mdata);
// Also protect the user by setting all values to zero
// and making the ActuatorCommand object readonly
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("ActuatorCommand")));
mdata = obj->getMetadata();
mdata.flightAccess = UAVObject::ACCESS_READONLY;
obj->setMetadata(mdata);
UAVObjectField *field = obj->getField("Channel");
for (int i=0; i< field->getNumElements(); i++) {
field->setValue(0,i);
}
obj->updated();
}
field = controlCommand->getField(QString("Channel"));
for (int i = 0; i < 8; i++)
updateChannelInSlider(inSliders[i], inMinLabels[i], inMaxLabels[i], reversals[i], field->getValue(i).toInt());
firstUpdate = false;
}
else
{
if (!firstUpdate)
{
// Restore original data rate from the board:
UAVObject::Metadata mdata = controlCommand->getMetadata();
mdata.flightTelemetryUpdateMode = UAVObject::UPDATEMODE_PERIODIC;
mdata.flightTelemetryUpdatePeriod = mccDataRate;
controlCommand->setMetadata(mdata);
UAVDataObject* obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("ActuatorCommand")));
mdata = obj->getMetadata();
mdata.flightAccess = UAVObject::ACCESS_READWRITE;
obj->setMetadata(mdata);
}
firstUpdate = true;
}
//Update the Flight mode channel slider
UAVObject* obj = getObjectManager()->getObject("ManualControlSettings");
// Find the channel currently assigned to flightmode
field = obj->getField("FlightMode");
int chIndex = field->getOptions().indexOf(field->getValue().toString());
if (chIndex < field->getOptions().length() - 1)
{
float valueScaled;
int chMin = inSliders[chIndex]->minimum();
int chMax = inSliders[chIndex]->maximum();
int chNeutral = inSliders[chIndex]->value();
int value = controlCommand->getField("Channel")->getValue(chIndex).toInt();
if ((chMax > chMin && value >= chNeutral) || (chMin > chMax && value <= chNeutral))
{
if (chMax != chNeutral)
valueScaled = (float)(value - chNeutral) / (float)(chMax - chNeutral);
else
valueScaled = 0;
}
else
{
if (chMin != chNeutral)
valueScaled = (float)(value - chNeutral) / (float)(chNeutral - chMin);
else
valueScaled = 0;
}
// Bound
if (valueScaled > 1.0) valueScaled = 1.0;
else
if (valueScaled < -1.0) valueScaled = -1.0;
m_config->fmsSlider->setValue(valueScaled * 100);
}
}
