ParameterList.cc 10.8 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
/*=====================================================================

QGroundControl Open Source Ground Control Station

(c) 2009, 2010 QGROUNDCONTROL PROJECT <http://www.qgroundcontrol.org>

This file is part of the QGROUNDCONTROL project

    QGROUNDCONTROL is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    QGROUNDCONTROL is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with QGROUNDCONTROL. If not, see <http://www.gnu.org/licenses/>.

======================================================================*/

/**
 * @file
 *   @brief Implementation of class OpalRT::ParameterList
 *   @author Bryan Godbolt <godbolt@ualberta.ca>
 */

30
31
32
33
#include "ParameterList.h"
using namespace OpalRT;

ParameterList::ParameterList()
34
    :params(new QMap<int, QMap<QGCParamID, Parameter> >),
35
36
     paramList(new QList<QList<Parameter*> >()),
     reqdServoParams(new QStringList())
37
38
{

39
40
41
    QDir settingsDir = QDir(qApp->applicationDirPath());
    if (settingsDir.dirName() == "bin")
        settingsDir.cdUp();
Jessica's avatar
Jessica committed
42
    settingsDir.cd("data");
43

Bryan Godbolt's avatar
Bryan Godbolt committed
44
45
46
47
    // Enforce a list of parameters which are necessary for flight
    reqdServoParams->append("AIL_RIGHT_IN");
    reqdServoParams->append("AIL_CENTER_IN");
    reqdServoParams->append("AIL_LEFT_IN");
48
49
50
    reqdServoParams->append("AIL_RIGHT_OUT");
    reqdServoParams->append("AIL_CENTER_OUT");
    reqdServoParams->append("AIL_LEFT_OUT");
Bryan Godbolt's avatar
Bryan Godbolt committed
51
52
53
    reqdServoParams->append("ELE_DOWN_IN");
    reqdServoParams->append("ELE_CENTER_IN");
    reqdServoParams->append("ELE_UP_IN");
54
55
56
    reqdServoParams->append("ELE_DOWN_OUT");
    reqdServoParams->append("ELE_CENTER_OUT");
    reqdServoParams->append("ELE_UP_OUT");
57
58
59
    reqdServoParams->append("RUD_LEFT_IN");
    reqdServoParams->append("RUD_CENTER_IN");
    reqdServoParams->append("RUD_RIGHT_IN");
Bryan Godbolt's avatar
Bryan Godbolt committed
60

61
    QString filename(settingsDir.path() + "/ParameterList.xml");
62
    if ((QFile::exists(filename)) && open(filename)) {
63
64
65
66
67
68
69
70
71

        /* Get a list of the available parameters from opal-rt */
        QMap<QString, unsigned short> *opalParams = new QMap<QString, unsigned short>;
        getParameterList(opalParams);

        /* Iterate over the parameters we want to use in qgc and populate their ids */
        QMap<int, QMap<QGCParamID, Parameter> >::iterator componentIter;
        QMap<QGCParamID, Parameter>::iterator paramIter;
        QString s;
72
        for (componentIter = params->begin(); componentIter != params->end(); ++componentIter) {
73
            paramList->append(QList<Parameter*>());
74
            for (paramIter = (*componentIter).begin(); paramIter != (*componentIter).end(); ++paramIter) {
75
76
                paramList->last().append(paramIter.operator ->());
                s = (*paramIter).getSimulinkPath() + (*paramIter).getSimulinkName();
77
                if (opalParams->contains(s)) {
78
79
                    (*paramIter).setOpalID(opalParams->value(s));
                    //                qDebug() << __FILE__ << " Line:" << __LINE__ << ": Successfully added " << s;
80
                } else {
Don Gagne's avatar
Don Gagne committed
81
                    qDebug() << __FILE__ << " Line:" << __LINE__ << ": " << s << " was not found in param list";
82
                }
83
84
            }
        }
85
        delete opalParams;
86
87
88
89
90
91
    }
}

ParameterList::~ParameterList()
{
    delete params;
92
    delete paramList;
93
94
}

95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
/**
  Get the list of parameters in the simulink model.  This function does not require
  any prior knowlege of the parameters.  It works by first calling OpalGetParameterList to
  get the number of paramters, then allocates the required amount of memory and then gets
  the paramter list using a second call to OpalGetParameterList.
  */
void ParameterList::getParameterList(QMap<QString, unsigned short> *opalParams)
{
    /* inputs */
    unsigned short allocatedParams=0;
    unsigned short allocatedPathLen=0;
    unsigned short allocatedNameLen=0;
    unsigned short allocatedVarLen=0;

    /* outputs */
    unsigned short numParams;
    unsigned short *idParam=NULL;
    unsigned short maxPathLen;
    char **paths=NULL;
    unsigned short maxNameLen;
    char **names=NULL;
    unsigned short maxVarLen;
    char **var=NULL;

    int returnValue;

    returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
122
123
124
125
                                       allocatedPathLen, &maxPathLen, paths,
                                       allocatedNameLen, &maxNameLen, names,
                                       allocatedVarLen, &maxVarLen, var);
    if (returnValue!=E2BIG) {
126
127
//        OpalRT::setLastErrorMsg();
        OpalRT::OpalErrorMsg::displayLastErrorMsg();
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
        return;
    }

    // allocate memory for parameter list

    idParam = new unsigned short[numParams];
    allocatedParams = numParams;

    paths = new char*[numParams];
    for (int i=0; i<numParams; i++)
        paths[i]=new char[maxPathLen];
    allocatedPathLen = maxPathLen;

    names = new char*[numParams];
    for (int i=0; i<numParams; i++)
        names[i] = new char[maxNameLen];
    allocatedNameLen = maxNameLen;

    var = new char*[numParams];
    for (int i=0; i<numParams; i++)
        var[i] = new char[maxVarLen];
    allocatedVarLen = maxVarLen;

    returnValue = OpalGetParameterList(allocatedParams, &numParams, idParam,
152
153
154
                                       allocatedPathLen, &maxPathLen, paths,
                                       allocatedNameLen, &maxNameLen, names,
                                       allocatedVarLen, &maxVarLen, var);
155

156
    if (returnValue != EOK) {
157
158
//        OpalRT::setLastErrorMsg();
        OpalRT::OpalErrorMsg::displayLastErrorMsg();
159
160
161
162
        return;
    }

    QString path, name;
163
    for (int i=0; i<numParams; ++i) {
164
165
166
167
168
169
170
171
172
        path.clear();
        path.append(paths[i]);
        name.clear();
        name.append(names[i]);
        if (path[path.size()-1] == '/')
            opalParams->insert(path+name, idParam[i]);
        else
            opalParams->insert(path+'/'+name, idParam[i]);
    }
173
//     Dump out the list of parameters
174
175
176
177
178
179
//    QMap<QString, unsigned short>::const_iterator paramPrint;
//    for (paramPrint = opalParams->begin(); paramPrint != opalParams->end(); ++paramPrint)
//        qDebug() << paramPrint.key();


}
180

181
182
183
184
185
186
187
int ParameterList::indexOf(const Parameter &p)
{
    // incase p is a copy of the actual parameter we want (i.e., addresses differ)
    Parameter *pPtr = &((*params)[p.getComponentID()][p.getParamID()]);

    QList<QList<Parameter*> >::const_iterator iter;
    int index = -1;
188
    for (iter = paramList->begin(); iter != paramList->end(); ++iter) {
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
        if ((index = (*iter).indexOf(pPtr)) != -1)
            return index;
    }
    return index;
}


ParameterList::const_iterator::const_iterator(QList<Parameter> paramList)
{
    this->paramList = QList<Parameter>(paramList);
    index = 0;
}

ParameterList::const_iterator::const_iterator(const const_iterator &other)
{
    paramList = QList<Parameter>(other.paramList);
    index = other.index;
}

ParameterList::const_iterator ParameterList::begin() const
{
    QList<QMap<QGCParamID, Parameter> > compList = params->values();
    QList<Parameter> paramList;
    QList<QMap<QGCParamID, Parameter> >::const_iterator compIter;
    for (compIter = compList.begin(); compIter != compList.end(); ++compIter)
        paramList.append((*compIter).values());
    return const_iterator(paramList);
}

ParameterList::const_iterator ParameterList::end() const
219
220
{
    const_iterator iter = begin();
221
222
223
224
225
226
227
228
229
230
    return iter+=iter.paramList.size();
}

int ParameterList::count()
{
    int count = 0;
    QList<QList<Parameter*> >::const_iterator iter;
    for (iter = paramList->begin(); iter != paramList->end(); ++iter)
        count += (*iter).count();
    return count;
231
}
232
233
234
235
236
237

/* Functions related to reading the xml config file */

bool ParameterList::open(QString filename)
{
    QFile paramFile(filename);
238
    if (!paramFile.exists()) {
239
240
241
242
        /// \todo open dialog box (maybe: that could also go in  comm config window)
        return false;
    }

243
    if (!paramFile.open(QIODevice::ReadOnly)) {
244
245
246
247
248
        return false;
    }

    read(&paramFile);

Bryan Godbolt's avatar
Bryan Godbolt committed
249
250
    paramFile.close();

251
252
253
254
255
256
257
258
259
260
261
262
    return true;
}

bool ParameterList::read(QIODevice *device)
{
    QDomDocument *paramConfig = new QDomDocument();

    QString errorStr;
    int errorLine;
    int errorColumn;

    if (!paramConfig->setContent(device, true, &errorStr, &errorLine,
263
                                 &errorColumn)) {
264
265
266
267
268
269
270
271
272
273
274
        qDebug() << "Error reading XML Parameter File on line: " << errorLine << errorStr;
        return false;
    }

    QDomElement root = paramConfig->documentElement();
    if (root.tagName() != "ParameterList") {
        qDebug() << __FILE__ << __LINE__ << "This is not a parameter list xml file";
        return false;
    }

    QDomElement child = root.firstChildElement("Block");
275
    while (!child.isNull()) {
276
277
278
279
        parseBlock(child);
        child = child.nextSiblingElement("Block");
    }

280
    if (!reqdServoParams->empty()) {
281
        qDebug() << __FILE__ << __LINE__ << "Missing the following required servo parameters";
282
        foreach(QString s, *reqdServoParams) {
283
284
285
286
            qDebug() << s;
        }
    }

287
288
289
290
291
292
293
294
295
296
297
298
    delete paramConfig;
    return true;
}

void ParameterList::parseBlock(const QDomElement &block)
{

    QDomNodeList paramList;
    QDomElement e;
    Parameter *p;
    SubsystemIds id;
    if (block.attribute("name") == "Navigation")
299
        id = OpalRT::NAV;
300
    else if (block.attribute("name") == "Controller")
301
        id = OpalRT::CONTROLLER;
302
303
304
305
306
    else if (block.attribute("name") == "ServoOutputs")
        id = OpalRT::SERVO_OUTPUTS;
    else if (block.attribute("name") == "ServoInputs")
        id = OpalRT::SERVO_INPUTS;

307
308
309
310
    paramList = block.elementsByTagName("Parameter");
    for (int i=0; i < paramList.size(); ++i) {
        e = paramList.item(i).toElement();
        if (e.hasAttribute("SimulinkPath") &&
311
                e.hasAttribute("SimulinkParameterName") &&
312
                e.hasAttribute("QGCParamID")) {
313

314
315
316
317
318
319
320
            p = new Parameter(e.attribute("SimulinkPath"),
                              e.attribute("SimulinkParameterName"),
                              static_cast<uint8_t>(id),
                              QGCParamID(e.attribute("QGCParamID")));
            (*params)[id].insert(p->getParamID(), *p);
            if (reqdServoParams->contains((QString)p->getParamID()))
                reqdServoParams->removeAt(reqdServoParams->indexOf((QString)p->getParamID()));
Bryan Godbolt's avatar
Bryan Godbolt committed
321

322
            delete p;
Bryan Godbolt's avatar
Bryan Godbolt committed
323
324


325
326
        } else {
            qDebug() << __FILE__ << ":" << __LINE__ << ": error in xml doc in block" << block.attribute("name");
327
        }
328
    }
329

330

Bryan Godbolt's avatar
Bryan Godbolt committed
331

332
}