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

Merge branch 'master' of github.com:DzikuVx/QuadMeUp_Crossbow

Browse Source
This commit is contained in:
Pawel Spychalski
2017-11-01 10:18:53 +01:00
parent 2af594137f 5ae986112f
commit 4641d974ac
12 changed files with 550 additions and 218 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
.vscode/arduino.json vendored
View File

@@ -1,6 +1,6 @@
{
"board": "bsfrance:avr:lora32u4",
"sketch": "crossbow.ino",
"port": "COM17",
"port": "COM15",
"output": "./build"
}

16
README.md
View File

@@ -13,9 +13,8 @@ Development, not yet functional
| 1 | Preamble | "Q" 0x51 |
| 2 | Channel ID | channel used for comunication between TX and RX |
| 3 | Frame type & Length | bits 7-5 defines frame, bits 4-0 payload length |
| 4 | Packet ID | |
| 5 - 36 | Payload | 32 bytes max |
| payload length + 5 | CRC | XOR of all previous bytes |
| payload length + 4 | CRC | XOR of all previous bytes |
## Frame types
@@ -26,8 +25,8 @@ Development, not yet functional
| 0010 | 0x2 | Request receiver configuration | TX -> RX |
| 0011 | 0x3 | Receiver configuration | RX -> TX |
| 0100 | 0x4 | Set receiver configuration | TX -> RX |
| 0101 | 0x5 | PING frame | TX -> RX |
| 0110 | 0x6 | PONG frame | RX -> TX |
| 0101 | 0x5 | PING frame, uses 9 byte payload | TX -> RX |
| 0110 | 0x6 | PONG frame, the same payload as PING | RX -> TX |
### `RC_DATA` frame format
@@ -48,7 +47,14 @@ Total length of `RC_DATA` payload is 9 bytes
| 3 | RX supply volatage, sent in 0,1V |
| 4 | RX analog input 1 sent in 0,1V |
| 5 | RX analog input 2 sent in 0,1V |
| 6 | Last received packet ID |
| 6 | Flags |
#### Flags
| Bit | Meaning |
| ---- | ---- |
| 00000001 | Device in Failsafe mode |
### `PING` and `PONG` frames

430
crossbow.ino
View File

@@ -1,38 +1,43 @@
#define LORA_HARDWARE_SPI
// #define DEVICE_MODE_TX
#define DEVICE_MODE_RX
#define FEATURE_TX_OLED
// #define DEBUG_SERIAL
// #define DEBUG_PING_PONG
// #define DEBUG_LED
// #define WAIT_FOR_SERIAL
#include <LoRa.h>
// #include <PinChangeInterrupt.h>
#include "variables.h"
#include "sbus.h"
#include "qsp.h"
volatile int ppm[16] = {0};
// LoRa32u4 ports
#define LORA32U4_SS_PIN 8
#define LORA32U4_RST_PIN 4
#define LORA32U4_DI0_PIN 7
int ppm[16] = {0};
/*
* Main defines for device working in TX mode
*/
#ifdef DEVICE_MODE_TX
// #define OLED_RESET -1
#include <PPMReader.h>
// #include <Adafruit_SSD1306.h>
PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
// PPMReader ppmReader(11, 2, MODE_PIN_CHANGE_INTERRUPT);
// Adafruit_SSD1306 display(OLED_RESET);
#include "txbuzzer.h"
BuzzerState_t buzzer;
#ifdef FEATURE_TX_OLED
#define OLED_RESET -1
#include <Adafruit_SSD1306.h>
Adafruit_SSD1306 display(OLED_RESET);
uint32_t lastOledTaskTime = 0;
#endif
#endif
@@ -40,6 +45,9 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
* Main defines for device working in RX mode
*/
#ifdef DEVICE_MODE_RX
#include "sbus.h"
uint32_t sbusTime = 0;
uint8_t sbusPacket[SBUS_PACKET_LENGTH] = {0};
uint32_t lastRxStateTaskTime = 0;
@@ -48,10 +56,10 @@ PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
/*
* Start of QSP protocol implementation
*/
QspConfiguration_t qsp = {};
RxDeviceState_t rxDeviceState = {};
volatile QspConfiguration_t qsp = {};
volatile RxDeviceState_t rxDeviceState = {};
volatile TxDeviceState_t txDeviceState = {};
#ifdef LORA_HARDWARE_SPI
uint8_t getRadioRssi(void)
{
@@ -59,9 +67,9 @@ uint8_t getRadioRssi(void)
return map(constrain(LoRa.packetRssi() * -1, 0, 164), 0, 164, 255, 0);
}
float getRadioSnr(void)
uint8_t getRadioSnr(void)
{
return (uint8_t) constrain(LoRa.packetSnr() * 10, 0, 255);
return (uint8_t) constrain(LoRa.packetSnr(), 0, 255);
}
void radioPacketStart(void)
@@ -85,16 +93,6 @@ void writeToRadio(uint8_t dataByte, QspConfiguration_t *qsp)
LoRa.write(dataByte);
}
#endif
/*
display.clearDisplay();
display.setCursor(0,0);
display.print("Lat:");
display.print(remoteData.latitude);
display.display();
*/
void setup(void)
{
#ifdef DEBUG_SERIAL
@@ -109,8 +107,6 @@ void setup(void)
qsp.deviceState = DEVICE_STATE_OK;
#endif
#ifdef LORA_HARDWARE_SPI
#ifdef WAIT_FOR_SERIAL
while (!Serial) {
; // wait for serial port to connect. Needed for native USB
@@ -134,24 +130,14 @@ void setup(void)
while (true);
}
LoRa.setSignalBandwidth(250E3);
LoRa.setSpreadingFactor(7);
LoRa.setCodingRate4(5);
LoRa.setSignalBandwidth(500E3);
LoRa.setSpreadingFactor(8);
LoRa.setCodingRate4(6);
LoRa.enableCrc();
LoRa.onReceive(onReceive);
LoRa.receive();
#endif
#ifdef DEVICE_MODE_RX
/*
* Initialize OLED display
*/
// display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
// display.setTextSize(1);
// display.setTextColor(WHITE);
// display.clearDisplay();
// display.display();
//initiallize default ppm values
for (int i = 0; i < 16; i++)
{
@@ -169,17 +155,28 @@ void setup(void)
TCCR1A = 0; //reset timer1
TCCR1B = 0;
TCCR1B |= (1 << CS11); //set timer1 to increment every 0,5 us or 1us on 8MHz
#ifdef FEATURE_TX_OLED
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
display.setTextSize(1);
display.setTextColor(WHITE);
display.clearDisplay();
display.display();
#endif
/*
* TX should start talking imediately after power up
*/
qsp.canTransmit = true;
pinMode(TX_BUZZER_PIN, OUTPUT);
//Play single tune to indicate power up
buzzerSingleMode(BUZZER_MODE_CHIRP, &buzzer);
#endif
pinMode(LED_BUILTIN, OUTPUT);
/*
* TX should start talking imediately after power up
*/
#ifdef DEVICE_MODE_TX
qsp.canTransmit = true;
#endif
#ifdef DEBUG_SERIAL
qsp.debugConfig |= DEBUG_FLAG_SERIAL;
#endif
@@ -189,121 +186,294 @@ void setup(void)
}
int8_t txSendSequence[16] = {
QSP_FRAME_PING,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA,
QSP_FRAME_RC_DATA
};
int8_t rxSendSequence[16] = {
QSP_FRAME_RX_HEALTH,
-1,
-1,
-1,
QSP_FRAME_RX_HEALTH,
-1,
-1,
-1,
QSP_FRAME_RX_HEALTH,
-1,
-1,
-1,
QSP_FRAME_RX_HEALTH,
-1,
-1,
-1
};
uint8_t currentSequenceIndex = 0;
#define TRANSMIT_SEQUENCE_COUNT 16
#ifdef DEVICE_MODE_RX
void updateRxDeviceState(RxDeviceState_t *rxDeviceState) {
rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255);
rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255);
rxDeviceState->a2Voltage = map(analogRead(RX_ADC_PIN_3), 0, 1024, 0, 255);
rxDeviceState->rssi = getRadioRssi();
rxDeviceState->snr = getRadioSnr();
}
int8_t getFrameToTransmit(QspConfiguration_t *qsp) {
if (qsp->forcePongFrame) {
qsp->forcePongFrame = false;
return QSP_FRAME_PONG;
}
int8_t retVal = rxSendSequence[currentSequenceIndex];
currentSequenceIndex++;
if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) {
currentSequenceIndex = 0;
}
return retVal;
}
#endif
#ifdef DEVICE_MODE_TX
int8_t getFrameToTransmit(QspConfiguration_t *qsp) {
int8_t retVal = txSendSequence[currentSequenceIndex];
currentSequenceIndex++;
if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) {
currentSequenceIndex = 0;
}
return retVal;
}
#endif
void loop(void)
{
#ifdef DEVICE_MODE_TX
if (txDeviceState.readPacket) {
int incomingByte = LoRa.read();
if (incomingByte > -1) {
qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState);
} else {
txDeviceState.rssi = getRadioRssi();
txDeviceState.snr = getRadioSnr();
txDeviceState.readPacket = false;
}
}
#endif
uint32_t currentMillis = millis();
bool transmitPayload = false;
static uint32_t previousAnyFrameReceivedAt = 0;
/*
* Watchdog for frame decoding stuck somewhere in the middle of a process
*/
if (
qsp.protocolState != QSP_STATE_IDLE &&
abs(currentMillis - qsp.frameDecodingStartedAt) > QSP_MAX_FRAME_DECODE_TIME
qsp.frameDecodingStartedAt + QSP_MAX_FRAME_DECODE_TIME < currentMillis
) {
qsp.protocolState = QSP_STATE_IDLE;
}
if (
qsp.forcePongFrame &&
!transmitPayload &&
qsp.protocolState == QSP_STATE_IDLE
)
{
qsp.forcePongFrame = false;
qsp.lastFrameTransmitedAt[QSP_FRAME_PONG] = currentMillis;
qsp.frameToSend = QSP_FRAME_PONG;
transmitPayload = true;
}
#ifdef DEVICE_MODE_TX
#ifdef DEBUG_PING_PONG
//PING frame
if (
currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_PING] > TX_PING_RATE &&
!transmitPayload &&
qsp.protocolState == QSP_STATE_IDLE
)
{
qsp.lastFrameTransmitedAt[QSP_FRAME_PING] = currentMillis;
qsp.protocolState == QSP_STATE_IDLE &&
qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
) {
int8_t frameToSend = getFrameToTransmit(&qsp);
if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) {
frameToSend = -1;
//FIXME uncomment to enable full Failsafe
}
if (frameToSend > -1) {
qsp.frameToSend = frameToSend;
qspClearPayload(&qsp);
switch (qsp.frameToSend) {
case QSP_FRAME_PING:
encodePingPayload(&qsp, micros());
qsp.frameToSend = QSP_FRAME_PING;
break;
case QSP_FRAME_RC_DATA:
encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT);
break;
}
transmitPayload = true;
}
#endif
/*
* RC_DATA QSP frame
*/
if (
currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] > TX_RC_FRAME_RATE &&
!transmitPayload &&
qsp.protocolState == QSP_STATE_IDLE
)
{
qsp.lastFrameTransmitedAt[QSP_FRAME_RC_DATA] = currentMillis;
qspClearPayload(&qsp);
encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT);
qsp.frameToSend = QSP_FRAME_RC_DATA;
transmitPayload = true;
qsp.lastTxSlotTimestamp = currentMillis;
}
#endif
#ifdef DEVICE_MODE_RX
if (currentMillis > sbusTime) {
sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
sbusTime = currentMillis + SBUS_UPDATE_RATE;
}
/*
* This routine updates RX device state and updates one of radio channels with RSSI value
*/
if (currentMillis - lastRxStateTaskTime > RX_TASK_HEALTH) {
if (lastRxStateTaskTime + RX_TASK_HEALTH < currentMillis) {
lastRxStateTaskTime = currentMillis;
updateRxDeviceState(&rxDeviceState);
ppm[RSSI_CHANNEL - 1] = map(rxDeviceState.rssi, 0, 255, 1000, 2000);
if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
digitalWrite(LED_BUILTIN, HIGH);
} else {
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
}
}
/*
* RX_HEALTH QSP frame
* Main routine to answer to TX module
*/
if (
currentMillis - qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] > RX_RX_HEALTH_FRAME_RATE &&
!transmitPayload &&
qsp.protocolState == QSP_STATE_IDLE
)
{
qsp.lastFrameTransmitedAt[QSP_FRAME_RX_HEALTH] = currentMillis;
if (qsp.transmitWindowOpen && qsp.protocolState == QSP_STATE_IDLE) {
qsp.transmitWindowOpen = false;
int8_t frameToSend = getFrameToTransmit(&qsp);
if (frameToSend > -1) {
qsp.frameToSend = frameToSend;
if (frameToSend != QSP_FRAME_PONG) {
qspClearPayload(&qsp);
}
switch (qsp.frameToSend) {
case QSP_FRAME_PONG:
/*
* Pong frame just responses with received payload
*/
break;
case QSP_FRAME_RX_HEALTH:
encodeRxHealthPayload(&qsp, &rxDeviceState);
qsp.frameToSend = QSP_FRAME_RX_HEALTH;
break;
}
transmitPayload = true;
}
}
if (currentMillis > sbusTime) {
sbusPreparePacket(sbusPacket, ppm, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
sbusTime = currentMillis + SBUS_UPDATE_RATE;
}
if (qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA] + RX_FAILSAFE_DELAY < currentMillis) {
qsp.deviceState = DEVICE_STATE_FAILSAFE;
} else {
qsp.deviceState = DEVICE_STATE_OK;
}
#endif
#ifdef DEVICE_MODE_TX
buzzerProcess(TX_BUZZER_PIN, currentMillis, &buzzer);
// This routing enables when TX starts to receive signal from RX for a first time or after
// failsafe
if (txDeviceState.isReceiving == false && qsp.anyFrameRecivedAt != 0) {
//TX module started to receive data
buzzerSingleMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer);
txDeviceState.isReceiving = true;
qsp.deviceState = DEVICE_STATE_OK;
}
//Here we detect failsafe state on TX module
if (
txDeviceState.isReceiving &&
qsp.anyFrameRecivedAt + TX_FAILSAFE_DELAY < currentMillis
) {
txDeviceState.isReceiving = false;
rxDeviceState.a1Voltage = 0;
rxDeviceState.a2Voltage = 0;
rxDeviceState.rxVoltage = 0;
rxDeviceState.rssi = 0;
rxDeviceState.snr = 0;
rxDeviceState.flags = 0;
txDeviceState.roundtrip = 0;
qsp.deviceState = DEVICE_STATE_FAILSAFE;
qsp.anyFrameRecivedAt = 0;
}
//FIXME rxDeviceState should be resetted also in RC_HEALT frame is not received in a long period
//Handle audible alarms
if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
//Failsafe detected by TX
buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer);
} else if (txDeviceState.isReceiving && (rxDeviceState.flags & 0x1) == 1) {
//Failsafe reported by RX module
buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer);
} else if (txDeviceState.isReceiving && txDeviceState.rssi < 100) {
buzzerContinousMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer);
} else if (txDeviceState.isReceiving && txDeviceState.rssi < 128) {
buzzerContinousMode(BUZZER_MODE_CHIRP, &buzzer);
} else {
buzzerContinousMode(BUZZER_MODE_OFF, &buzzer);
}
#ifdef FEATURE_TX_OLED
if (
currentMillis - lastOledTaskTime > OLED_UPDATE_RATE
) {
lastOledTaskTime = currentMillis;
display.clearDisplay();
display.setTextColor(WHITE, BLACK);
display.setCursor(0, 0);
display.setTextSize(3);
display.print(map(txDeviceState.rssi, 0, 255, 0, 164));
display.setCursor(18, 28);
display.setTextSize(2);
display.print(txDeviceState.snr);
display.setCursor(74, 0);
display.setTextSize(3);
display.print(map(rxDeviceState.rssi, 0, 255, 0, 164));
display.setCursor(92, 28);
display.setTextSize(2);
display.print(rxDeviceState.snr);
display.setCursor(54, 48);
display.setTextSize(2);
display.print(txDeviceState.roundtrip);
display.display();
}
#endif
#endif
if (qsp.canTransmit && transmitPayload)
@@ -311,40 +481,30 @@ void loop(void)
radioPacketStart();
qspEncodeFrame(&qsp);
radioPacketEnd();
#ifdef DEBUG_LED
digitalWrite(LED_BUILTIN, HIGH);
delay(10);
digitalWrite(LED_BUILTIN, LOW);
delay(70);
digitalWrite(LED_BUILTIN, HIGH);
delay(10);
digitalWrite(LED_BUILTIN, LOW);
#endif
transmitPayload = false;
}
/*
* Here we do state handling and similar operations
*/
#ifdef DEVICE_MODE_RX
if (abs(currentMillis - qsp.lastFrameReceivedAt[QSP_FRAME_RC_DATA]) > RX_FAILSAFE_DELAY) {
qsp.deviceState = DEVICE_STATE_FAILSAFE;
} else {
qsp.deviceState = DEVICE_STATE_OK;
}
#endif
}
#ifdef LORA_HARDWARE_SPI
void onReceive(int packetSize)
{
if (packetSize == 0)
return;
while (LoRa.available())
{
qspDecodeIncomingFrame(&qsp, LoRa.read(), ppm, &rxDeviceState);
}
}
#ifdef DEVICE_MODE_TX
txDeviceState.readPacket = true;
#endif
#ifdef DEVICE_MODE_RX
int incomingByte;
while (incomingByte = LoRa.read(), incomingByte > -1)
{
qspDecodeIncomingFrame(&qsp, incomingByte, ppm, &rxDeviceState, &txDeviceState);
}
rxDeviceState.rssi = getRadioRssi();
rxDeviceState.snr = getRadioSnr();
#endif
}

80
qsp.cpp
View File

@@ -60,7 +60,14 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
qsp->payload[2] = rxDeviceState->rxVoltage;
qsp->payload[3] = rxDeviceState->a1Voltage;
qsp->payload[4] = rxDeviceState->a2Voltage;
qsp->payload[5] = qsp->lastReceivedPacketId;
uint8_t flags = 0;
if (qsp->deviceState == DEVICE_STATE_FAILSAFE) {
flags |= 0x01 << 0;
}
qsp->payload[5] = flags;
qsp->payloadLength = 6;
}
@@ -71,7 +78,7 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
rxDeviceState->rxVoltage = qsp->payload[2];
rxDeviceState->a1Voltage = qsp->payload[3];
rxDeviceState->a2Voltage = qsp->payload[4];
// rxDeviceState->rssi = qsp->payload[0]; //TODO we skipped decoding this byte, figure it out
rxDeviceState->flags = qsp->payload[5];
}
/**
@@ -127,13 +134,6 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t
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++)
@@ -143,16 +143,19 @@ 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;
static uint8_t packetId; //TODO move this to global scope maybe?
if (qsp->protocolState == QSP_STATE_IDLE && incomingByte == QSP_PREAMBLE)
{
//FIXME there should be a way to go back to IDLE if frame did not finished decoding in reasonable time
//If in IDLE and correct preamble comes, start to decode frame
qsp->protocolState = QSP_STATE_PREAMBLE_RECEIVED;
qsp->crc = 0 ^ incomingByte;
@@ -169,7 +172,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
qspClearPayload(qsp);
receivedPayload = 0;
packetId = 0;
}
else
{
@@ -188,12 +190,9 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
}
else if (qsp->protocolState == QSP_STATE_FRAME_TYPE_RECEIVED)
{
qsp->crc ^= incomingByte;
packetId = incomingByte;
qsp->protocolState = QSP_STATE_PACKET_ID_RECEIVED;
if (receivedPayload >= QSP_PAYLOAD_LENGTH) {
qsp->protocolState = QSP_STATE_IDLE;
}
else if (qsp->protocolState == QSP_STATE_PACKET_ID_RECEIVED)
{
//Now it's time for payload
qsp->crc ^= incomingByte;
@@ -209,7 +208,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
}
else if (qsp->protocolState == QSP_STATE_PAYLOAD_RECEIVED)
{
if (qsp->crc == incomingByte) {
//CRC is correct
@@ -220,21 +218,7 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
if (frameId < QSP_FRAME_COUNT) {
qsp->lastFrameReceivedAt[frameId] = millis();
}
qsp->lastReceivedPacketId = packetId;
if (qsp->debugConfig & DEBUG_FLAG_SERIAL) {
Serial.print("Frame ");
Serial.print(frameId);
Serial.println(" received");
}
if (qsp->debugConfig & DEBUG_FLAG_LED) {
digitalWrite(LED_BUILTIN, HIGH);
delay(10);
digitalWrite(LED_BUILTIN, LOW);
delay(100);
}
qsp->anyFrameRecivedAt = millis();
switch (frameId) {
case QSP_FRAME_RC_DATA:
@@ -243,12 +227,6 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
case QSP_FRAME_RX_HEALTH:
decodeRxHealthPayload(qsp, rxDeviceState);
if (qsp->debugConfig & DEBUG_FLAG_SERIAL) {
Serial.print("RX RSSI: ");
Serial.println(rxDeviceState->rssi);
Serial.print("RX SNR: ");
Serial.println(rxDeviceState->snr);
}
break;
case QSP_FRAME_PING:
@@ -256,18 +234,13 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
break;
case QSP_FRAME_PONG:
if (qsp->debugConfig & DEBUG_FLAG_SERIAL) {
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;
uint32_t incoming = 0;
txDeviceState->roundtrip = (micros() - txDeviceState->roundtrip) / 1000;
incoming = qsp->payload[0];
incoming += (uint32_t) qsp->payload[1] << 8;
incoming += (uint32_t) qsp->payload[2] << 16;
incoming += (uint32_t) qsp->payload[3] << 24;
Serial.print("Rountrip: ");
Serial.println((micros() - incoming) / 1000);
}
break;
default:
@@ -275,6 +248,8 @@ void qspDecodeIncomingFrame(QspConfiguration_t *qsp, uint8_t incomingByte, int p
//TODO do something in this case
break;
}
qsp->transmitWindowOpen = true;
}
else
{
@@ -304,9 +279,6 @@ void qspEncodeFrame(QspConfiguration_t *qsp) {
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++)
{

9
qsp.h
View File

@@ -9,8 +9,13 @@ uint8_t get10bitLowShift(uint8_t channel);
void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels);
uint8_t qspGetPacketId(void);
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
);
void qspClearPayload(QspConfiguration_t *qsp);
void qspEncodeFrame(QspConfiguration_t *qsp);

11
sbus.cpp
View File

@@ -12,6 +12,15 @@
#define SBUS_STATE_FAILSAFE 0x08
#define SBUS_STATE_SIGNALLOSS 0x04
/*
Precomputed mapping from 990-2010 to 173:1811
equivalent to
map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
*/
int mapChannelToSbus(int in) {
return (((long) in * 1605l) / 1000l) - 1417;
}
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){
static int output[SBUS_CHANNEL_NUMBER] = {0};
@@ -21,7 +30,7 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool
* 173-1811 with middle at 992 S.BUS protocol requires
*/
for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
output[i] = map(channels[i], RC_CHANNEL_MIN, RC_CHANNEL_MAX, SBUS_MIN_OFFSET, SBUS_MAX_OFFSET);
output[i] = mapChannelToSbus(channels[i]);
}
uint8_t stateByte = 0x00;

31
track1.txt Normal file
View File

@@ -0,0 +1,31 @@
2
3
4
5
12
13
12
13
12
13
12
13
12
13
12
13
12
13
12
13
12
13
6
7
12
13
12
13
12
13
2

14
track2.txt Normal file
View File

@@ -0,0 +1,14 @@
13
12
13
6
7
12
13
12
13
12
13
12
13
2

10
track3.txt Normal file
View File

@@ -0,0 +1,10 @@
12
13
12
13
2
21
22
23
24
25

66
txbuzzer.cpp Normal file
View File

@@ -0,0 +1,66 @@
#include "Arduino.h"
#include "txbuzzer.h"
/**
* This method plays selected pattern only once
* It disables continious mode
*/
void buzzerSingleMode(uint8_t mode, BuzzerState_t *buzzer) {
buzzer->singleModeEnabled = true;
buzzer->enabled = false;
buzzer->mode = mode;
buzzer->tick = 0;
}
void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer) {
buzzer->singleModeEnabled = false;
buzzer->enabled = true;
buzzer->mode = mode;
}
void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer)
{
if (!buzzer->enabled && !buzzer->singleModeEnabled)
{
digitalWrite(pin, LOW);
return;
}
if (timestamp > buzzer->updateTime)
{
int8_t currentPattern = buzzer->pattern[buzzer->mode][buzzer->element];
if (currentPattern == PATTERN_CYCLE_OFF)
{
digitalWrite(pin, LOW);
}
else if (currentPattern == PATTERN_CYCLE_ON)
{
digitalWrite(pin, HIGH);
}
else if (currentPattern == PATTERN_CYCLE_IGNORE || currentPattern == buzzer->tick)
{
if (currentPattern != PATTERN_CYCLE_IGNORE)
{
digitalWrite(pin, !digitalRead(pin));
}
buzzer->element++;
if (buzzer->element == PATTERN_ELEMENT_NUMBER)
{
buzzer->element = 0;
}
}
buzzer->tick++;
if (buzzer->tick >= buzzer->patternMaxTick)
{
buzzer->tick = 0;
buzzer->singleModeEnabled = false;
}
buzzer->updateTime = timestamp + buzzer->patternTickPerdiod;
}
};

46
txbuzzer.h Normal file
View File

@@ -0,0 +1,46 @@
#pragma once
#include "Arduino.h"
#define PATTERN_CYCLE_OFF 127
#define PATTERN_CYCLE_ON -1
#define PATTERN_CYCLE_IGNORE -2
#define PATTERN_MODES_NUMBER 6
#define PATTERN_ELEMENT_NUMBER 4
enum {
BUZZER_MODE_OFF = 0,
BUZZER_MODE_CONTINUOUS = 1,
BUZZER_MODE_SLOW_BEEP = 2,
BUZZER_MODE_FAST_BEEP = 3,
BUZZER_MODE_CHIRP = 4,
BUZZER_MODE_DOUBLE_CHIRP = 5
};
struct BuzzerState_t {
bool enabled = false; //Continous mode buzzer
bool singleModeEnabled = false;
uint8_t mode = BUZZER_MODE_OFF;
uint32_t updateTime = 0;
uint8_t tick = 0;
uint8_t element = 0;
const uint8_t patternMaxTick = 20;
const uint8_t patternTickPerdiod = 75;
const int8_t pattern[PATTERN_MODES_NUMBER][PATTERN_ELEMENT_NUMBER] = {
{PATTERN_CYCLE_OFF},
{PATTERN_CYCLE_ON},
{0, 7, 10, 17},
{0, 4, 10, 14},
{0, 1, PATTERN_CYCLE_IGNORE, PATTERN_CYCLE_IGNORE},
{0, 1, 2, 3}
};
};
void buzzerSingleMode(uint8_t mode, BuzzerState_t *buzzer);
void buzzerContinousMode(uint8_t mode, BuzzerState_t *buzzer);
void buzzerProcess(uint8_t pin, uint32_t timestamp, BuzzerState_t *buzzer);

39
variables.h
View File

@@ -1,5 +1,7 @@
#pragma once
#define OLED_UPDATE_RATE 500
#define SBUS_UPDATE_RATE 15 //ms
#define SBUS_PACKET_LENGTH 25
@@ -9,17 +11,15 @@
#define RX_TASK_HEALTH 200 //5Hz should be enough
#define RSSI_CHANNEL 11
#define RX_RX_HEALTH_FRAME_RATE 1000
#define TX_RC_FRAME_RATE 500 //ms
#define RX_FAILSAFE_DELAY (TX_RC_FRAME_RATE * 8)
#define TX_PING_RATE 2000
#define TX_TRANSMIT_SLOT_RATE 70 //ms
#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8)
#define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4)
#define CHANNEL_ID 0x01
#define QSP_PREAMBLE 0x51
#define QSP_PAYLOAD_LENGTH 32
#define QSP_MAX_FRAME_DECODE_TIME 50 //max time that frame can be decoded in ms
#define QSP_MAX_FRAME_DECODE_TIME 10 //max time that frame can be decoded in ms
#define QSP_FRAME_RC_DATA 0x0
#define QSP_FRAME_RX_HEALTH 0x1
@@ -39,14 +39,14 @@ enum dataStates {
QSP_STATE_PREAMBLE_RECEIVED,
QSP_STATE_CHANNEL_RECEIVED,
QSP_STATE_FRAME_TYPE_RECEIVED,
QSP_STATE_PACKET_ID_RECEIVED,
QSP_STATE_PAYLOAD_RECEIVED,
QSP_STATE_CRC_RECEIVED
};
enum deviceStates {
DEVICE_STATE_OK,
DEVICE_STATE_FAILSAFE
DEVICE_STATE_FAILSAFE,
DEVICE_STATE_UNDETERMINED
};
enum debugConfigFlags {
@@ -60,6 +60,8 @@ enum debugConfigFlags {
#define PPM_INPUT_CHANNEL_COUNT 10
#define PPM_OUTPUT_CHANNEL_COUNT 10
#define TX_BUZZER_PIN A5
#define PPM_CHANNEL_DEFAULT_VALUE 1500 //set the default servo value
#define PPM_FRAME_LENGTH 30500 //set the PPM frame length in microseconds (1ms = 1000µs)
#define PPM_PULSE_LENGTH 300 //set the pulse length
@@ -74,20 +76,31 @@ struct QspConfiguration_t {
uint8_t payloadLength = 0;
uint8_t frameToSend = 0;
uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
uint32_t lastFrameTransmitedAt[QSP_FRAME_COUNT] = {0};
uint8_t deviceState = DEVICE_STATE_OK;
uint32_t anyFrameRecivedAt = 0;
uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
void (* hardwareWriteFunction)(uint8_t, QspConfiguration_t*);
uint8_t lastReceivedPacketId = 0;
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 rssi = 0;
uint8_t snr = 0;
uint8_t flags = 0;
uint32_t roundtrip = 0;
bool readPacket = false;
bool isReceiving = false; //Indicates that TX module is receiving frames from RX module
};
struct RxDeviceState_t {
int rssi = 0;
float snr = 0;
uint8_t rssi = 0;
uint8_t snr = 0;
uint8_t rxVoltage = 0;
uint8_t a1Voltage = 0;
uint8_t a2Voltage = 0;
uint8_t flags = 0;
};