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);
}
}
}
}
}
/**
* 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;
}
/**
* 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;
}