void ConfigStabilizationWidget::swapBankAndCurrent(int bank)
{
    UAVObject *fromStabBankObject = getStabBankObject(m_currentStabSettingsBank);
    UAVObject *toStabBankObject   = getStabBankObject(bank);

    if (fromStabBankObject && toStabBankObject) {
        quint8 fromStabBankObjectData[fromStabBankObject->getNumBytes()];
        quint8 toStabBankObjectData[toStabBankObject->getNumBytes()];
        fromStabBankObject->pack(fromStabBankObjectData);
        toStabBankObject->pack(toStabBankObjectData);
        toStabBankObject->unpack(fromStabBankObjectData);
        fromStabBankObject->unpack(toStabBankObjectData);
    }
}
void ConfigStabilizationWidget::copyFromBankToBank(int fromBank, int toBank)
{
    UAVObject *fromStabBankObject = getStabBankObject(fromBank);
    UAVObject *toStabBankObject   = getStabBankObject(toBank);

    if (fromStabBankObject && toStabBankObject) {
        quint8 data[fromStabBankObject->getNumBytes()];
        fromStabBankObject->pack(data);
        toStabBankObject->unpack(data);
    }
}
void ConfigStabilizationWidget::copyCurrentStabBank()
{
    UAVObject *fromStabBankObject = getStabBankObject(m_currentStabSettingsBank);

    if (fromStabBankObject) {
        quint8 fromStabBankObjectData[fromStabBankObject->getNumBytes()];
        fromStabBankObject->pack(fromStabBankObjectData);
        for (int i = 0; i < m_stabSettingsBankCount; i++) {
            if (i != m_currentStabSettingsBank) {
                UAVObject *toStabBankObject = getStabBankObject(i);
                if (toStabBankObject) {
                    toStabBankObject->unpack(fromStabBankObjectData);
                }
            }
        }
    }
}
示例#4
0
/**
 * Process an byte from the telemetry stream.
 * \param[in] rxbyte Received byte
 * \return Success (true), Failure (false)
 */
bool UAVTalk::processInputByte(quint8 rxbyte)
{
    // Update stats
    stats.rxBytes++;

    rxPacketLength++;   // update packet byte count

    // Receive state machine
    switch (rxState)
    {
        case STATE_SYNC:

            if (rxbyte != SYNC_VAL)
            {
                UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Sync (" + QString::number(rxbyte) + " " + QString("0x%1").arg(rxbyte,2,16) + ")");
                break;
            }

            // Initialize and update CRC
            rxCS = updateCRC(0, rxbyte);

            rxPacketLength = 1;

            rxState = STATE_TYPE;
            UAVTALK_QXTLOG_DEBUG("UAVTalk: Sync->Type");
            break;

        case STATE_TYPE:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            if ((rxbyte & TYPE_MASK) != TYPE_VER)
            {
                rxState = STATE_SYNC;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Sync");
                break;
            }

            rxType = rxbyte;

            packetSize = 0;

            rxState = STATE_SIZE;
            UAVTALK_QXTLOG_DEBUG("UAVTalk: Type->Size");
            rxCount = 0;
            break;

        case STATE_SIZE:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            if (rxCount == 0)
            {
                packetSize += rxbyte;
                rxCount++;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Size");
                break;
            }

            packetSize += (quint32)rxbyte << 8;

            if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH)
            {   // incorrect packet size
                rxState = STATE_SYNC;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->Sync");
                break;
            }

            rxCount = 0;
            rxState = STATE_OBJID;
            UAVTALK_QXTLOG_DEBUG("UAVTalk: Size->ObjID");
            break;

        case STATE_OBJID:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            rxTmpBuffer[rxCount++] = rxbyte;
            if (rxCount < 4)
            {
                UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->ObjID");
                break;
            }

            // Search for object, if not found reset state machine
            rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
            {
                UAVObject *rxObj = objMngr->getObject(rxObjId);
                if (rxObj == NULL && rxType != TYPE_OBJ_REQ)
                {
                    stats.rxErrors++;
                    rxState = STATE_SYNC;
                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (badtype)");
                    break;
                }

                // Determine data length
                if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK)
                {
                    rxLength = 0;
                    rxInstanceLength = 0;
                }
                else
                {
                    rxLength = rxObj->getNumBytes();
                    rxInstanceLength = (rxObj->isSingleInstance() ? 0 : 2);
                }

                // Check length and determine next state
                if (rxLength >= MAX_PAYLOAD_LENGTH)
                {
                    stats.rxErrors++;
                    rxState = STATE_SYNC;
                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (oversize)");
                    break;
                }

                // Check the lengths match
                if ((rxPacketLength + rxInstanceLength + rxLength) != packetSize)
                {   // packet error - mismatched packet size
                    stats.rxErrors++;
                    rxState = STATE_SYNC;
                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Sync (length mismatch)");
                    break;
                }

                // Check if this is a single instance object (i.e. if the instance ID field is coming next)
                if (rxObj == NULL)
                {
                   // This is a non-existing object, just skip to checksum
                   // and we'll send a NACK next.
                   rxState   = STATE_CS;
                   UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->CSum (no obj)");
                   rxInstId = 0;
                   rxCount = 0;
                }
                else if (rxObj->isSingleInstance())
                {
                    // If there is a payload get it, otherwise receive checksum
                    if (rxLength > 0)
                    {
                        rxState = STATE_DATA;
                        UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Data (needs data)");
                    }
                    else
                    {
                        rxState = STATE_CS;
                        UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->Checksum");
                    }
                    rxInstId = 0;
                    rxCount = 0;
                }
                else
                {
                    rxState = STATE_INSTID;
                    UAVTALK_QXTLOG_DEBUG("UAVTalk: ObjID->InstID");
                    rxCount = 0;
                }
            }

            break;

        case STATE_INSTID:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            rxTmpBuffer[rxCount++] = rxbyte;
            if (rxCount < 2)
            {
                UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->InstID");
                break;
            }

            rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);

            rxCount = 0;

            // If there is a payload get it, otherwise receive checksum
            if (rxLength > 0)
            {
                rxState = STATE_DATA;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->Data");
            }
            else
            {
                rxState = STATE_CS;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: InstID->CSum");
            }
            break;

        case STATE_DATA:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            rxBuffer[rxCount++] = rxbyte;
            if (rxCount < rxLength)
            {
                UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->Data");
                break;
            }

            rxState = STATE_CS;
            UAVTALK_QXTLOG_DEBUG("UAVTalk: Data->CSum");
            rxCount = 0;
            break;

        case STATE_CS:

            // The CRC byte
            rxCSPacket = rxbyte;

            if (rxCS != rxCSPacket)
            {   // packet error - faulty CRC
                stats.rxErrors++;
                rxState = STATE_SYNC;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (badcrc)");
                break;
            }

            if (rxPacketLength != packetSize + 1)
            {   // packet error - mismatched packet size
                stats.rxErrors++;
                rxState = STATE_SYNC;
                UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (length mismatch)");
                break;
            }

            mutex->lock();
                receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
                stats.rxObjectBytes += rxLength;
                stats.rxObjects++;
            mutex->unlock();

            rxState = STATE_SYNC;
            UAVTALK_QXTLOG_DEBUG("UAVTalk: CSum->Sync (OK)");
            break;

        default:
            rxState = STATE_SYNC;
            stats.rxErrors++;
            UAVTALK_QXTLOG_DEBUG("UAVTalk: ???->Sync");
    }

    // Done
    return true;
}
示例#5
0
/**
 * Process an byte from the telemetry stream.
 * \param[in] rxbyte Received byte
 * \return Success (true), Failure (false)
 */
bool UAVTalk::processInputByte(quint8 rxbyte)
{
	if (!objMngr || !io)	// Pip
		return false;

    // Update stats
    stats.rxBytes++;

    rxPacketLength++;   // update packet byte count

    // Receive state machine
    switch (rxState)
    {
        case STATE_SYNC:

            if (rxbyte != SYNC_VAL)
                break;

            // Initialize and update CRC
            rxCS = updateCRC(0, rxbyte);

            rxPacketLength = 1;

            rxState = STATE_TYPE;
            break;

        case STATE_TYPE:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            if ((rxbyte & TYPE_MASK) != TYPE_VER)
            {
                rxState = STATE_SYNC;
                break;
            }

            rxType = rxbyte;

            packetSize = 0;

            rxState = STATE_SIZE;
            rxCount = 0;
            break;

        case STATE_SIZE:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            if (rxCount == 0)
            {
                packetSize += rxbyte;
                rxCount++;
                break;
            }

            packetSize += (quint32)rxbyte << 8;

            if (packetSize < MIN_HEADER_LENGTH || packetSize > MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH)
            {   // incorrect packet size
                rxState = STATE_SYNC;
                break;
            }

            rxCount = 0;
            rxState = STATE_OBJID;
            break;

        case STATE_OBJID:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            rxTmpBuffer[rxCount++] = rxbyte;
            if (rxCount < 4)
                break;

            // Search for object, if not found reset state machine
            rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
            {
                UAVObject *rxObj = objMngr->getObject(rxObjId);
                if (rxObj == NULL)
                {
                    stats.rxErrors++;
                    rxState = STATE_SYNC;
                    break;
                }

                // Determine data length
                if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
                    rxLength = 0;
                else
                    rxLength = rxObj->getNumBytes();

                // Check length and determine next state
                if (rxLength >= MAX_PAYLOAD_LENGTH)
                {
                    stats.rxErrors++;
                    rxState = STATE_SYNC;
                    break;
                }

                // Check the lengths match
                if ((rxPacketLength + rxLength) != packetSize)
                {   // packet error - mismatched packet size
                    stats.rxErrors++;
                    rxState = STATE_SYNC;
                    break;
                }

                // Check if this is a single instance object (i.e. if the instance ID field is coming next)
                if (rxObj->isSingleInstance())
                {
                    // If there is a payload get it, otherwise receive checksum
                    if (rxLength > 0)
                        rxState = STATE_DATA;
                    else
                        rxState = STATE_CS;
                    rxInstId = 0;
                    rxCount = 0;
                }
                else
                {
                    rxState = STATE_INSTID;
                    rxCount = 0;
                }
            }

            break;

        case STATE_INSTID:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            rxTmpBuffer[rxCount++] = rxbyte;
            if (rxCount < 2)
                break;

            rxInstId = (qint16)qFromLittleEndian<quint16>(rxTmpBuffer);

            rxCount = 0;

            // If there is a payload get it, otherwise receive checksum
            if (rxLength > 0)
                rxState = STATE_DATA;
            else
                rxState = STATE_CS;

            break;

        case STATE_DATA:

            // Update CRC
            rxCS = updateCRC(rxCS, rxbyte);

            rxBuffer[rxCount++] = rxbyte;
            if (rxCount < rxLength)
                break;

            rxState = STATE_CS;
            rxCount = 0;
            break;

        case STATE_CS:

            // The CRC byte
            rxCSPacket = rxbyte;

            if (rxCS != rxCSPacket)
            {   // packet error - faulty CRC
                stats.rxErrors++;
                rxState = STATE_SYNC;
                break;
            }

            if (rxPacketLength != packetSize + 1)
            {   // packet error - mismatched packet size
                stats.rxErrors++;
                rxState = STATE_SYNC;
                break;
            }

            mutex->lock();
                receiveObject(rxType, rxObjId, rxInstId, rxBuffer, rxLength);
                stats.rxObjectBytes += rxLength;
                stats.rxObjects++;
            mutex->unlock();

            rxState = STATE_SYNC;
            break;

        default:
            rxState = STATE_SYNC;
            stats.rxErrors++;
    }

    // Done
    return true;
}