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

342
crossbow.ino
View File

@@ -1,11 +1,14 @@
#include <LoRa.h> #include <LoRa.h>
#include "variables.h" #include "variables.h"
#include "qsp.h"
// #define LORA_HARDWARE_SERIAL // #define LORA_HARDWARE_SERIAL
#define LORA_HARDWARE_SPI #define LORA_HARDWARE_SPI
// #define DEVICE_MODE_TX #define DEVICE_MODE_TX
#define DEVICE_MODE_RX // #define DEVICE_MODE_RX
int ppm[PPM_CHANNEL_COUNT] = {0};
/* /*
* Main defines for device working in TX mode * Main defines for device working in TX mode
@@ -29,8 +32,6 @@ bool canTransmit = true;
#define OLED_RESET 4 #define OLED_RESET 4
Adafruit_SSD1306 display(OLED_RESET); Adafruit_SSD1306 display(OLED_RESET);
int ppm[PPM_CHANNEL_COUNT] = {0};
bool canTransmit = false; bool canTransmit = false;
#endif #endif
@@ -39,88 +40,63 @@ bool canTransmit = false;
* Start of QSP protocol implementation * Start of QSP protocol implementation
*/ */
static uint8_t protocolState = IDLE; static uint8_t protocolState = IDLE;
static uint8_t packetId = 0;
static uint8_t qspCrc = 0; static uint8_t qspCrc = 0;
static uint8_t qspPayload[QSP_PAYLOAD_LENGTH] = {0}; static uint8_t qspPayload[QSP_PAYLOAD_LENGTH] = {0};
static uint8_t qspPayloadLength = 0; static uint8_t qspPayloadLength = 0;
static uint8_t qspFrameToSend = 0; static uint8_t qspFrameToSend = 0;
uint8_t qspGetPacketId() { uint8_t qspGetPacketId()
{
static uint8_t packetId = 0;
return packetId++; return packetId++;
} }
void qspClearPayload() { void qspClearPayload()
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++) { {
for (uint8_t i = 0; i < QSP_PAYLOAD_LENGTH; i++)
{
qspPayload[i] = 0; qspPayload[i] = 0;
} }
qspPayloadLength = 0; qspPayloadLength = 0;
} }
void qspDecodeRcDataFrame() { void qspDecodeIncomingFrame(uint8_t incomingByte)
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) {
static uint8_t frameId; static uint8_t frameId;
static uint8_t payloadLength; static uint8_t payloadLength;
static uint8_t receivedPayload; static uint8_t receivedPayload;
static uint8_t packetId; //TODO move this to global scope maybe? 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 //If in IDLE and correct preamble comes, start to decode frame
protocolState = PREAMBLE_RECEIVED; protocolState = PREAMBLE_RECEIVED;
qspCrc = 0 ^ incomingByte; qspCrc = 0 ^ incomingByte;
} else if (protocolState == PREAMBLE_RECEIVED) { }
else if (protocolState == PREAMBLE_RECEIVED)
{
// Check if incomming channel ID is the same as receiver // Check if incomming channel ID is the same as receiver
if (incomingByte == CHANNEL_ID) { if (incomingByte == CHANNEL_ID)
{
protocolState = CHANNEL_RECEIVED; protocolState = CHANNEL_RECEIVED;
qspCrc ^= incomingByte; 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; qspPayload[i] = 0x00;
} }
receivedPayload = 0; receivedPayload = 0;
packetId = 0; packetId = 0;
} else { }
else
{
protocolState = IDLE; protocolState = IDLE;
} }
} else if (protocolState == CHANNEL_RECEIVED) { }
else if (protocolState == CHANNEL_RECEIVED)
{
//Frame ID and payload length //Frame ID and payload length
qspCrc ^= incomingByte; qspCrc ^= incomingByte;
@@ -128,12 +104,15 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
payloadLength = incomingByte & 0x0f; payloadLength = incomingByte & 0x0f;
protocolState = FRAME_TYPE_RECEIVED; protocolState = FRAME_TYPE_RECEIVED;
}
} else if (protocolState == FRAME_TYPE_RECEIVED) { else if (protocolState == FRAME_TYPE_RECEIVED)
{
qspCrc ^= incomingByte; qspCrc ^= incomingByte;
packetId = incomingByte; packetId = incomingByte;
protocolState = PACKET_ID_RECEIVED; protocolState = PACKET_ID_RECEIVED;
} else if (protocolState == PACKET_ID_RECEIVED) { }
else if (protocolState == PACKET_ID_RECEIVED)
{
//Now it's time for payload //Now it's time for payload
qspCrc ^= incomingByte; qspCrc ^= incomingByte;
@@ -141,32 +120,37 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
receivedPayload++; receivedPayload++;
if (receivedPayload == payloadLength) { if (receivedPayload == payloadLength)
protocolState = PAYLOAD_RECEIVED; {
protocolState = PAYLOAD_RECEIVED;
} }
}
else if (protocolState == PAYLOAD_RECEIVED)
{
} else if (protocolState == PAYLOAD_RECEIVED) { if (qspCrc == incomingByte)
{
//CRC is correct
if (qspCrc == incomingByte) { #ifdef DEVICE_MODE_RX
//CRC is correct
#ifdef DEVICE_MODE_RX
//If devide received a valid frame, that means it can start to talk //If devide received a valid frame, that means it can start to talk
canTransmit = true; canTransmit = true;
#endif #endif
switch (frameId) { switch (frameId)
case QSP_FRAME_RC_DATA: {
qspDecodeRcDataFrame(); case QSP_FRAME_RC_DATA:
break; qspDecodeRcDataFrame(qspPayload, ppm);
break;
default: default:
//Unknown frame //Unknown frame
//TODO do something in this case //TODO do something in this case
break; break;
} }
}
} else { else
{
//CRC failed, frame has to be rejected //CRC failed, frame has to be rejected
//TODO do something in this case or something //TODO do something in this case or something
} }
@@ -174,13 +158,13 @@ void qspDecodeIncomingFrame(uint8_t incomingByte) {
// In both cases switch to listening for next preamble // In both cases switch to listening for next preamble
protocolState = IDLE; 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 //Zero CRC
qspCrc = 0; qspCrc = 0;
//Write preamble //Write preamble
writeToRadio(QSP_PREAMBLE); writeToRadio(QSP_PREAMBLE);
//Write CHANNEL_ID //Write CHANNEL_ID
@@ -195,7 +179,8 @@ void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) {
writeToRadio(qspGetPacketId()); writeToRadio(qspGetPacketId());
//Write payload //Write payload
for (uint8_t i = 0; i < length; i++) { for (uint8_t i = 0; i < length; i++)
{
writeToRadio(payload[i]); writeToRadio(payload[i]);
} }
@@ -209,44 +194,36 @@ void qspEncodeFrame(uint8_t frameId, uint8_t length, uint8_t *payload) {
static uint32_t lastRcFrameTransmit = 0; 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 * Serial port used to send data
*/ */
#ifdef LORA_HARDWARE_SERIAL #ifdef LORA_HARDWARE_SERIAL
int getRadioRssi(void) { int getRadioRssi(void)
{
return 0; return 0;
} }
int getRadioSnr(void) { float getRadioSnr(void)
{
return 0; return 0;
} }
void radioPacketStart(void) { void radioPacketStart(void)
{
} }
void radioPacketEnd(void) { void radioPacketEnd(void)
{
Serial.end(); Serial.end();
delay(E45_TTL_100_UART_DOWNTIME); delay(E45_TTL_100_UART_DOWNTIME);
Serial.begin(UART_SPEED); Serial.begin(UART_SPEED);
} }
void writeToRadio(uint8_t dataByte) { void writeToRadio(uint8_t dataByte)
{
//Compute CRC //Compute CRC
computeCrc(dataByte); qspComputeCrc(&qspCrc, dataByte);
//Write to radio //Write to radio
Serial.write(dataByte); Serial.write(dataByte);
@@ -256,25 +233,30 @@ void writeToRadio(uint8_t dataByte) {
#ifdef LORA_HARDWARE_SPI #ifdef LORA_HARDWARE_SPI
int getRadioRssi(void) { int getRadioRssi(void)
{
return LoRa.packetRssi(); return LoRa.packetRssi();
} }
int getRadioSnr(void) { float getRadioSnr(void)
{
return LoRa.packetSnr(); return LoRa.packetSnr();
} }
void radioPacketStart(void) { void radioPacketStart(void)
{
LoRa.beginPacket(); LoRa.beginPacket();
} }
void radioPacketEnd(void) { void radioPacketEnd(void)
{
LoRa.endPacket(); LoRa.endPacket();
} }
void writeToRadio(uint8_t dataByte) { void writeToRadio(uint8_t dataByte)
{
//Compute CRC //Compute CRC
computeCrc(dataByte); qspComputeCrc(&qspCrc, dataByte);
//Write to radio //Write to radio
LoRa.write(dataByte); LoRa.write(dataByte);
@@ -290,16 +272,19 @@ display.print(remoteData.latitude);
display.display(); display.display();
*/ */
void setup(void) { void setup(void)
{
#ifdef LORA_HARDWARE_SERIAL #ifdef LORA_HARDWARE_SERIAL
Serial.begin(UART_SPEED); Serial.begin(UART_SPEED);
#endif #endif
#ifdef LORA_HARDWARE_SPI #ifdef LORA_HARDWARE_SPI
if (!LoRa.begin(868E6)) { if (!LoRa.begin(868E6))
{
Serial.println("LoRa init failed. Check your connections."); Serial.println("LoRa init failed. Check your connections.");
while (true); while (true)
;
} }
LoRa.onReceive(onReceive); LoRa.onReceive(onReceive);
LoRa.receive(); LoRa.receive();
@@ -311,110 +296,76 @@ void setup(void) {
/* /*
* Initialize OLED display * Initialize OLED display
*/ */
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
display.setTextSize(1); display.setTextSize(1);
display.setTextColor(WHITE); display.setTextColor(WHITE);
display.clearDisplay(); display.clearDisplay();
display.display(); display.display();
//initiallize default ppm values //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; {
ppm[i] = PPM_CHANNEL_DEFAULT_VALUE;
} }
pinMode(PPM_OUTPUT_PIN, OUTPUT); pinMode(PPM_OUTPUT_PIN, OUTPUT);
digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE); //set the PPM signal pin to the default state (off) digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE); //set the PPM signal pin to the default state (off)
cli(); cli();
TCCR1A = 0; // set entire TCCR1 register to 0 TCCR1A = 0; // set entire TCCR1 register to 0
TCCR1B = 0; TCCR1B = 0;
OCR1A = 100; // compare match register, change this OCR1A = 100; // compare match register, change this
TCCR1B |= (1 << WGM12); // turn on CTC mode TCCR1B |= (1 << WGM12); // turn on CTC mode
TCCR1B |= (1 << CS11); // 8 prescaler: 0,5 microseconds at 16mhz TCCR1B |= (1 << CS11); // 8 prescaler: 0,5 microseconds at 16mhz
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
sei(); sei();
#endif #endif
} }
#ifdef DEVICE_MODE_TX #ifdef DEVICE_MODE_RX
/** ISR(TIMER1_COMPA_vect)
* Encode 10 RC channels { //leave this alone
*/ static boolean state = true;
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) { TCNT1 = 0;
/*
* First 4 channels encoded with 10 bits if (state)
*/ { //start pulse
uint8_t bitIndex = i + (i / 4); digitalWrite(PPM_OUTPUT_PIN, PPM_SIGNAL_POSITIVE_STATE);
qspPayload[bitIndex] |= (channelValue10 >> get10bitHighShift(i)) & (0x3ff >> get10bitHighShift(i)); OCR1A = PPM_PULSE_LENGTH * 2;
qspPayload[bitIndex + 1] |= (channelValue10 << get10bitLowShift(i)) & 0xff << (8 - get10bitHighShift(i)); state = false;
} else if (i == 4 || i == 5) { }
/* else
* Next 2 with 8 bits { //end pulse and calculate when to start the next pulse
*/ static byte cur_chan_numb;
qspPayload[i + 1] |= channelValue8; static unsigned int calc_rest;
} else if (i == 6) {
/* digitalWrite(PPM_OUTPUT_PIN, !PPM_SIGNAL_POSITIVE_STATE);
* And last 4 with 4 bits per channel state = true;
*/
qspPayload[7] |= (channelValue4 << 4) & B11110000; if (cur_chan_numb >= PPM_CHANNEL_COUNT)
} else if (i == 7) { {
qspPayload[7] |= channelValue4 & B00001111; cur_chan_numb = 0;
} else if (i == 8) { calc_rest = calc_rest + PPM_PULSE_LENGTH; //
qspPayload[8] |= (channelValue4 << 4) & B11110000; OCR1A = (PPM_FRAME_LENGTH - calc_rest) * 2;
} else if (i == 9) { calc_rest = 0;
qspPayload[8] |= channelValue4 & B00001111; }
else
{
OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * 2;
calc_rest = calc_rest + ppm[cur_chan_numb];
cur_chan_numb++;
} }
} }
qspPayloadLength = 9;
} }
#endif #endif
#ifdef DEVICE_MODE_RX void loop(void)
{
ISR(TIMER1_COMPA_vect){ //leave this alone
static boolean state = true;
TCNT1 = 0;
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
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){
cur_chan_numb = 0;
calc_rest = calc_rest + PPM_PULSE_LENGTH;//
OCR1A = (PPM_FRAME_LENGTH - calc_rest) * 2;
calc_rest = 0;
}
else{
OCR1A = (ppm[cur_chan_numb] - PPM_PULSE_LENGTH) * 2;
calc_rest = calc_rest + ppm[cur_chan_numb];
cur_chan_numb++;
}
}
}
#endif
void loop(void) {
bool transmitPayload = false; bool transmitPayload = false;
@@ -422,15 +373,16 @@ void loop(void) {
uint32_t currentMillis = millis(); uint32_t currentMillis = millis();
//TODO It should be only possible to transmit when radio is not receiveing //TODO It should be only possible to transmit when radio is not receiveing
/* /*
* RC_DATA QSP frame * 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; {
lastRcFrameTransmit = currentMillis;
qspClearPayload(); qspClearPayload();
encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT); encodeRcDataPayload(&ppmReader, PPM_CHANNEL_COUNT, qspPayload, &qspPayloadLength);
qspFrameToSend = QSP_FRAME_RC_DATA; qspFrameToSend = QSP_FRAME_RC_DATA;
transmitPayload = true; transmitPayload = true;
@@ -438,27 +390,31 @@ void loop(void) {
#endif #endif
#ifdef LORA_HARDWARE_SERIAL #ifdef LORA_HARDWARE_SERIAL
if (Serial.available()) { if (Serial.available())
{
qspDecodeIncomingFrame(Serial.read()); qspDecodeIncomingFrame(Serial.read());
} }
#endif #endif
if (canTransmit && transmitPayload) { if (canTransmit && transmitPayload)
{
transmitPayload = false; transmitPayload = false;
radioPacketStart(); radioPacketStart();
qspEncodeFrame(qspFrameToSend, qspPayloadLength, qspPayload); qspEncodeFrame(qspFrameToSend, qspPayloadLength, qspPayload);
radioPacketEnd(); radioPacketEnd();
} }
} }
#ifdef LORA_HARDWARE_SPI #ifdef LORA_HARDWARE_SPI
void onReceive(int packetSize) { void onReceive(int packetSize)
if (packetSize == 0) return; {
if (packetSize == 0)
return;
while (LoRa.available()) { while (LoRa.available())
{
qspDecodeIncomingFrame(LoRa.read()); 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);