1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-18 03:52:11 +01:00

OP-4 GCS/Telemetry Limit transmit buffer size

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@542 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-04-25 02:21:36 +00:00 committed by vassilis
parent 7b99c3d7f0
commit e406db3786
2 changed files with 11 additions and 2 deletions

View File

@ -568,8 +568,16 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance
cs = updateChecksum(cs, txBuffer, dataOffset+length);
qToLittleEndian<quint16>(cs, &txBuffer[dataOffset+length]);
// Send buffer
io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH);
// Send buffer, check that the transmit backlog does not grow above limit
if ( io->bytesToWrite() < TX_BUFFER_SIZE )
{
io->write((const char*)txBuffer, dataOffset+length+CHECKSUM_LENGTH);
}
else
{
++stats.txErrors;
return false;
}
// Update stats
++stats.txObjects;

View File

@ -77,6 +77,7 @@ private:
static const int MAX_PAYLOAD_LENGTH = 256;
static const int MAX_PACKET_LENGTH = (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH);
static const quint16 ALL_INSTANCES = 0xFFFF;
static const int TX_BUFFER_SIZE = 2*1024;
typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType;