QGCCameraIO.cc 12 KB
Newer Older
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)
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 60
            break;
        case FactMetaData::valueTypeFloat:
61
            _mavParamType = MAV_PARAM_EXT_TYPE_REAL32;
62
            break;
Gus Grubba's avatar
Gus Grubba committed
63 64
            //-- String and custom are the same for now
        case FactMetaData::valueTypeString:
65
        case FactMetaData::valueTypeCustom:
66
            _mavParamType = MAV_PARAM_EXT_TYPE_CUSTOM;
67
            break;
68
        default:
Gus Grubba's avatar
Gus Grubba committed
69
            qWarning() << "Unsupported fact type" << _fact->type() << "for" << _fact->name();
70 71
            // Fall through
        case FactMetaData::valueTypeInt32:
72
            _mavParamType = MAV_PARAM_EXT_TYPE_INT32;
73 74
            break;
    }
75 76 77 78 79 80
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::setParamRequest()
{
81 82 83 84 85
    if(!_fact->writeOnly()) {
        _paramRequestReceived = false;
        _requestRetries = 0;
        _paramRequestTimer.start();
    }
86 87 88 89 90 91
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_factChanged(QVariant value)
{
92 93 94 95 96
    if(!_forceUIUpdate) {
        Q_UNUSED(value);
        qCDebug(CameraIOLog) << "UI Fact" << _fact->name() << "changed to" << value;
        _control->factChanged(_fact);
    }
97 98
}

Gus Grubba's avatar
Gus Grubba committed
99 100 101 102
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_containerRawValueChanged(const QVariant value)
{
103 104
    if(!_fact->readOnly()) {
        Q_UNUSED(value);
Gus Grubba's avatar
Gus Grubba committed
105
        qCDebug(CameraIOLog) << "Update Fact from camera" << _fact->name();
106 107 108
        _sentRetries = 0;
        _sendParameter();
    }
Gus Grubba's avatar
Gus Grubba committed
109 110
}

111 112 113 114 115 116 117 118 119 120
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::sendParameter(bool updateUI)
{
    qCDebug(CameraIOLog) << "Send Fact" << _fact->name();
    _sentRetries = 0;
    _updateOnSet = updateUI;
    _sendParameter();
}

121 122 123 124
//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_sendParameter()
{
125 126
    mavlink_param_ext_set_t p;
    memset(&p, 0, sizeof(mavlink_param_ext_set_t));
127 128
    param_ext_union_t   union_value;
    mavlink_message_t   msg;
129
    FactMetaData::ValueType_t factType = _fact->type();
130
    p.param_type = _mavParamType;
131 132
    switch (factType) {
        case FactMetaData::valueTypeUint8:
Gus Grubba's avatar
Gus Grubba committed
133
        case FactMetaData::valueTypeBool:
134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150
            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;
        case FactMetaData::valueTypeFloat:
            union_value.param_float = _fact->rawValue().toFloat();
            break;
Gus Grubba's avatar
Gus Grubba committed
151 152
            //-- String and custom are the same for now
        case FactMetaData::valueTypeString:
153 154 155 156 157 158
        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;
159
        default:
Gus Grubba's avatar
Gus Grubba committed
160
            qCritical() << "Unsupported fact type" << factType << "for" << _fact->name();
161 162 163 164 165
            // fall through
        case FactMetaData::valueTypeInt32:
            union_value.param_int32 = (int32_t)_fact->rawValue().toInt();
            break;
    }
166
    memcpy(&p.param_value[0], &union_value.bytes[0], MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
167 168
    p.target_system = (uint8_t)_vehicle->id();
    p.target_component = (uint8_t)_control->compID();
169 170 171 172 173 174 175 176
    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);
177 178 179 180 181 182 183 184 185
    _paramWriteTimer.start();
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::_paramWriteTimeout()
{
    if(++_sentRetries > 3) {
        qCWarning(CameraIOLog) << "No response for param set:" << _fact->name();
186
        _updateOnSet = false;
187 188
    } else {
        //-- Send it again
189
        qCDebug(CameraIOLog) << "Param set retry:" << _fact->name() << _sentRetries;
190
        _sendParameter();
191 192 193 194 195 196 197 198 199 200
        _paramWriteTimer.start();
    }
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::handleParamAck(const mavlink_param_ext_ack_t& ack)
{
    _paramWriteTimer.stop();
    if(ack.param_result == PARAM_ACK_ACCEPTED) {
201 202 203
        QVariant val = _valueFromMessage(ack.param_value, ack.param_type);
        if(_fact->rawValue() != val) {
            _fact->_containerSetRawValue(val);
204 205 206 207
            if(_updateOnSet) {
                _updateOnSet = false;
                _control->factChanged(_fact);
            }
208 209
        }
    } else if(ack.param_result == PARAM_ACK_IN_PROGRESS) {
210 211 212 213 214
        //-- 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
215
            if(++_sentRetries < 3) {
216 217 218 219 220
                //-- Try again
                qCWarning(CameraIOLog) << "Param set failed:" << _fact->name() << _sentRetries;
                _paramWriteTimer.start();
            }
            return;
221 222 223
        } else if(ack.param_result == PARAM_ACK_VALUE_UNSUPPORTED) {
            qCWarning(CameraIOLog) << "Param set unsuported:" << _fact->name();
        }
224 225 226
        //-- If UI changed and value was not set, restore UI
        QVariant val = _valueFromMessage(ack.param_value, ack.param_type);
        if(_fact->rawValue() != val) {
227 228 229
            if(_control->validateParameter(_fact, val)) {
                _fact->_containerSetRawValue(val);
            }
230
        }
231 232 233 234 235 236 237 238
    }
}

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::handleParamValue(const mavlink_param_ext_value_t& value)
{
    _paramRequestTimer.stop();
239 240 241 242
    QVariant newValue = _valueFromMessage(value.param_value, value.param_type);
    if(_control->incomingParameter(_fact, newValue)) {
        _fact->_containerSetRawValue(newValue);
    }
243
    _paramRequestReceived = true;
244 245 246 247 248
    if(_forceUIUpdate) {
        emit _fact->rawValueChanged(_fact->rawValue());
        emit _fact->valueChanged(_fact->rawValue());
        _forceUIUpdate = false;
    }
249 250 251 252
    if(!_done) {
        _done = true;
        _control->_paramDone();
    }
253 254 255 256 257 258 259 260
    qCDebug(CameraIOLog) << QString("handleParamValue() %1 %2").arg(_fact->name()).arg(_fact->rawValueString());
}

//-----------------------------------------------------------------------------
QVariant
QGCCameraParamIO::_valueFromMessage(const char* value, uint8_t param_type)
{
    QVariant var;
261 262
    param_ext_union_t u;
    memcpy(u.bytes, value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN);
263
    switch (param_type) {
264
        case MAV_PARAM_EXT_TYPE_REAL32:
265 266
            var = QVariant(u.param_float);
            break;
267
        case MAV_PARAM_EXT_TYPE_UINT8:
268 269
            var = QVariant(u.param_uint8);
            break;
270
        case MAV_PARAM_EXT_TYPE_INT8:
271 272
            var = QVariant(u.param_int8);
            break;
273
        case MAV_PARAM_EXT_TYPE_UINT16:
274 275
            var = QVariant(u.param_uint16);
            break;
276
        case MAV_PARAM_EXT_TYPE_INT16:
277 278
            var = QVariant(u.param_int16);
            break;
279
        case MAV_PARAM_EXT_TYPE_UINT32:
280 281
            var = QVariant(u.param_uint32);
            break;
282
        case MAV_PARAM_EXT_TYPE_INT32:
283 284
            var = QVariant(u.param_int32);
            break;
285
        case MAV_PARAM_EXT_TYPE_CUSTOM:
286 287
            var = QVariant(QByteArray(value, MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN));
            break;
288 289 290 291 292 293 294 295 296 297 298 299 300
        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();
301 302 303 304
        if(!_done) {
            _done = true;
            _control->_paramDone();
        }
305 306 307
    } else {
        //-- Request it again
        qCDebug(CameraIOLog) << "Param request retry:" << _fact->name();
308
        paramRequest(false);
309 310 311
        _paramRequestTimer.start();
    }
}
312 313 314 315 316

//-----------------------------------------------------------------------------
void
QGCCameraParamIO::paramRequest(bool reset)
{
317 318 319 320 321 322 323 324
    //-- If it's write only, we don't request it.
    if(_fact->writeOnly()) {
        if(!_done) {
            _done = true;
            _control->_paramDone();
        }
        return;
    }
325 326
    if(reset) {
        _requestRetries = 0;
327
        _forceUIUpdate  = true;
328
    }
329
    qCDebug(CameraIOLog) << "Request parameter:" << _fact->name();
330 331 332 333 334 335 336 337 338 339 340 341 342 343 344 345
    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();
}