Commit f850094f authored by Lorenz Meier's avatar Lorenz Meier

Merge pull request #338 from tstellanova/config_debug

RC calibration and mapping debug
parents 4cfe93cc e98081fe
......@@ -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);
}
......
......@@ -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();
......
......@@ -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,7 +213,7 @@ 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;
......@@ -427,6 +424,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 +473,10 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para
// Mark this parameter as received in write ACK list
QMap<QString, QVariant>* 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) {
......@@ -498,6 +501,10 @@ void UASParameterCommsMgr::receivedParameterUpdate(int uas, int compId, int para
setParameterStatusMsg(tr("SUCCESS: Wrote %2 (#%1/%4): %3 [%5]").arg(paramId+1).arg(paramName).arg(value.toDouble()).arg(paramCount).arg(missWriteCount));
if (0 == missWriteCount) {
setParameterStatusMsg(tr("SUCCESS: WROTE ALL PARAMETERS"));
if (persistParamsAfterSend) {
writeParamsToPersistentStorage();
persistParamsAfterSend = false;
}
}
}
else {
......@@ -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<int, QMap<QString, QVariant>*>* 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) {
......
......@@ -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<int, bool> 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
......
......@@ -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 &paramName, 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!
......
......@@ -42,7 +42,7 @@ public:
virtual QList<int> 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);
......
This diff is collapsed.
......@@ -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;
......@@ -254,6 +262,7 @@ protected:
QGCPX4AirframeConfig* px4AirframeConfig;
DialogBare* firmwareDialog;
QMessageBox msgBox;
QPushButton* skipActionButton;
private:
Ui::QGCPX4VehicleConfig *ui;
......
......@@ -154,7 +154,7 @@ Config</string>
<item>
<widget class="QStackedWidget" name="stackedWidget">
<property name="currentIndex">
<number>0</number>
<number>5</number>
</property>
<widget class="QWidget" name="firmwareTab">
<layout class="QVBoxLayout" name="firmwareLayout">
......@@ -746,6 +746,13 @@ Config</string>
</item>
</layout>
</item>
<item>
<widget class="QPushButton" name="persistRcValuesButt">
<property name="text">
<string>Persist RC Mapping and Calibration</string>
</property>
</widget>
</item>
</layout>
</widget>
</item>
......@@ -874,8 +881,8 @@ Config</string>
<rect>
<x>0</x>
<y>0</y>
<width>98</width>
<height>28</height>
<width>16</width>
<height>16</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_4">
......@@ -911,8 +918,8 @@ Config</string>
<rect>
<x>0</x>
<y>0</y>
<width>98</width>
<height>28</height>
<width>16</width>
<height>16</height>
</rect>
</property>
<layout class="QHBoxLayout" name="horizontalLayout_5">
......@@ -1003,8 +1010,8 @@ Config</string>
<rect>
<x>0</x>
<y>0</y>
<width>98</width>
<height>28</height>
<width>597</width>
<height>569</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_7">
......
......@@ -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;
}
......
......@@ -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);
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment