From ce1e6e140a80a0fce18325fce63fa99a46c5171c Mon Sep 17 00:00:00 2001 From: tstellanova Date: Mon, 19 Aug 2013 21:45:52 -0700 Subject: [PATCH] Support persist-params-after-send Add persistRcValuesButt to allow mappings to be persisted outside the calibration mode; cleanup const params in param dataq model; add some debug for mapping problems --- src/uas/QGCUASParamManager.cc | 9 +- src/uas/QGCUASParamManager.h | 10 +- src/uas/UASParameterCommsMgr.cc | 26 +- src/uas/UASParameterCommsMgr.h | 3 +- src/uas/UASParameterDataModel.cc | 10 +- src/uas/UASParameterDataModel.h | 10 +- src/ui/QGCPX4VehicleConfig.cc | 247 +++++++++--------- src/ui/QGCPX4VehicleConfig.h | 10 +- src/ui/QGCPX4VehicleConfig.ui | 21 +- src/ui/QGCVehicleConfig.cc | 30 +-- .../px4_configuration/QGCPX4AirframeConfig.cc | 7 +- 11 files changed, 203 insertions(+), 180 deletions(-) diff --git a/src/uas/QGCUASParamManager.cc b/src/uas/QGCUASParamManager.cc index aa4bd8174b..788a227c29 100644 --- a/src/uas/QGCUASParamManager.cc +++ b/src/uas/QGCUASParamManager.cc @@ -111,12 +111,15 @@ void QGCUASParamManager::setParameter(int compId, QString paramName, QVariant va paramDataModel.updatePendingParamWithValue(compId,paramName,value); } -void QGCUASParamManager::sendPendingParameters() +void QGCUASParamManager::sendPendingParameters(bool persistAfterSend) { - paramCommsMgr->sendPendingParameters(); + paramCommsMgr->sendPendingParameters(persistAfterSend); } -void QGCUASParamManager::setPendingParam(int compId, QString& paramName, const QVariant& value) + + + +void QGCUASParamManager::setPendingParam(int compId, const QString& paramName, const QVariant& value) { paramDataModel.updatePendingParamWithValue(compId,paramName,value); } diff --git a/src/uas/QGCUASParamManager.h b/src/uas/QGCUASParamManager.h index c0cf6cd21a..6866b2b951 100644 --- a/src/uas/QGCUASParamManager.h +++ b/src/uas/QGCUASParamManager.h @@ -84,8 +84,11 @@ public slots: /** @brief Send one parameter to the MAV: changes value in transient memory of MAV */ virtual void setParameter(int component, QString parameterName, QVariant value); - /** @brief Send all pending parameters to the MAV, for storage in transient (RAM) memory */ - virtual void sendPendingParameters(); + /** @brief Send all pending parameters to the MAV, for storage in transient (RAM) memory + * @param persistAfterSend If true, all parameters will be written to persistent storage as well + */ + virtual void sendPendingParameters(bool persistAfterSend = false); + /** @brief Request list of parameters from MAV */ virtual void requestParameterList(); @@ -93,7 +96,8 @@ public slots: /** @brief Request a list of params onboard the MAV if the onboard param list we have is empty */ virtual void requestParameterListIfEmpty(); - virtual void setPendingParam(int componentId, QString& key, const QVariant& value); + /** @brief queue a pending parameter for sending to the MAV */ + virtual void setPendingParam(int componentId, const QString& key, const QVariant& value); /** @brief remove all params from the pending list */ virtual void clearAllPendingParams(); diff --git a/src/uas/UASParameterCommsMgr.cc b/src/uas/UASParameterCommsMgr.cc index a5ed632ca4..90b2f1c1d5 100644 --- a/src/uas/UASParameterCommsMgr.cc +++ b/src/uas/UASParameterCommsMgr.cc @@ -78,9 +78,6 @@ void UASParameterCommsMgr::requestParameterList() return; } - //TODO check: no need to cause datamodel to forget params here? -// paramDataModel->forgetAllOnboardParameters(); - if (!transmissionListMode) { // Clear transmission state receivedParamsList.clear(); @@ -216,11 +213,15 @@ void UASParameterCommsMgr::resendReadWriteRequests() } if ((0 == requestedWriteCount) && (0 == requestedReadCount) ) { - qDebug() << __FILE__ << __LINE__ << "NO re-read or rewrite requests??"; + qDebug() << __FILE__ << __LINE__ << "No pending re-read or rewrite requests"; if (!transmissionListMode) { setRetransmissionGuardEnabled(false); transmissionActive = false; } + if (persistParamsAfterSend) { + writeParamsToPersistentStorage(); + persistParamsAfterSend = false; + } } else { //restart the timer now that we've sent @@ -427,6 +428,8 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para { Q_UNUSED(uas); //this object is assigned to one UAS only + qDebug() << "compId" << compId << "receivedParameterUpdate:" << paramName; + //notify the data model that we have an updated param paramDataModel->handleParamUpdate(compId,paramName,value); @@ -474,6 +477,10 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para // Mark this parameter as received in write ACK list QMap* compMissWritePackets = missingWriteAckPackets.value(compId); + if (!compMissWritePackets) { + //we sometimes send a write request on compId 0 and get a response on a nonzero compId eg 50 + compMissWritePackets = missingWriteAckPackets.value(0); + } if (compMissWritePackets && compMissWritePackets->contains(paramName)) { justWritten = true; if (compMissWritePackets->value(paramName) != value) { @@ -563,12 +570,15 @@ void UASParameterCommsMgr::writeParamsToPersistentStorage() { if (mav) { mav->writeParametersToStorage(); //TODO track timeout, retransmit etc? + persistParamsAfterSend = false; //done } } -void UASParameterCommsMgr::sendPendingParameters() +void UASParameterCommsMgr::sendPendingParameters(bool copyToPersistent) { + persistParamsAfterSend |= copyToPersistent; + // Iterate through all components, through all pending parameters and send them to UAS int parametersSent = 0; QMap*>* changedValues = paramDataModel->getAllPendingParams(); @@ -590,7 +600,11 @@ void UASParameterCommsMgr::sendPendingParameters() // Change transmission status if necessary if (parametersSent == 0) { setParameterStatusMsg(tr("No transmission: No changed values."),ParamCommsStatusLevel_Warning); - } else { + if (persistParamsAfterSend) { + writeParamsToPersistentStorage(); + } + } + else { setParameterStatusMsg(tr("Transmitting %1 parameters.").arg(parametersSent)); // Set timeouts if (transmissionActive) { diff --git a/src/uas/UASParameterCommsMgr.h b/src/uas/UASParameterCommsMgr.h index 1320d7187e..958fa42114 100644 --- a/src/uas/UASParameterCommsMgr.h +++ b/src/uas/UASParameterCommsMgr.h @@ -64,7 +64,7 @@ signals: public slots: /** @brief Iterate through all components, through all pending parameters and send them to UAS */ - virtual void sendPendingParameters(); + virtual void sendPendingParameters(bool copyToPersistent = false); /** @brief Write the current onboard parameters from transient RAM into persistent storage, e.g. EEPROM or harddisk */ virtual void writeParamsToPersistentStorage(); @@ -99,6 +99,7 @@ protected: bool transmissionListMode; ///< Currently requesting list QMap transmissionListSizeKnown; ///< List size initialized? bool transmissionActive; ///< Missing packets, working on list? + bool persistParamsAfterSend; ///< Copy all parameters to persistent storage after sending quint64 transmissionTimeout; ///< Timeout QTimer retransmissionTimer; ///< Timer handling parameter retransmission quint64 lastTimerReset; ///< Last time the guard timer was reset, to prevent premature firing diff --git a/src/uas/UASParameterDataModel.cc b/src/uas/UASParameterDataModel.cc index b3aaed28ef..da1a6b9878 100644 --- a/src/uas/UASParameterDataModel.cc +++ b/src/uas/UASParameterDataModel.cc @@ -45,7 +45,7 @@ int UASParameterDataModel::countOnboardParams() } -bool UASParameterDataModel::updatePendingParamWithValue(int compId, QString& key, const QVariant& value) +bool UASParameterDataModel::updatePendingParamWithValue(int compId, const QString& key, const QVariant& value) { bool pending = true; //ensure we have this component in our onboard and pending lists already @@ -76,7 +76,7 @@ bool UASParameterDataModel::isParamChangePending(int compId, const QString& key) return ((NULL != pendingParms) && pendingParms->contains(key)); } -void UASParameterDataModel::setPendingParam(int compId, QString& key, const QVariant &value) +void UASParameterDataModel::setPendingParam(int compId, const QString& key, const QVariant &value) { //ensure we have a placeholder map for this component addComponent(compId); @@ -101,7 +101,7 @@ void UASParameterDataModel::removePendingParam(int compId, const QString& key) } } -void UASParameterDataModel::setOnboardParam(int compId, QString& key, const QVariant& value) +void UASParameterDataModel::setOnboardParam(int compId, const QString &key, const QVariant& value) { //ensure we have a placeholder map for this component addComponent(compId); @@ -109,7 +109,7 @@ void UASParameterDataModel::setOnboardParam(int compId, QString& key, const QVa params->insert(key,value); } -void UASParameterDataModel::setOnboardParamWithType(int compId, QString& key, QVariant& value) +void UASParameterDataModel::setOnboardParamWithType(int compId, const QString& key, const QVariant &value) { switch ((int)value.type()) @@ -155,7 +155,7 @@ void UASParameterDataModel::addComponent(int compId) } -void UASParameterDataModel::handleParamUpdate(int compId, QString& paramName, QVariant& value) +void UASParameterDataModel::handleParamUpdate(int compId, const QString ¶mName, const QVariant &value) { //verify that the value requested by the user matches the set value //if it doesn't match, leave the pending parameter in the pending list! diff --git a/src/uas/UASParameterDataModel.h b/src/uas/UASParameterDataModel.h index bcff40ab8c..c7c1dd057d 100644 --- a/src/uas/UASParameterDataModel.h +++ b/src/uas/UASParameterDataModel.h @@ -42,7 +42,7 @@ public: virtual QList getComponentForOnboardParam(const QString& parameter) const; /** @brief Save the onboard parameter with a the type specified in the QVariant as fixed */ - virtual void setOnboardParamWithType(int componentId, QString& key, QVariant& value); + virtual void setOnboardParamWithType(int componentId, const QString &key, const QVariant& value); /** @brief clears every parameter for every loaded component */ virtual void forgetAllOnboardParams(); @@ -52,8 +52,8 @@ public: /** @brief add this parameter to pending list iff it has changed from onboard value * @return true if the parameter is now pending */ - virtual bool updatePendingParamWithValue(int componentId, QString& key, const QVariant &value); - virtual void handleParamUpdate(int componentId, QString& key, QVariant& value); + virtual bool updatePendingParamWithValue(int componentId, const QString &key, const QVariant &value); + virtual void handleParamUpdate(int componentId, const QString& key, const QVariant& value); virtual bool getOnboardParamValue(int componentId, const QString& key, QVariant& value) const; virtual bool isParamChangePending(int componentId,const QString& key); @@ -89,10 +89,10 @@ public: protected: /** @brief set the confirmed value of a parameter in the onboard params list */ - virtual void setOnboardParam(int componentId, QString& key, const QVariant& value); + virtual void setOnboardParam(int componentId, const QString& key, const QVariant& value); /** @brief Write a new pending parameter value that may be eventually sent to the UAS */ - virtual void setPendingParam(int componentId, QString& key, const QVariant& value); + virtual void setPendingParam(int componentId, const QString &key, const QVariant& value); /** @brief remove a parameter from the pending list */ virtual void removePendingParam(int compId, const QString &key); diff --git a/src/ui/QGCPX4VehicleConfig.cc b/src/ui/QGCPX4VehicleConfig.cc index c7c335ca10..ef7fa279ac 100644 --- a/src/ui/QGCPX4VehicleConfig.cc +++ b/src/ui/QGCPX4VehicleConfig.cc @@ -16,7 +16,7 @@ #include "QGCPX4VehicleConfig.h" #include "QGC.h" -#include "QGCPendingParamWidget.h" +//#include "QGCPendingParamWidget.h" #include "QGCToolWidget.h" #include "UASManager.h" #include "UASParameterCommsMgr.h" @@ -38,6 +38,7 @@ QGCPX4VehicleConfig::QGCPX4VehicleConfig(QWidget *parent) : QWidget(parent), mav(NULL), chanCount(0), + channelWanted(-1), rcRoll(0.0f), rcPitch(0.0f), rcYaw(0.0f), @@ -47,7 +48,6 @@ QGCPX4VehicleConfig::QGCPX4VehicleConfig(QWidget *parent) : rcAux2(0.0f), rcAux3(0.0f), dataModelChanged(true), - channelWanted(-1), rc_mode(RC_MODE_NONE), calibrationEnabled(false), px4AirframeConfig(NULL), @@ -160,13 +160,15 @@ QGCPX4VehicleConfig::QGCPX4VehicleConfig(QWidget *parent) : connect(ui->subButton, SIGNAL(clicked()), this, SLOT(identifySubModeChannel())); connect(ui->aux1Button, SIGNAL(clicked()), this, SLOT(identifyAux1Channel())); connect(ui->aux2Button, SIGNAL(clicked()), this, SLOT(identifyAux2Channel())); + connect(ui->persistRcValuesButt,SIGNAL(clicked()), this, SLOT(writeCalibrationRC())); + //set rc values to defaults for (unsigned int i = 0; i < chanMax; i++) { rcValue[i] = UINT16_MAX; rcMapping[i] = i; - channelWantedList[i] = UINT16_MAX; - rcMin[i] = 1000; - rcMax[i] = 2000; + channelWantedList[i] = (float)UINT16_MAX;//TODO need to clean these up! + rcMin[i] = 1000.0f; + rcMax[i] = 2000.0f; } updateTimer.setInterval(150); @@ -211,11 +213,9 @@ void QGCPX4VehicleConfig::identifyChannelMapping(int aert_index) return; int oldmapping = rcMapping[aert_index]; - channelWanted = aert_index; - for (unsigned i = 0; i < sizeof(channelWantedList) / sizeof(channelWantedList[0]); i++) - { + for (unsigned i = 0; i < chanMax; i++) { if (i >= chanCount) { channelWantedList[i] = 0; } else { @@ -227,8 +227,7 @@ void QGCPX4VehicleConfig::identifyChannelMapping(int aert_index) msgBox.setInformativeText(tr("Please move stick, switch or potentiometer for the %1 channel\n all the way up/down or left/right.").arg(channelNames[channelWanted])); msgBox.setStandardButtons(QMessageBox::Ok | QMessageBox::Cancel); msgBox.setDefaultButton(QMessageBox::Cancel); - if (QMessageBox::Cancel == msgBox.exec()) - { + if (QMessageBox::Cancel == msgBox.exec()) { channelWanted = -1; rcMapping[aert_index] = oldmapping; return; @@ -264,18 +263,24 @@ void QGCPX4VehicleConfig::toggleCalibrationRC(bool enabled) void QGCPX4VehicleConfig::setTrimPositions() { + int rollMap = rcMapping[0]; + int pitchMap = rcMapping[1]; + int yawMap = rcMapping[2]; + int throttleMap = rcMapping[3]; + int modeSwMap = rcMapping[4]; + int aux1Map = rcMapping[5]; + int aux2Map = rcMapping[6]; + int aux3Map = rcMapping[7]; + // Set trim to min if stick is close to min - if (abs(rcValue[rcMapping[3]] - rcMin[rcMapping[3]]) < 100) - { - rcTrim[rcMapping[3]] = rcMin[rcMapping[3]]; // throttle + if (abs(rcValue[throttleMap] - rcMin[throttleMap]) < 100) { + rcTrim[throttleMap] = rcMin[throttleMap]; // throttle } // Set trim to max if stick is close to max - else if (abs(rcValue[rcMapping[3]] - rcMax[rcMapping[3]]) < 100) - { - rcTrim[rcMapping[3]] = rcMax[rcMapping[3]]; // throttle + else if (abs(rcValue[throttleMap] - rcMax[throttleMap]) < 100) { + rcTrim[throttleMap] = rcMax[throttleMap]; // throttle } - else - { + else { // Reject QMessageBox msgBox; msgBox.setText(tr("Throttle Stick Trim Position Invalid")); @@ -286,14 +291,14 @@ void QGCPX4VehicleConfig::setTrimPositions() } // Set trim for roll, pitch, yaw, throttle - rcTrim[rcMapping[0]] = rcValue[rcMapping[0]]; // roll - rcTrim[rcMapping[1]] = rcValue[rcMapping[1]]; // pitch - rcTrim[rcMapping[2]] = rcValue[rcMapping[2]]; // yaw - - rcTrim[rcMapping[4]] = ((rcMax[rcMapping[4]] - rcMin[rcMapping[4]]) / 2.0f) + rcMin[rcMapping[4]]; // mode sw - rcTrim[rcMapping[5]] = ((rcMax[rcMapping[5]] - rcMin[rcMapping[5]]) / 2.0f) + rcMin[rcMapping[5]]; // aux 1 - rcTrim[rcMapping[6]] = ((rcMax[rcMapping[6]] - rcMin[rcMapping[6]]) / 2.0f) + rcMin[rcMapping[6]]; // aux 2 - rcTrim[rcMapping[7]] = ((rcMax[rcMapping[7]] - rcMin[rcMapping[7]]) / 2.0f) + rcMin[rcMapping[7]]; // aux 3 + rcTrim[rollMap] = rcValue[rollMap]; // roll + rcTrim[pitchMap] = rcValue[pitchMap]; // pitch + rcTrim[yawMap] = rcValue[yawMap]; // yaw + + rcTrim[modeSwMap] = ((rcMax[modeSwMap] - rcMin[modeSwMap]) / 2.0f) + rcMin[modeSwMap]; // mode sw + rcTrim[aux1Map] = ((rcMax[aux1Map] - rcMin[aux1Map]) / 2.0f) + rcMin[aux1Map]; // aux 1 + rcTrim[aux2Map] = ((rcMax[aux2Map] - rcMin[aux2Map]) / 2.0f) + rcMin[aux2Map]; // aux 2 + rcTrim[aux3Map] = ((rcMax[aux3Map] - rcMin[aux3Map]) / 2.0f) + rcMin[aux3Map]; // aux 3 } void QGCPX4VehicleConfig::detectChannelInversion() @@ -341,15 +346,12 @@ void QGCPX4VehicleConfig::stopCalibrationRC() ui->radio7Widget->hideMinMax(); ui->radio8Widget->hideMinMax(); - for (int i=0;i 1350) - { + for (unsigned int i = 0; i < chanCount; i++) { + if (rcMin[i] > 1350) { rcMin[i] = 1000; } - if (rcMax[i] < 1650) - { + if (rcMax[i] < 1650) { rcMax[i] = 2000; } } @@ -362,8 +364,7 @@ void QGCPX4VehicleConfig::stopCalibrationRC() statusstr += "Normal values are around 1100 to 1900, with disconnected channels reading 1000, 1500, 2000\n\n"; statusstr += "Channel\tMin\tCenter\tMax\n"; statusstr += "--------------------\n"; - for (int i=0;isetParameter(0, minTpl.arg(i+1), rcMin[i]); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, trimTpl.arg(i+1), rcTrim[i]); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, maxTpl.arg(i+1), rcMax[i]); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, revTpl.arg(i+1), (rcRev[i]) ? -1.0f : 1.0f); - QGC::SLEEP::usleep(50000); + paramMgr->setPendingParam(0, minTpl.arg(i+1), rcMin[i]); + paramMgr->setPendingParam(0, trimTpl.arg(i+1), rcTrim[i]); + paramMgr->setPendingParam(0, maxTpl.arg(i+1), rcMax[i]); + paramMgr->setPendingParam(0, revTpl.arg(i+1), (rcRev[i]) ? -1.0f : 1.0f); } // Write mappings - mav->setParameter(0, "RC_MAP_ROLL", (int32_t)(rcMapping[0]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_PITCH", (int32_t)(rcMapping[1]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_YAW", (int32_t)(rcMapping[2]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[3]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_MODE_SW", (int32_t)(rcMapping[4]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_AUX1", (int32_t)(rcMapping[5]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_AUX2", (int32_t)(rcMapping[6]+1)); - QGC::SLEEP::usleep(50000); - mav->setParameter(0, "RC_MAP_AUX3", (int32_t)(rcMapping[7]+1)); - QGC::SLEEP::usleep(50000); - - mav->getParamManager()->copyVolatileParamsToPersistent(); + paramMgr->setPendingParam(0, "RC_MAP_ROLL", (int32_t)(rcMapping[0]+1)); + paramMgr->setPendingParam(0, "RC_MAP_PITCH", (int32_t)(rcMapping[1]+1)); + paramMgr->setPendingParam(0, "RC_MAP_YAW", (int32_t)(rcMapping[2]+1)); + paramMgr->setPendingParam(0, "RC_MAP_THROTTLE", (int32_t)(rcMapping[3]+1)); + paramMgr->setPendingParam(0, "RC_MAP_MODE_SW", (int32_t)(rcMapping[4]+1)); + paramMgr->setPendingParam(0, "RC_MAP_AUX1", (int32_t)(rcMapping[5]+1)); + paramMgr->setPendingParam(0, "RC_MAP_AUX2", (int32_t)(rcMapping[6]+1)); + paramMgr->setPendingParam(0, "RC_MAP_AUX3", (int32_t)(rcMapping[7]+1)); + + //let the param mgr manage sending all the pending RC_foo updates and persisting after + paramMgr->sendPendingParameters(true); + } void QGCPX4VehicleConfig::requestCalibrationRC() @@ -1122,13 +1111,12 @@ void QGCPX4VehicleConfig::writeParameters() { updateStatus(tr("Writing all onboard parameters.")); writeCalibrationRC(); - mav->writeParametersToStorage(); } -void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float val) +void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float fval) { // Check if index and values are sane - if (chan < 0 || static_cast(chan) >= chanMax || val < 500 || val > 2500) + if (chan < 0 || static_cast(chan) >= chanMax || fval < 500.0f || fval > 2500.0f) return; if (chan + 1 > (int)chanCount) { @@ -1136,27 +1124,35 @@ void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float val) } // Raw value - rcValue[chan] = val; + int ival = (int)fval; + int delta = abs(ival - rcValue[chan]); + if (delta < 3) { + //ignore tiny jitter values + return; + } + else { + qDebug() << "chan" << chan << " delta:" << delta; + rcValue[chan] = ival; + } + // Update calibration data if (calibrationEnabled) { - if (val < rcMin[chan]) { - rcMin[chan] = val; + if (fval < rcMin[chan]) { + rcMin[chan] = fval; } - if (val > rcMax[chan]) { - rcMax[chan] = val; + if (fval > rcMax[chan]) { + rcMax[chan] = fval; } } if (channelWanted >= 0) { // If the first channel moved considerably, pick it - if (fabsf(channelWantedList[chan] - val) > 300) - { + if (fabsf(channelWantedList[chan] - fval) > 300.0f) { rcMapping[channelWanted] = chan; updateInvertedCheckboxes(chan); int chanFound = channelWanted; - channelWanted = -1; // Confirm found channel @@ -1169,12 +1165,9 @@ void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float val) } // Find correct mapped channel - for (int i = 0; i < chanCount; i++) - { - if (chan == rcMapping[i]) - { - - rcMappedValue[i] = (rcRev[chan]) ? rcMax[chan] - (val - rcMin[chan]) : val; + for (unsigned int i = 0; i < chanCount; i++) { + if (chan == rcMapping[i]) { + rcMappedValue[i] = (rcRev[chan]) ? rcMax[chan] - (fval - rcMin[chan]) : fval; // Copy min / max rcMappedMin[i] = rcMin[chan]; @@ -1184,12 +1177,12 @@ void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float val) // Normalized value float normalized; - - if (val >= rcTrim[chan]) { - normalized = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); + float chanTrim = rcTrim[chan]; + if (fval >= rcTrim[chan]) { + normalized = (fval - chanTrim)/(rcMax[chan] - chanTrim); } else { - normalized = -(rcTrim[chan] - val)/(rcTrim[chan] - rcMin[chan]); + normalized = -(chanTrim - fval)/(chanTrim - rcMin[chan]); } // Bound @@ -1209,7 +1202,8 @@ void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float val) else if (chan == rcMapping[3]) { if (rcRev[chan]) { rcThrottle = 1.0f + normalized; - } else { + } + else { rcThrottle = normalized; } @@ -1230,7 +1224,7 @@ void QGCPX4VehicleConfig::remoteControlChannelRawChanged(int chan, float val) dataModelChanged = true; - //qDebug() << "RC CHAN:" << chan << "PPM:" << val << "NORMALIZED:" << normalized; + qDebug() << "RC CHAN:" << chan << "PPM:" << fval << "NORMALIZED:" << normalized; } void QGCPX4VehicleConfig::updateInvertedCheckboxes(int index) @@ -1505,10 +1499,8 @@ void QGCPX4VehicleConfig::setRCType(int type) void QGCPX4VehicleConfig::checktimeOuts() { - if (rcTypeUpdateRequested > 0) - { - if (QGC::groundTimeMilliseconds() - rcTypeUpdateRequested > rcTypeTimeout) - { + if (rcTypeUpdateRequested > 0) { + if (QGC::groundTimeMilliseconds() - rcTypeUpdateRequested > rcTypeTimeout) { updateError(tr("Setting remote control timed out - is the system connected?")); } } @@ -1528,42 +1520,49 @@ void QGCPX4VehicleConfig::updateRcWidgetValues() ui->radio8Widget->setValueAndRange(rcMappedValue[7],rcMin[7],rcMax[7]); } -void QGCPX4VehicleConfig::updateView() +void QGCPX4VehicleConfig::updateRcChanLabels() { - if (dataModelChanged) { - dataModelChanged = false; - - updateRcWidgetValues(); - - //update the channel labels - ui->rollChanLabel->setText(QString("%1").arg(rcRoll, 5, 'f', 2, QChar(' '))); - ui->pitchChanLabel->setText(QString("%1").arg(rcPitch, 5, 'f', 2, QChar(' '))); - ui->yawChanLabel->setText(QString("%1").arg(rcYaw, 5, 'f', 2, QChar(' '))); - ui->throttleChanLabel->setText(QString("%1").arg(rcThrottle, 5, 'f', 2, QChar(' '))); + ui->rollChanLabel->setText(labelForRcValue(rcRoll)); + ui->pitchChanLabel->setText(labelForRcValue(rcPitch)); + ui->yawChanLabel->setText(labelForRcValue(rcYaw)); + ui->throttleChanLabel->setText(labelForRcValue(rcThrottle)); + + QString blankLabel = tr("---"); + if (rcValue[rcMapping[4] != UINT16_MAX]) { + ui->modeChanLabel->setText(labelForRcValue(rcMode)); + } + else { + ui->modeChanLabel->setText(blankLabel); + } - if (rcValue[rcMapping[4] != UINT16_MAX]) { - ui->modeChanLabel->setText(QString("%1").arg(rcMode, 5, 'f', 2, QChar(' '))); - } else { - ui->modeChanLabel->setText(tr("---")); - } + if (rcValue[rcMapping[5]] != UINT16_MAX) { + ui->aux1ChanLabel->setText(labelForRcValue(rcAux1)); + } + else { + ui->aux1ChanLabel->setText(blankLabel); + } - if (rcValue[rcMapping[5]] != UINT16_MAX) { - ui->aux1ChanLabel->setText(QString("%1").arg(rcAux1, 5, 'f', 2, QChar(' '))); - } else { - ui->aux1ChanLabel->setText(tr("---")); - } + if (rcValue[rcMapping[6]] != UINT16_MAX) { + ui->aux2ChanLabel->setText(labelForRcValue(rcAux2)); + } + else { + ui->aux2ChanLabel->setText(blankLabel); + } - if (rcValue[rcMapping[6]] != UINT16_MAX) { - ui->aux2ChanLabel->setText(QString("%1").arg(rcAux2, 5, 'f', 2, QChar(' '))); - } else { - ui->aux2ChanLabel->setText(tr("---")); - } + if (rcValue[rcMapping[7]] != UINT16_MAX) { + ui->aux3ChanLabel->setText(labelForRcValue(rcAux3)); + } + else { + ui->aux3ChanLabel->setText(blankLabel); + } +} - if (rcValue[rcMapping[7]] != UINT16_MAX) { - ui->aux3ChanLabel->setText(QString("%1").arg(rcAux3, 5, 'f', 2, QChar(' '))); - } else { - ui->aux3ChanLabel->setText(tr("---")); - } +void QGCPX4VehicleConfig::updateView() +{ + if (dataModelChanged) { + dataModelChanged = false; + updateRcWidgetValues(); + updateRcChanLabels(); } } diff --git a/src/ui/QGCPX4VehicleConfig.h b/src/ui/QGCPX4VehicleConfig.h index f9727d8da7..ff4eb0d35c 100644 --- a/src/ui/QGCPX4VehicleConfig.h +++ b/src/ui/QGCPX4VehicleConfig.h @@ -66,7 +66,7 @@ public slots: void setRCModeIndex(int newRcMode); /** Render the data updated */ void updateView(); - void updateRcWidgetValues(); + void handleRcParameterChange(QString parameterName, QVariant value); @@ -205,6 +205,14 @@ protected slots: void checktimeOuts(); /** Update checkbox status */ void updateInvertedCheckboxes(int index); + /** Update the displayed values */ + void updateRcWidgetValues(); + /** update the channel labels */ + void updateRcChanLabels(); + + QString labelForRcValue(float val) { + return QString("%1").arg(val, 5, 'f', 2, QChar(' ')); + } protected: bool doneLoadingConfig; diff --git a/src/ui/QGCPX4VehicleConfig.ui b/src/ui/QGCPX4VehicleConfig.ui index 4322884b09..66f46c185e 100644 --- a/src/ui/QGCPX4VehicleConfig.ui +++ b/src/ui/QGCPX4VehicleConfig.ui @@ -154,7 +154,7 @@ Config - 0 + 5 @@ -746,6 +746,13 @@ Config + + + + Persist RC Mapping and Calibration + + + @@ -874,8 +881,8 @@ Config 0 0 - 98 - 28 + 16 + 16 @@ -911,8 +918,8 @@ Config 0 0 - 98 - 28 + 16 + 16 @@ -1003,8 +1010,8 @@ Config 0 0 - 98 - 28 + 597 + 569 diff --git a/src/ui/QGCVehicleConfig.cc b/src/ui/QGCVehicleConfig.cc index 171c83ce80..919eec4b4f 100644 --- a/src/ui/QGCVehicleConfig.cc +++ b/src/ui/QGCVehicleConfig.cc @@ -1020,12 +1020,10 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) // Normalized value float normalized; - if (val >= rcTrim[chan]) - { + if (val >= rcTrim[chan]) { normalized = (val - rcTrim[chan])/(rcMax[chan] - rcTrim[chan]); } - else - { + else { normalized = -(rcTrim[chan] - val)/(rcTrim[chan] - rcMin[chan]); } @@ -1034,22 +1032,18 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) // Invert normalized = (rcRev[chan]) ? -1.0f*normalized : normalized; - if (chan == rcMapping[0]) - { + if (chan == rcMapping[0]) { // ROLL rcRoll = normalized; } - if (chan == rcMapping[1]) - { + if (chan == rcMapping[1]) { // PITCH rcPitch = normalized; } - if (chan == rcMapping[2]) - { + if (chan == rcMapping[2]) { rcYaw = normalized; } - if (chan == rcMapping[3]) - { + if (chan == rcMapping[3]) { // THROTTLE if (rcRev[chan]) { rcThrottle = 1.0f + normalized; @@ -1059,23 +1053,19 @@ void QGCVehicleConfig::remoteControlChannelRawChanged(int chan, float val) rcThrottle = qBound(0.0f, rcThrottle, 1.0f); } - if (chan == rcMapping[4]) - { + if (chan == rcMapping[4]) { // MODE SWITCH rcMode = normalized; } - if (chan == rcMapping[5]) - { + if (chan == rcMapping[5]) { // AUX1 rcAux1 = normalized; } - if (chan == rcMapping[6]) - { + if (chan == rcMapping[6]) { // AUX2 rcAux2 = normalized; } - if (chan == rcMapping[7]) - { + if (chan == rcMapping[7]) { // AUX3 rcAux3 = normalized; } diff --git a/src/ui/px4_configuration/QGCPX4AirframeConfig.cc b/src/ui/px4_configuration/QGCPX4AirframeConfig.cc index f9f84436ac..7077e7d064 100644 --- a/src/ui/px4_configuration/QGCPX4AirframeConfig.cc +++ b/src/ui/px4_configuration/QGCPX4AirframeConfig.cc @@ -161,11 +161,8 @@ void QGCPX4AirframeConfig::applyAndReboot() //mav->getParamManager()->setParameter(components.first(), "SYS_AUTOSTART", (qint32)selectedId); // Send pending params - mav->getParamManager()->sendPendingParameters(); - QGC::SLEEP::msleep(300); - // Store parameters - mav->getParamManager()->copyVolatileParamsToPersistent(); - QGC::SLEEP::msleep(500); + mav->getParamManager()->sendPendingParameters(true); + // Reboot mav->executeCommand(MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN, 1, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0); } -- GitLab