david.tahir/QuadMeUp_Crossbow
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Small optimization

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2017-10-29 09:56:06 +01:00
parent a2fb5abffa
commit 86c7c3af0a
4 changed files with 22 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

19
qsp.cpp
View File

@@ -143,8 +143,13 @@ void qspClearPayload(QspConfiguration_t *qsp)
qsp->payloadLength = 0;
}
void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[], RxDeviceState_t *rxDeviceState)
{
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,
int ppm[],
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
) {
static uint8_t frameId;
static uint8_t payloadLength;
static uint8_t receivedPayload;
@@ -228,12 +233,12 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
break;
case QSP_FRAME_PONG:
rxDeviceState->roundtrip = qsp->payload[0];
rxDeviceState->roundtrip += (uint32_t) qsp->payload[1] << 8;
rxDeviceState->roundtrip += (uint32_t) qsp->payload[2] << 16;
rxDeviceState->roundtrip += (uint32_t) qsp->payload[3] << 24;
txDeviceState->roundtrip = qsp->payload[0];
txDeviceState->roundtrip += (uint32_t) qsp->payload[1] << 8;
txDeviceState->roundtrip += (uint32_t) qsp->payload[2] << 16;
txDeviceState->roundtrip += (uint32_t) qsp->payload[3] << 24;
rxDeviceState->roundtrip = (micros() - rxDeviceState->roundtrip) / 1000;
txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
break;