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

Merge pull request #27 from DzikuVx/qsp-callbacks

Browse Source 
Air protocol decoding using callbacks
This commit is contained in:
Paweł Spychalski
2017-11-16 20:18:42 +01:00
committed by GitHub
parent 025315489c 3778fcd0cd
commit 36b7933f11
4 changed files with 83 additions and 75 deletions
Download Patch File Download Diff File Expand all files Collapse all files

56
crossbow.ino
View File

@@ -13,8 +13,6 @@
#include "main_variables.h"
#include "qsp.h"
int ppm[16] = {0};
// LoRa32u4 ports
#define LORA32U4_SS_PIN 8
#define LORA32U4_RST_PIN 4
@@ -81,6 +79,50 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp)
LoRa.write(dataByte);
}
void onQspSuccess(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, RadioState_t *radioState) {
//If devide received a valid frame, that means it can start to talk
qsp->canTransmit = true;
//Store the last timestamp when frame was received
if (qsp->frameId < QSP_FRAME_COUNT) {
qsp->lastFrameReceivedAt[qsp->frameId] = millis();
}
qsp->anyFrameRecivedAt = millis();
switch (qsp->frameId) {
case QSP_FRAME_RC_DATA:
qspDecodeRcDataFrame(qsp, rxDeviceState);
break;
case QSP_FRAME_RX_HEALTH:
decodeRxHealthPayload(qsp, rxDeviceState);
break;
case QSP_FRAME_PING:
qsp->forcePongFrame = true;
break;
case QSP_FRAME_PONG:
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;
txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
break;
default:
//Unknown frame
//TODO do something in this case
break;
}
qsp->transmitWindowOpen = true;
}
void onQspFailure(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDeviceState_t *rxDeviceState, RadioState_t *radioState) {
}
void setup(void)
{
#ifdef DEBUG_SERIAL
@@ -88,6 +130,8 @@ void setup(void)
#endif
qsp.hardwareWriteFunction = writeToRadio;
qsp.onSuccessCallback = onQspSuccess;
qsp.onFailureCallback = onQspFailure;
#ifdef DEVICE_MODE_RX
qsp.deviceState = DEVICE_STATE_FAILSAFE;
@@ -123,7 +167,7 @@ void setup(void)
//initiallize default ppm values
for (int i = 0; i < 16; i++)
{
ppm[i] = PPM_CHANNEL_DEFAULT_VALUE;
rxDeviceState.channels[i] = PPM_CHANNEL_DEFAULT_VALUE;
}
pinMode(RX_ADC_PIN_1, INPUT);
@@ -217,7 +261,7 @@ void loop(void)
if (radioState.bytesToRead != NO_DATA_TO_READ) {
for (uint8_t i = 0; i < radioState.bytesToRead; i++) {
qspDecodeIncomingFrame(&qsp, radioState.data[i], ppm, &rxDeviceState, &txDeviceState);
qspDecodeIncomingFrame(&qsp, radioState.data[i], &rxDeviceState, &txDeviceState, &radioState);
}
radioState.bytesToRead = NO_DATA_TO_READ;
@@ -283,7 +327,7 @@ void loop(void)
if (lastRxStateTaskTime + RX_TASK_HEALTH < currentMillis) {
lastRxStateTaskTime = currentMillis;
updateRxDeviceState(&rxDeviceState);
ppm[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 164, 1000, 2000);
rxDeviceState.channels[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 164, 1000, 2000);
if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
digitalWrite(LED_BUILTIN, HIGH);
} else {
@@ -322,7 +366,7 @@ void loop(void)
}
if (currentMillis > sbusTime) {
sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
sbusPreparePacket(sbusPacket, rxDeviceState.channels, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
sbusTime = currentMillis + SBUS_UPDATE_RATE;
}

56
qsp.cpp
View File

@@ -2,7 +2,7 @@
#include "variables.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]) {
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) {
int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0};
//TODO fix it, baby :)
@@ -37,7 +37,7 @@ void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]) {
* Copy tremporary to real output
*/
for (uint8_t i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++) {
output[i] = temporaryPpmOutput[i];
rxDeviceSate->channels[i] = temporaryPpmOutput[i];
}
}
@@ -159,9 +159,9 @@ void qspClearPayload(QspConfiguration_t *qsp)
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,
int ppm[],
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
TxDeviceState_t *txDeviceState,
RadioState_t *radioState
) {
static uint8_t frameId;
static uint8_t payloadLength;
@@ -190,7 +190,7 @@ void qspDecodeIncomingFrame(
//Frame ID and payload length
qspComputeCrc(qsp, incomingByte);
frameId = (incomingByte >> 4) & 0x0f;
qsp->frameId = (incomingByte >> 4) & 0x0f;
payloadLength = incomingByte & 0x0f;
qsp->protocolState = QSP_STATE_FRAME_TYPE_RECEIVED;
@@ -217,49 +217,9 @@ void qspDecodeIncomingFrame(
{
if (qsp->crc == incomingByte) {
//CRC is correct
//If devide received a valid frame, that means it can start to talk
qsp->canTransmit = true;
//Store the last timestamp when frame was received
if (frameId < QSP_FRAME_COUNT) {
qsp->lastFrameReceivedAt[frameId] = millis();
}
qsp->anyFrameRecivedAt = millis();
switch (frameId) {
case QSP_FRAME_RC_DATA:
qspDecodeRcDataFrame(qsp, ppm);
break;
case QSP_FRAME_RX_HEALTH:
decodeRxHealthPayload(qsp, rxDeviceState);
break;
case QSP_FRAME_PING:
qsp->forcePongFrame = true;
break;
case QSP_FRAME_PONG:
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;
txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
break;
default:
//Unknown frame
//TODO do something in this case
break;
}
qsp->transmitWindowOpen = true;
}
else
{
//CRC failed, frame has to be rejected
//TODO do something in this case or something
qsp->onSuccessCallback(qsp, txDeviceState, rxDeviceState, radioState);
} else {
qsp->onFailureCallback(qsp, txDeviceState, rxDeviceState, radioState);
}
// In both cases switch to listening for next preamble

6
qsp.h
View File

@@ -1,7 +1,7 @@
#include "Arduino.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]);
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate);
void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
uint8_t get10bitHighShift(uint8_t channel);
@@ -12,9 +12,9 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,
int ppm[],
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
TxDeviceState_t *txDeviceState,
RadioState_t *radioState
);
void qspClearPayload(QspConfiguration_t *qsp);
void qspEncodeFrame(QspConfiguration_t *qsp);

40
variables.h
View File

@@ -85,24 +85,6 @@ struct RadioState_t {
uint8_t data[20] = {0}; //Max size of packet that can be processed in QSP
};
struct QspConfiguration_t {
uint8_t protocolState = QSP_STATE_IDLE;
uint8_t crc = 0;
uint8_t payload[QSP_PAYLOAD_LENGTH] = {0};
uint8_t payloadLength = 0;
uint8_t frameToSend = 0;
uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
uint32_t anyFrameRecivedAt = 0;
uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
bool canTransmit = false;
bool forcePongFrame = false;
uint8_t debugConfig = 0;
uint32_t frameDecodingStartedAt = 0;
uint32_t lastTxSlotTimestamp = 0;
bool transmitWindowOpen = false;
};
struct TxDeviceState_t {
uint8_t flags = 0;
uint32_t roundtrip = 0;
@@ -116,4 +98,26 @@ struct RxDeviceState_t {
uint8_t a1Voltage = 0;
uint8_t a2Voltage = 0;
uint8_t flags = 0;
int16_t channels[16] = {};
};
struct QspConfiguration_t {
uint8_t protocolState = QSP_STATE_IDLE;
uint8_t crc = 0;
uint8_t payload[QSP_PAYLOAD_LENGTH] = {0};
uint8_t payloadLength = 0;
uint8_t frameToSend = 0;
uint8_t frameId = 0;
uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
uint32_t anyFrameRecivedAt = 0;
uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, RadioState_t*);
void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, RadioState_t*);
bool canTransmit = false;
bool forcePongFrame = false;
uint8_t debugConfig = 0;
uint32_t frameDecodingStartedAt = 0;
uint32_t lastTxSlotTimestamp = 0;
bool transmitWindowOpen = false;
};