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

refactoring

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2017-10-07 14:53:50 +02:00
parent 5b5ca9fedf
commit 3deec452ef
3 changed files with 263 additions and 193 deletions
Download Patch File Download Diff File Expand all files Collapse all files

262
crossbow.ino
View File

@@ -1,11 +1,14 @@
#include <LoRa.h>
#include "variables.h"
#include "qsp.h"
// #define LORA_HARDWARE_SERIAL
#define LORA_HARDWARE_SPI
// #define DEVICE_MODE_TX
#define DEVICE_MODE_RX
#define DEVICE_MODE_TX
// #define DEVICE_MODE_RX
int ppm[PPM_CHANNEL_COUNT] = {0};
/*
* Main defines for device working in TX mode
@@ -29,8 +32,6 @@ bool canTransmit = true;
#define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET);
int ppm[PPM_CHANNEL_COUNT] = {0};
bool canTransmit = false;
#endif
@@ -39,88 +40,63 @@ bool canTransmit = false;
* Start of QSP protocol implementation
*/
static uint8_t protocolState = IDLE;
static uint8_t packetId = 0;
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() {
uint8_t qspGetPacketId()
{
static uint8_t packetId = 0;
return packetId++;
}
void qspClearPayload() {
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) {
void qspClearPayload()
{
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
{
qspPayload[i] = 0;
}
qspPayloadLength = 0;
}
void qspDecodeRcDataFrame() {
int temporaryPpmOutput[PPM_CHANNEL_COUNT] = {0};
//TODO fix it, baby :)
temporaryPpmOutput[0] = (uint16_t) (((uint16_t) qspPayload[0] << 2) & 0x3fc) | ((qspPayload[1] >> 6) & 0x03);
temporaryPpmOutput[1] = (uint16_t) (((uint16_t) qspPayload[1] << 4) & 0x3f0) | ((qspPayload[2] >> 4) & 0x0F);
temporaryPpmOutput[2] = (uint16_t) (((uint16_t) qspPayload[2] << 6) & 0x3c0) | ((qspPayload[3] >> 2) & 0x3F);
temporaryPpmOutput[3] = (uint16_t) (((uint16_t) qspPayload[3] << 8) & 0x300) | ((qspPayload[4] >> 2) & 0xFF);
temporaryPpmOutput[4] = qspPayload[5];
temporaryPpmOutput[5] = qspPayload[6];
temporaryPpmOutput[6] = (qspPayload[7] >> 4) & 0b00001111;
temporaryPpmOutput[7] = qspPayload[7] & 0b00001111;
temporaryPpmOutput[8] = (qspPayload[8] >> 4) & 0b00001111;
temporaryPpmOutput[9] = qspPayload[8] & 0b00001111;
//10bit channels
temporaryPpmOutput[0] = map(temporaryPpmOutput[0], 0, 1000, 1000, 2000);
temporaryPpmOutput[1] = map(temporaryPpmOutput[1], 0, 1000, 1000, 2000);
temporaryPpmOutput[2] = map(temporaryPpmOutput[2], 0, 1000, 1000, 2000);
temporaryPpmOutput[3] = map(temporaryPpmOutput[3], 0, 1000, 1000, 2000);
//8bit channels
temporaryPpmOutput[4] = map(temporaryPpmOutput[4], 0, 0xff, 1000, 2000);
temporaryPpmOutput[5] = map(temporaryPpmOutput[5], 0, 0xff, 1000, 2000);
//4bit channels
temporaryPpmOutput[6] = map(temporaryPpmOutput[6], 0, 0x0f, 1000, 2000);
temporaryPpmOutput[7] = map(temporaryPpmOutput[7], 0, 0x0f, 1000, 2000);
temporaryPpmOutput[8] = map(temporaryPpmOutput[8], 0, 0x0f, 1000, 2000);
temporaryPpmOutput[9] = map(temporaryPpmOutput[9], 0, 0x0f, 1000, 2000);
/*
* Copy tremporary to real output
*/
for (uint8_t i = 0; i < PPM_CHANNEL_COUNT; i++) {
ppm[i] = temporaryPpmOutput[i];
}
}
void qspDecodeIncomingFrame(uint8_t incomingByte) {
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 (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) {
}
else if (protocolState == PREAMBLE_RECEIVED)
{
// Check if incomming channel ID is the same as receiver
if (incomingByte == CHANNEL_ID) {
if (incomingByte == CHANNEL_ID)
{
protocolState = CHANNEL_RECEIVED;
qspCrc ^= incomingByte;
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) {
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
{
qspPayload[i] = 0x00;
}
receivedPayload = 0;
packetId = 0;
} else {
}
else
{
protocolState = IDLE;
}
} else if (protocolState == CHANNEL_RECEIVED) {
}
else if (protocolState == CHANNEL_RECEIVED)
{
//Frame ID and payload length
qspCrc ^= incomingByte;
@@ -128,12 +104,15 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
payloadLength = incomingByte & 0x0f;
protocolState = FRAME_TYPE_RECEIVED;
} else if (protocolState == FRAME_TYPE_RECEIVED) {
}
else if (protocolState == FRAME_TYPE_RECEIVED)
{
qspCrc ^= incomingByte;
packetId = incomingByte;
protocolState = PACKET_ID_RECEIVED;
} else if (protocolState == PACKET_ID_RECEIVED) {
}
else if (protocolState == PACKET_ID_RECEIVED)
{
//Now it's time for payload
qspCrc ^= incomingByte;
@@ -141,13 +120,16 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
receivedPayload++;
if (receivedPayload == payloadLength) {
if (receivedPayload == payloadLength)
{
protocolState = PAYLOAD_RECEIVED;
}
}
else if (protocolState == PAYLOAD_RECEIVED)
{
} else if (protocolState == PAYLOAD_RECEIVED) {
if (qspCrc == incomingByte) {
if (qspCrc == incomingByte)
{
//CRC is correct
#ifdef DEVICE_MODE_RX
@@ -155,9 +137,10 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
canTransmit = true;
#endif
switch (frameId) {
switch (frameId)
{
case QSP_FRAME_RC_DATA:
qspDecodeRcDataFrame();
qspDecodeRcDataFrame(qspPayload, ppm);
break;
default:
@@ -165,8 +148,9 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
//TODO do something in this case
break;
}
} else {
}
else
{
//CRC failed, frame has to be rejected
//TODO do something in this case or something
}
@@ -174,10 +158,10 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
// In both cases switch to listening for next preamble
protocolState = IDLE;
}
}
void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) {
void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t payload[])
{
//Zero CRC
qspCrc = 0;
@@ -195,7 +179,8 @@ void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) {
writeToRadio(qspGetPacketId());
//Write payload
for (uint8_t i = 0; i < length; i++) {
for (uint8_t i = 0; i < length; i++)
{
writeToRadio(payload[i]);
}
@@ -209,44 +194,36 @@ void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) {
static uint32_t lastRcFrameTransmit = 0;
uint8_t get10bitHighShift(uint8_t channel) {
return ((channel % 4) * 2) + 2;
}
uint8_t get10bitLowShift(uint8_t channel) {
return 8 - get10bitHighShift(channel);
}
void computeCrc(uint8_t dataByte) {
qspCrc ^= dataByte;
}
/*
* Serial port used to send data
*/
#ifdef LORA_HARDWARE_SERIAL
int getRadioRssi(void) {
int getRadioRssi(void)
{
return 0;
}
int getRadioSnr(void) {
float getRadioSnr(void)
{
return 0;
}
void radioPacketStart(void) {
void radioPacketStart(void)
{
}
void radioPacketEnd(void) {
void radioPacketEnd(void)
{
Serial.end();
delay(E45_TTL_100_UART_DOWNTIME);
Serial.begin(UART_SPEED);
}
void writeToRadio(uint8_t dataByte) {
void writeToRadio(uint8_t dataByte)
{
//Compute CRC
computeCrc(dataByte);
qspComputeCrc(&qspCrc, dataByte);
//Write to radio
Serial.write(dataByte);
@@ -256,25 +233,30 @@ void writeToRadio(uint8_t dataByte) {
#ifdef LORA_HARDWARE_SPI
int getRadioRssi(void) {
int getRadioRssi(void)
{
return LoRa.packetRssi();
}
int getRadioSnr(void) {
float getRadioSnr(void)
{
return LoRa.packetSnr();
}
void radioPacketStart(void) {
void radioPacketStart(void)
{
LoRa.beginPacket();
}
void radioPacketEnd(void) {
void radioPacketEnd(void)
{
LoRa.endPacket();
}
void writeToRadio(uint8_t dataByte) {
void writeToRadio(uint8_t dataByte)
{
//Compute CRC
computeCrc(dataByte);
qspComputeCrc(&qspCrc, dataByte);
//Write to radio
LoRa.write(dataByte);
@@ -290,16 +272,19 @@ display.print(remoteData.latitude);
display.display();
*/
void setup(void) {
void setup(void)
{
#ifdef LORA_HARDWARE_SERIAL
Serial.begin(UART_SPEED);
#endif
#ifdef LORA_HARDWARE_SPI
if (!LoRa.begin(868E6)) {
if (!LoRa.begin(868E6))
{
Serial.println("LoRa init failed. Check your connections.");
while (true);
while (true)
;
}
LoRa.onReceive(onReceive);
LoRa.receive();
@@ -318,7 +303,8 @@ void setup(void) {
display.display();
//initiallize default ppm values
for(int i=0; i<PPM_CHANNEL_COUNT; i++){
for (int i = 0; i < PPM_CHANNEL_COUNT; i++)
{
ppm[i] = PPM_CHANNEL_DEFAULT_VALUE;
}
@@ -338,73 +324,37 @@ void setup(void) {
#endif
}
#ifdef DEVICE_MODE_TX
/**
* Encode 10 RC channels
*/
void encodeRcDataPayload(PPMReader* ppmSource, uint8_t noOfChannels) {
for (uint8_t i = 0; i < noOfChannels; i++) {
uint16_t channelValue10 = map(ppmSource->get(i), 1000, 2000, 0, 1000) & 0x03ff;
uint8_t channelValue8 = map(ppmSource->get(i), 1000, 2000, 0, 255) & 0xff;
uint8_t channelValue4 = map(ppmSource->get(i), 1000, 2000, 0, 15) & 0x0f;
if (i < 4) {
/*
* First 4 channels encoded with 10 bits
*/
uint8_t bitIndex = i + (i / 4);
qspPayload[bitIndex] |= (channelValue10 >> get10bitHighShift(i)) & (0x3ff >> get10bitHighShift(i));
qspPayload[bitIndex + 1] |= (channelValue10 << get10bitLowShift(i)) & 0xff << (8 - get10bitHighShift(i));
} else if (i == 4 || i == 5) {
/*
* Next 2 with 8 bits
*/
qspPayload[i + 1] |= channelValue8;
} else if (i == 6) {
/*
* And last 4 with 4 bits per channel
*/
qspPayload[7] |= (channelValue4 << 4) & B11110000;
} else if (i == 7) {
qspPayload[7] |= channelValue4 & B00001111;
} else if (i == 8) {
qspPayload[8] |= (channelValue4 << 4) & B11110000;
} else if (i == 9) {
qspPayload[8] |= channelValue4 & B00001111;
}
}
qspPayloadLength = 9;
}
#endif
#ifdef DEVICE_MODE_RX
ISR(TIMER1_COMPA_vect){ //leave this alone
ISR(TIMER1_COMPA_vect)
{ //leave this alone
static boolean state = true;
TCNT1 = 0;
if (state) { //start pulse
if (state)
{ //start pulse
digitalWrite(PPM_OUTPUT_PIN, PPM_SIGNAL_POSITIVE_STATE);
OCR1A = PPM_PULSE_LENGTH * 2;
state = false;
} else{ //end pulse and calculate when to start the next pulse
}
else
{ //end pulse and calculate when to start the next pulse
static byte cur_chan_numb;
static unsigned int calc_rest;
digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE);
state = true;
if(cur_chan_numb >= PPM_CHANNEL_COUNT){
if (cur_chan_numb >= PPM_CHANNEL_COUNT)
{
cur_chan_numb = 0;
calc_rest = calc_rest + PPM_PULSE_LENGTH; //
OCR1A = (PPM_FRAME_LENGTH - calc_rest) * 2;
calc_rest = 0;
}
else{
else
{
OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * 2;
calc_rest = calc_rest + ppm[cur_chan_numb];
cur_chan_numb++;
@@ -414,7 +364,8 @@ ISR(TIMER1_COMPA_vect){ //leave this alone
#endif
void loop(void) {
void loop(void)
{
bool transmitPayload = false;
@@ -426,11 +377,12 @@ 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 && protocolState == IDLE)
{
lastRcFrameTransmit = currentMillis;
qspClearPayload();
encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT);
encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT, qspPayload, &qspPayloadLength);
qspFrameToSend = QSP_FRAME_RC_DATA;
transmitPayload = true;
@@ -439,26 +391,30 @@ void loop(void) {
#endif
#ifdef LORA_HARDWARE_SERIAL
if (Serial.available()) {
if (Serial.available())
{
qspDecodeIncomingFrame(Serial.read());
}
#endif
if (canTransmit && transmitPayload) {
if (canTransmit && transmitPayload)
{
transmitPayload = false;
radioPacketStart();
qspEncodeFrame(qspFrameToSend, qspPayloadLength, qspPayload);
radioPacketEnd();
}
}
#ifdef LORA_HARDWARE_SPI
void onReceive(int packetSize) {
if (packetSize == 0) return;
void onReceive(int packetSize)
{
if (packetSize == 0)
return;
while (LoRa.available()) {
while (LoRa.available())
{
qspDecodeIncomingFrame(LoRa.read());
}
}

106
qsp.cpp Normal file
View File

@@ -0,0 +1,106 @@
#include "Arduino.h"
#include "variables.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(uint8_t payload[], 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;
//10bit channels
temporaryPpmOutput[0] = map(temporaryPpmOutput[0], 0, 1000, 1000, 2000);
temporaryPpmOutput[1] = map(temporaryPpmOutput[1], 0, 1000, 1000, 2000);
temporaryPpmOutput[2] = map(temporaryPpmOutput[2], 0, 1000, 1000, 2000);
temporaryPpmOutput[3] = map(temporaryPpmOutput[3], 0, 1000, 1000, 2000);
//8bit channels
temporaryPpmOutput[4] = map(temporaryPpmOutput[4], 0, 0xff, 1000, 2000);
temporaryPpmOutput[5] = map(temporaryPpmOutput[5], 0, 0xff, 1000, 2000);
//4bit channels
temporaryPpmOutput[6] = map(temporaryPpmOutput[6], 0, 0x0f, 1000, 2000);
temporaryPpmOutput[7] = map(temporaryPpmOutput[7], 0, 0x0f, 1000, 2000);
temporaryPpmOutput[8] = map(temporaryPpmOutput[8], 0, 0x0f, 1000, 2000);
temporaryPpmOutput[9] = map(temporaryPpmOutput[9], 0, 0x0f, 1000, 2000);
/*
* Copy tremporary to real output
*/
for (uint8_t i = 0; i < PPM_CHANNEL_COUNT; i++) {
output[i] = temporaryPpmOutput[i];
}
}
uint8_t get10bitHighShift(uint8_t channel) {
return ((channel % 4) * 2) + 2;
}
uint8_t get10bitLowShift(uint8_t channel) {
return 8 - get10bitHighShift(channel);
}
void qspComputeCrc(uint8_t *crc, uint8_t dataByte)
{
*crc ^= dataByte;
}
/**
* Encode 10 RC channels
*/
void encodeRcDataPayload(PPMReader *ppmSource, uint8_t noOfChannels, uint8_t payload[], uint8_t *payloadLength)
{
for (uint8_t i = 0; i < noOfChannels; i++)
{
uint16_t channelValue10 = map(ppmSource->get(i), 1000, 2000, 0, 1000) & 0x03ff;
uint8_t channelValue8 = map(ppmSource->get(i), 1000, 2000, 0, 255) & 0xff;
uint8_t channelValue4 = map(ppmSource->get(i), 1000, 2000, 0, 15) & 0x0f;
if (i < 4)
{
/*
* 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));
}
else if (i == 4 || i == 5)
{
/*
* Next 2 with 8 bits
*/
payload[i + 1] |= channelValue8;
}
else if (i == 6)
{
/*
* And last 4 with 4 bits per channel
*/
payload[7] |= (channelValue4 << 4) & B11110000;
}
else if (i == 7)
{
payload[7] |= channelValue4 & B00001111;
}
else if (i == 8)
{
payload[8] |= (channelValue4 << 4) & B11110000;
}
else if (i == 9)
{
payload[8] |= channelValue4 & B00001111;
}
}
*payloadLength = 9;
}

8
qsp.h Normal file
View File

@@ -0,0 +1,8 @@
#include "Arduino.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(uint8_t payload[], 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);