QGCCameraIO.cc 13 KB
Newer Older
Gus Grubba's avatar
Gus Grubba committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
/*!
 * @file
 *   @brief Camera Controller
 *   @author Gus Grubba <mavlink@grubba.com>
 *
 */

#include "QGCCameraControl.h"
#include "QGCCameraIO.h"

QGC_LOGGING_CATEGORY(CameraIOLog, "CameraIOLog")
QGC_LOGGING_CATEGORY(CameraIOLogVerbose, "CameraIOLogVerbose")

//-----------------------------------------------------------------------------
QGCCameraParamIO::QGCCameraParamIO(QGCCameraControl *control, Fact* fact, Vehicle *vehicle)
    : QObject(control)
    , _control(control)
    , _fact(fact)
    , _vehicle(vehicle)
    , _sentRetries(0)
    , _requestRetries(0)
22
    , _done(false)
23
    , _updateOnSet(false)
24
    , _forceUIUpdate(false)
Gus Grubba's avatar
Gus Grubba committed
25
{
Gus Grubba's avatar
Gus Grubba committed
26
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
Gus Grubba's avatar
Gus Grubba committed
27
    _paramWriteTimer.setSingleShot(true);
28
    _paramWriteTimer.setInterval(3000);
Gus Grubba's avatar
Gus Grubba committed
29
    _paramRequestTimer.setSingleShot(true);
30
    _paramRequestTimer.setInterval(3500);
31
32
33
34
35
36
    if(_fact->writeOnly()) {
        //-- Write mode is always "done" as it won't ever read
        _done = true;
    } else {
        connect(&_paramRequestTimer, &QTimer::timeout, this, &QGCCameraParamIO::_paramRequestTimeout);
    }
Gus Grubba's avatar
Gus Grubba committed
37
38
    connect(&_paramWriteTimer,   &QTimer::timeout, this, &QGCCameraParamIO::_paramWriteTimeout);
    connect(_fact, &Fact::rawValueChanged, this, &QGCCameraParamIO::_factChanged);
Gus Grubba's avatar
Gus Grubba committed
39
    connect(_fact, &Fact::_containerRawValueChanged, this, &QGCCameraParamIO::_containerRawValueChanged);
Gus Grubba's avatar
Gus Grubba committed
40
41
42
43
44
    _pMavlink = qgcApp()->toolbox()->mavlinkProtocol();
    //-- TODO: Even though we don't use anything larger than 32-bit, this should
    //   probably be updated.
    switch (_fact->type()) {
        case FactMetaData::valueTypeUint8:
Gus Grubba's avatar
Gus Grubba committed
45
        case FactMetaData::valueTypeBool:
46
            _mavParamType = MAV_PARAM_EXT_TYPE_UINT8;
Gus Grubba's avatar
Gus Grubba committed
47
48
            break;
        case FactMetaData::valueTypeInt8:
49
            _mavParamType = MAV_PARAM_EXT_TYPE_INT8;
Gus Grubba's avatar
Gus Grubba committed
50
51
            break;
        case FactMetaData::valueTypeUint16:
52
            _mavParamType = MAV_PARAM_EXT_TYPE_UINT16;
Gus Grubba's avatar
Gus Grubba committed
53
54
            break;
        case FactMetaData::valueTypeInt16:
55
            _mavParamType = MAV_PARAM_EXT_TYPE_INT16;
Gus Grubba's avatar
Gus Grubba committed
56
57
            break;
        case FactMetaData::valueTypeUint32:
58
            _mavParamType = MAV_PARAM_EXT_TYPE_UINT32;
Gus Grubba's avatar
Gus Grubba committed
59
            break;
60
61
62
63
64
65
        case FactMetaData::valueTypeUint64:
            _mavParamType = MAV_PARAM_EXT_TYPE_UINT64;
            break;
        case FactMetaData::valueTypeInt64:
            _mavParamType = MAV_PARAM_EXT_TYPE_INT64;
            break;
Gus Grubba's avatar
Gus Grubba committed
66
        case FactMetaData::valueTypeFloat:
67
            _mavParamType = MAV_PARAM_EXT_TYPE_REAL32;
Gus Grubba's avatar
Gus Grubba committed
68
            break;
69
70
71
        case FactMetaData::valueTypeDouble:
            _mavParamType = MAV_PARAM_EXT_TYPE_REAL64;
            break;
Gus Grubba's avatar
Gus Grubba committed
72
73
            //-- String and custom are the same for now
        case FactMetaData::valueTypeString:
Gus Grubba's avatar
Gus Grubba committed
74
        case FactMetaData::valueTypeCustom:
75
            _mavParamType = MAV_PARAM_EXT_TYPE_CUSTOM;
Gus Grubba's avatar
Gus Grubba committed
76
            break;
Gus Grubba's avatar
Gus Grubba committed
77
        default:
Gus Grubba's avatar
Gus Grubba committed
78
            qWarning() << "Unsupported fact type" << _fact->type() << "for" << _fact->name();
Gus Grubba's avatar
Gus Grubba committed
79
80
            // Fall through
        case FactMetaData::valueTypeInt32:
81
            _mavParamType = MAV_PARAM_EXT_TYPE_INT32;
Gus Grubba's avatar
Gus Grubba committed
82
83
            break;
    }
Gus Grubba's avatar
Gus Grubba committed
84
85
86
87
88
89
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::setParamRequest()
{
90
91
92
93
94
    if(!_fact->writeOnly()) {
        _paramRequestReceived = false;
        _requestRetries = 0;
        _paramRequestTimer.start();
    }
Gus Grubba's avatar
Gus Grubba committed
95
96
97
98
99
100
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_factChanged(QVariant value)
{
101
102
103
104
105
    if(!_forceUIUpdate) {
        Q_UNUSED(value);
        qCDebug(CameraIOLog) << "UI Fact" << _fact->name() << "changed to" << value;
        _control->factChanged(_fact);
    }
Gus Grubba's avatar
Gus Grubba committed
106
107
}

Gus Grubba's avatar
Gus Grubba committed
108
109
110
111
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_containerRawValueChanged(const QVariant value)
{
112
113
    if(!_fact->readOnly()) {
        Q_UNUSED(value);
Gus Grubba's avatar
Gus Grubba committed
114
        qCDebug(CameraIOLog) << "Update Fact from camera" << _fact->name();
115
116
117
        _sentRetries = 0;
        _sendParameter();
    }
Gus Grubba's avatar
Gus Grubba committed
118
119
}

120
121
122
123
124
125
126
127
128
129
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::sendParameter(bool updateUI)
{
    qCDebug(CameraIOLog) << "Send Fact" << _fact->name();
    _sentRetries = 0;
    _updateOnSet = updateUI;
    _sendParameter();
}

Gus Grubba's avatar
Gus Grubba committed
130
131
132
133
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_sendParameter()
{
Gus Grubba's avatar
Gus Grubba committed
134
135
    mavlink_param_ext_set_t p;
    memset(&p, 0, sizeof(mavlink_param_ext_set_t));
Gus Grubba's avatar
Gus Grubba committed
136
137
    param_ext_union_t   union_value;
    mavlink_message_t   msg;
Gus Grubba's avatar
Gus Grubba committed
138
    FactMetaData::ValueType_t factType = _fact->type();
Gus Grubba's avatar
Gus Grubba committed
139
    p.param_type = _mavParamType;
Gus Grubba's avatar
Gus Grubba committed
140
141
    switch (factType) {
        case FactMetaData::valueTypeUint8:
Gus Grubba's avatar
Gus Grubba committed
142
        case FactMetaData::valueTypeBool:
Gus Grubba's avatar
Gus Grubba committed
143
144
145
146
147
148
149
150
151
152
153
154
155
156
            union_value.param_uint8 = (uint8_t)_fact->rawValue().toUInt();
            break;
        case FactMetaData::valueTypeInt8:
            union_value.param_int8 = (int8_t)_fact->rawValue().toInt();
            break;
        case FactMetaData::valueTypeUint16:
            union_value.param_uint16 = (uint16_t)_fact->rawValue().toUInt();
            break;
        case FactMetaData::valueTypeInt16:
            union_value.param_int16 = (int16_t)_fact->rawValue().toInt();
            break;
        case FactMetaData::valueTypeUint32:
            union_value.param_uint32 = (uint32_t)_fact->rawValue().toUInt();
            break;
157
158
159
160
161
162
        case FactMetaData::valueTypeInt64:
            union_value.param_int64 = (int64_t)_fact->rawValue().toLongLong();
            break;
        case FactMetaData::valueTypeUint64:
            union_value.param_uint64 = (uint64_t)_fact->rawValue().toULongLong();
            break;
Gus Grubba's avatar
Gus Grubba committed
163
164
165
        case FactMetaData::valueTypeFloat:
            union_value.param_float = _fact->rawValue().toFloat();
            break;
166
167
168
        case FactMetaData::valueTypeDouble:
            union_value.param_double = _fact->rawValue().toDouble();
            break;
Gus Grubba's avatar
Gus Grubba committed
169
170
            //-- String and custom are the same for now
        case FactMetaData::valueTypeString:
Gus Grubba's avatar
Gus Grubba committed
171
172
173
174
175
176
        case FactMetaData::valueTypeCustom:
            {
                QByteArray custom = _fact->rawValue().toByteArray();
                memcpy(union_value.bytes, custom.data(), std::max(custom.size(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN));
            }
            break;
Gus Grubba's avatar
Gus Grubba committed
177
        default:
Gus Grubba's avatar
Gus Grubba committed
178
            qCritical() << "Unsupported fact type" << factType << "for" << _fact->name();
Gus Grubba's avatar
Gus Grubba committed
179
180
181
182
183
            // fall through
        case FactMetaData::valueTypeInt32:
            union_value.param_int32 = (int32_t)_fact->rawValue().toInt();
            break;
    }
Gus Grubba's avatar
Gus Grubba committed
184
    memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
Gus Grubba's avatar
Gus Grubba committed
185
186
    p.target_system = (uint8_t)_vehicle->id();
    p.target_component = (uint8_t)_control->compID();
Gus Grubba's avatar
Gus Grubba committed
187
188
189
190
191
192
193
194
    strncpy(p.param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN);
    mavlink_msg_param_ext_set_encode_chan(
        _pMavlink->getSystemId(),
        _pMavlink->getComponentId(),
        _vehicle->priorityLink()->mavlinkChannel(),
        &msg,
        &p);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
Gus Grubba's avatar
Gus Grubba committed
195
196
197
198
199
200
201
202
203
    _paramWriteTimer.start();
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_paramWriteTimeout()
{
    if(++_sentRetries > 3) {
        qCWarning(CameraIOLog) << "No response for param set:" << _fact->name();
204
        _updateOnSet = false;
Gus Grubba's avatar
Gus Grubba committed
205
206
    } else {
        //-- Send it again
207
        qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries;
Gus Grubba's avatar
Gus Grubba committed
208
        _sendParameter();
Gus Grubba's avatar
Gus Grubba committed
209
210
211
212
213
214
215
216
217
218
        _paramWriteTimer.start();
    }
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack)
{
    _paramWriteTimer.stop();
    if(ack.param_result == PARAM_ACK_ACCEPTED) {
Gus Grubba's avatar
Gus Grubba committed
219
220
221
        QVariant val = _valueFromMessage(ack.param_value, ack.param_type);
        if(_fact->rawValue() != val) {
            _fact->_containerSetRawValue(val);
222
223
224
225
            if(_updateOnSet) {
                _updateOnSet = false;
                _control->factChanged(_fact);
            }
Gus Grubba's avatar
Gus Grubba committed
226
227
        }
    } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) {
Gus Grubba's avatar
Gus Grubba committed
228
229
230
231
232
        //-- Wait a bit longer for this one
        qCDebug(CameraIOLogVerbose) << "Param set in progress:" << _fact->name();
        _paramWriteTimer.start();
    } else {
        if(ack.param_result == PARAM_ACK_FAILED) {
Gus Grubba's avatar
Gus Grubba committed
233
            if(++_sentRetries < 3) {
234
235
236
237
238
                //-- Try again
                qCWarning(CameraIOLog) << "Param set failed:" << _fact->name() << _sentRetries;
                _paramWriteTimer.start();
            }
            return;
Gus Grubba's avatar
Gus Grubba committed
239
240
241
        } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) {
            qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name();
        }
Gus Grubba's avatar
Gus Grubba committed
242
243
244
        //-- If UI changed and value was not set, restore UI
        QVariant val = _valueFromMessage(ack.param_value, ack.param_type);
        if(_fact->rawValue() != val) {
245
246
247
            if(_control->validateParameter(_fact, val)) {
                _fact->_containerSetRawValue(val);
            }
Gus Grubba's avatar
Gus Grubba committed
248
        }
Gus Grubba's avatar
Gus Grubba committed
249
250
251
252
253
254
255
256
    }
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value)
{
    _paramRequestTimer.stop();
257
258
259
260
    QVariant newValue = _valueFromMessage(value.param_value, value.param_type);
    if(_control->incomingParameter(_fact, newValue)) {
        _fact->_containerSetRawValue(newValue);
    }
Gus Grubba's avatar
Gus Grubba committed
261
    _paramRequestReceived = true;
262
263
264
265
266
    if(_forceUIUpdate) {
        emit _fact->rawValueChanged(_fact->rawValue());
        emit _fact->valueChanged(_fact->rawValue());
        _forceUIUpdate = false;
    }
267
268
269
270
    if(!_done) {
        _done = true;
        _control->_paramDone();
    }
Gus Grubba's avatar
Gus Grubba committed
271
272
273
274
275
276
277
278
    qCDebug(CameraIOLog) << QString("handleParamValue() %1 %2").arg(_fact->name()).arg(_fact->rawValueString());
}

//-----------------------------------------------------------------------------
QVariant
QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type)
{
    QVariant var;
Gus Grubba's avatar
Gus Grubba committed
279
280
    param_ext_union_t u;
    memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
Gus Grubba's avatar
Gus Grubba committed
281
    switch (param_type) {
282
        case MAV_PARAM_EXT_TYPE_REAL32:
Gus Grubba's avatar
Gus Grubba committed
283
284
            var = QVariant(u.param_float);
            break;
285
        case MAV_PARAM_EXT_TYPE_UINT8:
Gus Grubba's avatar
Gus Grubba committed
286
287
            var = QVariant(u.param_uint8);
            break;
288
        case MAV_PARAM_EXT_TYPE_INT8:
Gus Grubba's avatar
Gus Grubba committed
289
290
            var = QVariant(u.param_int8);
            break;
291
        case MAV_PARAM_EXT_TYPE_UINT16:
Gus Grubba's avatar
Gus Grubba committed
292
293
            var = QVariant(u.param_uint16);
            break;
294
        case MAV_PARAM_EXT_TYPE_INT16:
Gus Grubba's avatar
Gus Grubba committed
295
296
            var = QVariant(u.param_int16);
            break;
297
        case MAV_PARAM_EXT_TYPE_UINT32:
Gus Grubba's avatar
Gus Grubba committed
298
299
            var = QVariant(u.param_uint32);
            break;
300
        case MAV_PARAM_EXT_TYPE_INT32:
Gus Grubba's avatar
Gus Grubba committed
301
302
            var = QVariant(u.param_int32);
            break;
303
        case MAV_PARAM_EXT_TYPE_UINT64:
Gus Grubba's avatar
Gus Grubba committed
304
            var = QVariant((qulonglong)u.param_uint64);
305
306
            break;
        case MAV_PARAM_EXT_TYPE_INT64:
Gus Grubba's avatar
Gus Grubba committed
307
            var = QVariant((qlonglong)u.param_int64);
308
            break;
309
        case MAV_PARAM_EXT_TYPE_CUSTOM:
Gus Grubba's avatar
Gus Grubba committed
310
311
            var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN));
            break;
Gus Grubba's avatar
Gus Grubba committed
312
313
314
315
316
317
318
319
320
321
322
323
324
        default:
            var = QVariant(0);
            qCritical() << "Invalid param_type used for camera setting:" << param_type;
    }
    return var;
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_paramRequestTimeout()
{
    if(++_requestRetries > 3) {
        qCWarning(CameraIOLog) << "No response for param request:" << _fact->name();
325
326
327
328
        if(!_done) {
            _done = true;
            _control->_paramDone();
        }
Gus Grubba's avatar
Gus Grubba committed
329
330
331
    } else {
        //-- Request it again
        qCDebug(CameraIOLog) << "Param request retry:" << _fact->name();
332
        paramRequest(false);
Gus Grubba's avatar
Gus Grubba committed
333
334
335
        _paramRequestTimer.start();
    }
}
336
337
338
339
340

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::paramRequest(bool reset)
{
341
342
343
344
345
346
347
348
    //-- If it's write only, we don't request it.
    if(_fact->writeOnly()) {
        if(!_done) {
            _done = true;
            _control->_paramDone();
        }
        return;
    }
349
350
    if(reset) {
        _requestRetries = 0;
351
        _forceUIUpdate  = true;
352
    }
353
    qCDebug(CameraIOLog) << "Request parameter:" << _fact->name();
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
    char param_id[MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN + 1];
    memset(param_id, 0, sizeof(param_id));
    strncpy(param_id, _fact->name().toStdString().c_str(), MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN);
    mavlink_message_t msg;
    mavlink_msg_param_ext_request_read_pack_chan(
        _pMavlink->getSystemId(),
        _pMavlink->getComponentId(),
        _vehicle->priorityLink()->mavlinkChannel(),
        &msg,
        _vehicle->id(),
        _control->compID(),
        param_id,
        -1);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
    _paramRequestTimer.start();
}