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

#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)
25
{
26
    QQmlEngine::setObjectOwnership(this, QQmlEngine::CppOwnership);
27
    _paramWriteTimer.setSingleShot(true);
28
    _paramWriteTimer.setInterval(3000);
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);
    }
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);
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;
47 48
            break;
        case FactMetaData::valueTypeInt8:
49
            _mavParamType = MAV_PARAM_EXT_TYPE_INT8;
50 51
            break;
        case FactMetaData::valueTypeUint16:
52
            _mavParamType = MAV_PARAM_EXT_TYPE_UINT16;
53 54
            break;
        case FactMetaData::valueTypeInt16:
55
            _mavParamType = MAV_PARAM_EXT_TYPE_INT16;
56 57
            break;
        case FactMetaData::valueTypeUint32:
58
            _mavParamType = MAV_PARAM_EXT_TYPE_UINT32;
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;
66
        case FactMetaData::valueTypeFloat:
67
            _mavParamType = MAV_PARAM_EXT_TYPE_REAL32;
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:
74
        case FactMetaData::valueTypeCustom:
75
            _mavParamType = MAV_PARAM_EXT_TYPE_CUSTOM;
76
            break;
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
            //-- Fall Through (screw clang)
80
        case FactMetaData::valueTypeInt32:
81
            _mavParamType = MAV_PARAM_EXT_TYPE_INT32;
82 83
            break;
    }
84 85 86 87 88 89
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::setParamRequest()
{
90 91 92 93 94
    if(!_fact->writeOnly()) {
        _paramRequestReceived = false;
        _requestRetries = 0;
        _paramRequestTimer.start();
    }
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);
    }
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();
}

130 131 132 133
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_sendParameter()
{
134 135
    mavlink_param_ext_set_t p;
    memset(&p, 0, sizeof(mavlink_param_ext_set_t));
136 137
    param_ext_union_t   union_value;
    mavlink_message_t   msg;
138
    FactMetaData::ValueType_t factType = _fact->type();
139
    p.param_type = _mavParamType;
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
            union_value.param_uint8 = static_cast<uint8_t>(_fact->rawValue().toUInt());
144 145
            break;
        case FactMetaData::valueTypeInt8:
Gus Grubba's avatar
Gus Grubba committed
146
            union_value.param_int8 = static_cast<int8_t>(_fact->rawValue().toInt());
147 148
            break;
        case FactMetaData::valueTypeUint16:
Gus Grubba's avatar
Gus Grubba committed
149
            union_value.param_uint16 = static_cast<uint16_t>(_fact->rawValue().toUInt());
150 151
            break;
        case FactMetaData::valueTypeInt16:
Gus Grubba's avatar
Gus Grubba committed
152
            union_value.param_int16 = static_cast<int16_t>(_fact->rawValue().toInt());
153 154
            break;
        case FactMetaData::valueTypeUint32:
Gus Grubba's avatar
Gus Grubba committed
155
            union_value.param_uint32 = static_cast<uint32_t>(_fact->rawValue().toUInt());
156
            break;
157
        case FactMetaData::valueTypeInt64:
Gus Grubba's avatar
Gus Grubba committed
158
            union_value.param_int64 = static_cast<int64_t>(_fact->rawValue().toLongLong());
159 160
            break;
        case FactMetaData::valueTypeUint64:
Gus Grubba's avatar
Gus Grubba committed
161
            union_value.param_uint64 = static_cast<uint64_t>(_fact->rawValue().toULongLong());
162
            break;
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:
171 172 173
        case FactMetaData::valueTypeCustom:
            {
                QByteArray custom = _fact->rawValue().toByteArray();
Gus Grubba's avatar
Gus Grubba committed
174
                memcpy(union_value.bytes, custom.data(), static_cast<size_t>(std::max(custom.size(), MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN)));
175 176
            }
            break;
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
            //-- Fall Through (screw clang)
180
        case FactMetaData::valueTypeInt32:
Gus Grubba's avatar
Gus Grubba committed
181
            union_value.param_int32 = static_cast<int32_t>(_fact->rawValue().toInt());
182 183
            break;
    }
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     = static_cast<uint8_t>(_vehicle->id());
    p.target_component  = static_cast<uint8_t>(_control->compID());
187 188
    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(
Gus Grubba's avatar
Gus Grubba committed
189 190
        static_cast<uint8_t>(_pMavlink->getSystemId()),
        static_cast<uint8_t>(_pMavlink->getComponentId()),
191 192 193 194
        _vehicle->priorityLink()->mavlinkChannel(),
        &msg,
        &p);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
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;
205 206
    } else {
        //-- Send it again
207
        qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries;
208
        _sendParameter();
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) {
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);
            }
226 227
        }
    } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) {
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;
239 240 241
        } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) {
            qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name();
        }
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);
            }
248
        }
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);
    }
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();
    }
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;
279 280
    param_ext_union_t u;
    memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
281
    switch (param_type) {
282
        case MAV_PARAM_EXT_TYPE_REAL32:
283 284
            var = QVariant(u.param_float);
            break;
285
        case MAV_PARAM_EXT_TYPE_UINT8:
286 287
            var = QVariant(u.param_uint8);
            break;
288
        case MAV_PARAM_EXT_TYPE_INT8:
289 290
            var = QVariant(u.param_int8);
            break;
291
        case MAV_PARAM_EXT_TYPE_UINT16:
292 293
            var = QVariant(u.param_uint16);
            break;
294
        case MAV_PARAM_EXT_TYPE_INT16:
295 296
            var = QVariant(u.param_int16);
            break;
297
        case MAV_PARAM_EXT_TYPE_UINT32:
298 299
            var = QVariant(u.param_uint32);
            break;
300
        case MAV_PARAM_EXT_TYPE_INT32:
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(static_cast<qulonglong>(u.param_uint64));
305 306
            break;
        case MAV_PARAM_EXT_TYPE_INT64:
Gus Grubba's avatar
Gus Grubba committed
307
            var = QVariant(static_cast<qulonglong>(u.param_int64));
308
            break;
309
        case MAV_PARAM_EXT_TYPE_CUSTOM:
310 311
            var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN));
            break;
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();
        }
329 330 331
    } else {
        //-- Request it again
        qCDebug(CameraIOLog) << "Param request retry:" << _fact->name();
332
        paramRequest(false);
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
    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(
Gus Grubba's avatar
Gus Grubba committed
359 360
        static_cast<uint8_t>(_pMavlink->getSystemId()),
        static_cast<uint8_t>(_pMavlink->getComponentId()),
361 362
        _vehicle->priorityLink()->mavlinkChannel(),
        &msg,
Gus Grubba's avatar
Gus Grubba committed
363 364
        static_cast<uint8_t>(_vehicle->id()),
        static_cast<uint8_t>(_control->compID()),
365 366 367 368 369
        param_id,
        -1);
    _vehicle->sendMessageOnLink(_vehicle->priorityLink(), msg);
    _paramRequestTimer.start();
}