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

quite major overhaul

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2017-10-07 18:57:13 +02:00
parent 3deec452ef
commit 022f186175
4 changed files with 193 additions and 184 deletions
Download Patch File Download Diff File Expand all files Collapse all files

172
crossbow.ino
View File

@@ -39,154 +39,7 @@ bool canTransmit = false;
/*
* Start of QSP protocol implementation
*/
static uint8_t protocolState = IDLE;
static uint8_t qspCrc = 0;
static uint8_t qspPayload[QSP_PAYLOAD_LENGTH] = {0};
static uint8_t qspPayloadLength = 0;
static uint8_t qspFrameToSend = 0;
uint8_t qspGetPacketId()
{
static uint8_t packetId = 0;
return packetId++;
}
void qspClearPayload()
{
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
{
qspPayload[i] = 0;
}
qspPayloadLength = 0;
}
void qspDecodeIncomingFrame(uint8_t incomingByte)
{
static uint8_t frameId;
static uint8_t payloadLength;
static uint8_t receivedPayload;
static uint8_t packetId; //TODO move this to global scope maybe?
if (protocolState == IDLE && incomingByte == QSP_PREAMBLE)
{
//If in IDLE and correct preamble comes, start to decode frame
protocolState = PREAMBLE_RECEIVED;
qspCrc = 0 ^ incomingByte;
}
else if (protocolState == PREAMBLE_RECEIVED)
{
// Check if incomming channel ID is the same as receiver
if (incomingByte == CHANNEL_ID)
{
protocolState = CHANNEL_RECEIVED;
qspCrc ^= incomingByte;
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
{
qspPayload[i] = 0x00;
}
receivedPayload = 0;
packetId = 0;
}
else
{
protocolState = IDLE;
}
}
else if (protocolState == CHANNEL_RECEIVED)
{
//Frame ID and payload length
qspCrc ^= incomingByte;
frameId = (incomingByte >> 4) & 0x0f;
payloadLength = incomingByte & 0x0f;
protocolState = FRAME_TYPE_RECEIVED;
}
else if (protocolState == FRAME_TYPE_RECEIVED)
{
qspCrc ^= incomingByte;
packetId = incomingByte;
protocolState = PACKET_ID_RECEIVED;
}
else if (protocolState == PACKET_ID_RECEIVED)
{
//Now it's time for payload
qspCrc ^= incomingByte;
qspPayload[receivedPayload] = incomingByte;
receivedPayload++;
if (receivedPayload == payloadLength)
{
protocolState = PAYLOAD_RECEIVED;
}
}
else if (protocolState == PAYLOAD_RECEIVED)
{
if (qspCrc == incomingByte)
{
//CRC is correct
#ifdef DEVICE_MODE_RX
//If devide received a valid frame, that means it can start to talk
canTransmit = true;
#endif
switch (frameId)
{
case QSP_FRAME_RC_DATA:
qspDecodeRcDataFrame(qspPayload, ppm);
break;
default:
//Unknown frame
//TODO do something in this case
break;
}
}
else
{
//CRC failed, frame has to be rejected
//TODO do something in this case or something
}
// In both cases switch to listening for next preamble
protocolState = IDLE;
}
}
void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t payload[])
{
//Zero CRC
qspCrc = 0;
//Write preamble
writeToRadio(QSP_PREAMBLE);
//Write CHANNEL_ID
writeToRadio(CHANNEL_ID);
//Write frame type and length
uint8_t data = length & 0x0f;
data |= (frameId << 4) & 0xf0;
writeToRadio(data);
//Write packet ID
writeToRadio(qspGetPacketId());
//Write payload
for (uint8_t i = 0; i < length; i++)
{
writeToRadio(payload[i]);
}
//Finally write CRC
writeToRadio(qspCrc);
}
QspConfiguration_t qsp = {};
/*
* End of QSP protocol implementation
@@ -220,10 +73,10 @@ void radioPacketEnd(void)
Serial.begin(UART_SPEED);
}
void writeToRadio(uint8_t dataByte)
void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp)
{
//Compute CRC
qspComputeCrc(&qspCrc, dataByte);
qspComputeCrc(qsp, dataByte);
//Write to radio
Serial.write(dataByte);
@@ -253,10 +106,10 @@ void radioPacketEnd(void)
LoRa.endPacket();
}
void writeToRadio(uint8_t dataByte)
void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp)
{
//Compute CRC
qspComputeCrc(&qspCrc, dataByte);
qspComputeCrc(qsp, dataByte);
//Write to radio
LoRa.write(dataByte);
@@ -274,6 +127,7 @@ display.display();
void setup(void)
{
qsp.hardwareWriteFunction = writeToRadio;
#ifdef LORA_HARDWARE_SERIAL
Serial.begin(UART_SPEED);
@@ -377,13 +231,13 @@ void loop(void)
/*
* RC_DATA QSP frame
*/
if (currentMillis - lastRcFrameTransmit > TX_RC_FRAME_RATE && !transmitPayload && protocolState == IDLE)
if (currentMillis - lastRcFrameTransmit > TX_RC_FRAME_RATE && !transmitPayload && qsp.protocolState == IDLE)
{
lastRcFrameTransmit = currentMillis;
qspClearPayload();
encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT, qspPayload, &qspPayloadLength);
qspFrameToSend = QSP_FRAME_RC_DATA;
qspClearPayload(&qsp);
encodeRcDataPayload(&qsp, &ppmReader, PPM_CHANNEL_COUNT);
qsp.frameToSend = QSP_FRAME_RC_DATA;
transmitPayload = true;
}
@@ -393,7 +247,7 @@ void loop(void)
#ifdef LORA_HARDWARE_SERIAL
if (Serial.available())
{
qspDecodeIncomingFrame(Serial.read());
qspDecodeIncomingFrame(&qsp, Serial.read(), ppm);
}
#endif
@@ -402,7 +256,7 @@ void loop(void)
transmitPayload = false;
radioPacketStart();
qspEncodeFrame(qspFrameToSend, qspPayloadLength, qspPayload);
qspEncodeFrame(&qsp);
radioPacketEnd();
}
}
@@ -415,7 +269,7 @@ void onReceive(int packetSize)
while (LoRa.available())
{
qspDecodeIncomingFrame(LoRa.read());
qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm);
}
}
#endif

186
qsp.cpp
View File

@@ -2,20 +2,20 @@
#include "variables.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(uint8_t payload[], int output[]) {
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]) {
int temporaryPpmOutput[PPM_CHANNEL_COUNT] = {0};
//TODO fix it, baby :)
temporaryPpmOutput[0] = (uint16_t) (((uint16_t) payload[0] << 2) & 0x3fc) | ((payload[1] >> 6) & 0x03);
temporaryPpmOutput[1] = (uint16_t) (((uint16_t) payload[1] << 4) & 0x3f0) | ((payload[2] >> 4) & 0x0F);
temporaryPpmOutput[2] = (uint16_t) (((uint16_t) payload[2] << 6) & 0x3c0) | ((payload[3] >> 2) & 0x3F);
temporaryPpmOutput[3] = (uint16_t) (((uint16_t) payload[3] << 8) & 0x300) | ((payload[4] >> 2) & 0xFF);
temporaryPpmOutput[4] = payload[5];
temporaryPpmOutput[5] = payload[6];
temporaryPpmOutput[6] = (payload[7] >> 4) & 0b00001111;
temporaryPpmOutput[7] = payload[7] & 0b00001111;
temporaryPpmOutput[8] = (payload[8] >> 4) & 0b00001111;
temporaryPpmOutput[9] = payload[8] & 0b00001111;
temporaryPpmOutput[0] = (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03);
temporaryPpmOutput[1] = (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F);
temporaryPpmOutput[2] = (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F);
temporaryPpmOutput[3] = (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4] >> 2) & 0xFF);
temporaryPpmOutput[4] = qsp->payload[5];
temporaryPpmOutput[5] = qsp->payload[6];
temporaryPpmOutput[6] = (qsp->payload[7] >> 4) & 0b00001111;
temporaryPpmOutput[7] = qsp->payload[7] & 0b00001111;
temporaryPpmOutput[8] = (qsp->payload[8] >> 4) & 0b00001111;
temporaryPpmOutput[9] = qsp->payload[8] & 0b00001111;
//10bit channels
temporaryPpmOutput[0] = map(temporaryPpmOutput[0], 0, 1000, 1000, 2000);
@@ -49,15 +49,15 @@ uint8_t get10bitLowShift(uint8_t channel) {
return 8 - get10bitHighShift(channel);
}
void qspComputeCrc(uint8_t *crc, uint8_t dataByte)
void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte)
{
*crc ^= dataByte;
qsp->crc ^= dataByte;
}
/**
* Encode 10 RC channels
*/
void encodeRcDataPayload(PPMReader *ppmSource, uint8_t noOfChannels, uint8_t payload[], uint8_t *payloadLength)
void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels)
{
for (uint8_t i = 0; i < noOfChannels; i++)
{
@@ -71,36 +71,178 @@ void encodeRcDataPayload(PPMReader *ppmSource, uint8_t noOfChannels, uint8_t pay
* First 4 channels encoded with 10 bits
*/
uint8_t bitIndex = i + (i / 4);
payload[bitIndex] |= (channelValue10 >> get10bitHighShift(i)) & (0x3ff >> get10bitHighShift(i));
payload[bitIndex + 1] |= (channelValue10 << get10bitLowShift(i)) & 0xff << (8 - get10bitHighShift(i));
qsp->payload[bitIndex] |= (channelValue10 >> get10bitHighShift(i)) & (0x3ff >> get10bitHighShift(i));
qsp->payload[bitIndex + 1] |= (channelValue10 << get10bitLowShift(i)) & 0xff << (8 - get10bitHighShift(i));
}
else if (i == 4 || i == 5)
{
/*
* Next 2 with 8 bits
*/
payload[i + 1] |= channelValue8;
qsp->payload[i + 1] |= channelValue8;
}
else if (i == 6)
{
/*
* And last 4 with 4 bits per channel
*/
payload[7] |= (channelValue4 << 4) & B11110000;
qsp->payload[7] |= (channelValue4 << 4) & B11110000;
}
else if (i == 7)
{
payload[7] |= channelValue4 & B00001111;
qsp->payload[7] |= channelValue4 & B00001111;
}
else if (i == 8)
{
payload[8] |= (channelValue4 << 4) & B11110000;
qsp->payload[8] |= (channelValue4 << 4) & B11110000;
}
else if (i == 9)
{
payload[8] |= channelValue4 & B00001111;
qsp->payload[8] |= channelValue4 & B00001111;
}
}
*payloadLength = 9;
qsp->payloadLength = 9;
}
uint8_t qspGetPacketId()
{
static uint8_t packetId = 0;
return packetId++;
}
void qspClearPayload(QspConfiguration_t *qsp)
{
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
{
qsp->payload[i] = 0;
}
qsp->payloadLength = 0;
}
void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[])
{
static uint8_t frameId;
static uint8_t payloadLength;
static uint8_t receivedPayload;
static uint8_t packetId; //TODO move this to global scope maybe?
if (qsp->protocolState == IDLE && incomingByte == QSP_PREAMBLE)
{
//If in IDLE and correct preamble comes, start to decode frame
qsp->protocolState = PREAMBLE_RECEIVED;
qsp->crc = 0 ^ incomingByte;
}
else if (qsp->protocolState == PREAMBLE_RECEIVED)
{
// Check if incomming channel ID is the same as receiver
if (incomingByte == CHANNEL_ID)
{
qsp->protocolState = CHANNEL_RECEIVED;
qsp->crc ^= incomingByte;
qspClearPayload(qsp);
receivedPayload = 0;
packetId = 0;
}
else
{
qsp->protocolState = IDLE;
}
}
else if (qsp->protocolState == CHANNEL_RECEIVED)
{
//Frame ID and payload length
qsp->crc ^= incomingByte;
frameId = (incomingByte >> 4) & 0x0f;
payloadLength = incomingByte & 0x0f;
qsp->protocolState = FRAME_TYPE_RECEIVED;
}
else if (qsp->protocolState == FRAME_TYPE_RECEIVED)
{
qsp->crc ^= incomingByte;
packetId = incomingByte;
qsp->protocolState = PACKET_ID_RECEIVED;
}
else if (qsp->protocolState == PACKET_ID_RECEIVED)
{
//Now it's time for payload
qsp->crc ^= incomingByte;
qsp->payload[receivedPayload] = incomingByte;
receivedPayload++;
if (receivedPayload == payloadLength)
{
qsp->protocolState = PAYLOAD_RECEIVED;
qsp->payloadLength = payloadLength;
}
}
else if (qsp->protocolState == PAYLOAD_RECEIVED)
{
if (qsp->crc == incomingByte) {
//CRC is correct
#ifdef DEVICE_MODE_RX
//If devide received a valid frame, that means it can start to talk
canTransmit = true;
#endif
switch (frameId)
{
case QSP_FRAME_RC_DATA:
qspDecodeRcDataFrame(qsp, ppm);
break;
default:
//Unknown frame
//TODO do something in this case
break;
}
}
else
{
//CRC failed, frame has to be rejected
//TODO do something in this case or something
}
// In both cases switch to listening for next preamble
qsp->protocolState = IDLE;
}
}
/**
* Encode frame is corrent format and write to hardware
*/
void qspEncodeFrame(QspConfiguration_t *qsp) {
//Zero CRC
qsp->crc = 0;
//Write preamble
qsp->hardwareWriteFunction(QSP_PREAMBLE, qsp);
//Write CHANNEL_ID
qsp->hardwareWriteFunction(CHANNEL_ID, qsp);
//Write frame type and length
uint8_t data = qsp->payloadLength & 0x0f;
data |= (qsp->frameToSend << 4) & 0xf0;
qsp->hardwareWriteFunction(data, qsp);
//Write packet ID
qsp->hardwareWriteFunction(qspGetPacketId(), qsp);
//Write payload
for (uint8_t i = 0; i < qsp->payloadLength; i++)
{
qsp->hardwareWriteFunction(qsp->payload[i], qsp);
}
//Finally write CRC
qsp->hardwareWriteFunction(qsp->crc, qsp);
}

10
qsp.h
View File

@@ -1,8 +1,12 @@
#include "Arduino.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(uint8_t payload[], int output[]);
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, int output[]);
uint8_t get10bitHighShift(uint8_t channel);
uint8_t get10bitLowShift(uint8_t channel);
void qspComputeCrc(uint8_t *crc, uint8_t dataByte);
void encodeRcDataPayload(PPMReader *ppmSource, uint8_t noOfChannels, uint8_t payload[], uint8_t *payloadLength);
void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels);
uint8_t qspGetPacketId(void);
void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int ppm[]);
void qspClearPayload(QspConfiguration_t *qsp);
void qspEncodeFrame(QspConfiguration_t *qsp);

9
variables.h
View File

@@ -35,3 +35,12 @@ enum dataStates {
#define PPM_PULSE_LENGTH 300 //set the pulse length
#define PPM_SIGNAL_POSITIVE_STATE 1 //set polarity of the pulses: 1 is positive, 0 is negative
#define PPM_OUTPUT_PIN 10 //set PPM signal output pin on the arduino
struct QspConfiguration_t {
uint8_t protocolState = IDLE;
uint8_t crc = 0;
uint8_t payload[QSP_PAYLOAD_LENGTH] = {0};
uint8_t payloadLength = 0;
uint8_t frameToSend = 0;
void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
};