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

Merge pull request #60 from DzikuVx/optimizations-and-sanitizations

Browse Source 
Optimizations and sanitizations
This commit is contained in:
Paweł Spychalski
2018-05-17 20:54:57 +02:00
committed by GitHub
parent 16a90adbc0 d7bfb7740e
commit a83b1fc45b
14 changed files with 197 additions and 253 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
.vscode/settings.json vendored
View File

@@ -8,7 +8,9 @@
"iterator": "cpp",
"rope": "c",
"__locale": "c",
"string": "c"
"string": "c",
"ios": "cpp",
"locale": "cpp"
},
"files.exclude": {
"**/build": true

97
crossbow/crossbow.ino
View File

@@ -14,6 +14,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
#include "main_variables.h"
#include "qsp.h"
#include "sbus.h"
#include "platform_node.h"
#ifdef ARDUINO_AVR_FEATHER32U4
#define LORA_SS_PIN 8
@@ -34,6 +35,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
#endif
RadioNode radioNode;
PlatformNode platformNode;
/*
* Main defines for device working in TX mode
@@ -52,9 +54,6 @@ RadioNode radioNode;
#error please select tx input source
#endif
volatile int16_t TxInput::channels[TX_INPUT_CHANNEL_COUNT];
#include "txbuzzer.h"
BuzzerState_t buzzer;
@@ -91,7 +90,8 @@ void onQspSuccess(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDev
//If recide received a valid frame, that means it can start to talk
radioNode.lastReceivedChannel = receivedChannel;
qsp->canTransmit = true;
//RX can start transmitting only when an least one frame has been receiveds
radioNode.canTransmit = true;
radioNode.readRssi();
radioNode.readSnr();
@@ -155,19 +155,14 @@ void setup(void)
qsp.onFailureCallback = onQspFailure;
#ifdef DEVICE_MODE_RX
qsp.deviceState = DEVICE_STATE_FAILSAFE;
platformNode.platformState = DEVICE_STATE_FAILSAFE;
#else
qsp.deviceState = DEVICE_STATE_OK;
platformNode.platformState = DEVICE_STATE_OK;
#endif
radioNode.init(LORA_SS_PIN, LORA_RST_PIN, LORA_DI0_PIN, onReceive);
#ifdef DEVICE_MODE_RX
//initiallize default ppm values
for (int i = 0; i < 16; i++)
{
rxDeviceState.channels[i] = PPM_CHANNEL_DEFAULT_VALUE;
}
pinMode(RX_ADC_PIN_1, INPUT);
pinMode(RX_ADC_PIN_2, INPUT);
@@ -183,17 +178,13 @@ void setup(void)
#ifdef FEATURE_TX_OLED
oled.init();
oled.page(
&rxDeviceState,
&txDeviceState,
TX_PAGE_INIT
);
oled.page(TX_PAGE_INIT);
#endif
/*
* TX should start talking imediately after power up
*/
qsp.canTransmit = true;
radioNode.canTransmit = true;
pinMode(TX_BUZZER_PIN, OUTPUT);
@@ -215,20 +206,13 @@ void setup(void)
pinMode(LED_BUILTIN, OUTPUT);
#ifdef DEBUG_SERIAL
qsp.debugConfig |= DEBUG_FLAG_SERIAL;
#endif
#ifdef DEBUG_LED
qsp.debugConfig |= DEBUG_FLAG_LED;
#endif
/*
* Setup salt bind key
*/
qsp.bindKey[0] = 0x12;
qsp.bindKey[1] = 0x0a;
qsp.bindKey[2] = 0x36;
qsp.bindKey[3] = 0xa7;
platformNode.bindKey[0] = 0x12;
platformNode.bindKey[1] = 0x0a;
platformNode.bindKey[2] = 0x36;
platformNode.bindKey[3] = 0xa7;
}
@@ -303,12 +287,7 @@ void loop(void)
button1.loop();
#ifdef FEATURE_TX_OLED
oled.loop(
&rxDeviceState,
&txDeviceState,
&button0,
&button1
);
oled.loop();
#endif
txInput.recoverStuckFrames();
@@ -352,7 +331,7 @@ void loop(void)
txInput.loop();
if (
radioNode.deviceState == RADIO_STATE_RX &&
radioNode.radioState == RADIO_STATE_RX &&
qsp.protocolState == QSP_STATE_IDLE &&
qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
) {
@@ -380,7 +359,7 @@ void loop(void)
break;
case QSP_FRAME_RC_DATA:
encodeRcDataPayload(&qsp, txInput.channels, PPM_INPUT_CHANNEL_COUNT);
encodeRcDataPayload(&qsp, PLATFORM_CHANNEL_COUNT);
break;
}
@@ -403,11 +382,6 @@ void loop(void)
uint8_t output = constrain(radioNode.rssi - 40, 0, 100);
rxDeviceState.indicatedRssi = (output * 10) + 1000;
if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
digitalWrite(LED_BUILTIN, HIGH);
} else {
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
}
}
/*
@@ -441,27 +415,26 @@ void loop(void)
}
if (currentMillis > sbusTime) {
rxDeviceState.channels[RSSI_CHANNEL - 1] = rxDeviceState.indicatedRssi;
platformNode.setRcChannel(RSSI_CHANNEL - 1, rxDeviceState.indicatedRssi, 0);
sbusPreparePacket(sbusPacket, rxDeviceState.channels, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE));
sbusPreparePacket(sbusPacket, false, (platformNode.platformState == 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;
platformNode.platformState = DEVICE_STATE_FAILSAFE;
rxDeviceState.indicatedRssi = 0;
radioNode.rssi = 0;
} else {
qsp.deviceState = DEVICE_STATE_OK;
platformNode.platformState = DEVICE_STATE_OK;
}
#endif
if (qsp.canTransmit && transmitPayload)
if (transmitPayload)
{
radioNode.handleTx(&qsp);
transmitPayload = false;
}
#ifdef DEVICE_MODE_TX
@@ -474,7 +447,7 @@ void loop(void)
//TX module started to receive data
buzzerSingleMode(BUZZER_MODE_DOUBLE_CHIRP, &buzzer);
txDeviceState.isReceiving = true;
qsp.deviceState = DEVICE_STATE_OK;
platformNode.platformState = DEVICE_STATE_OK;
}
//Here we detect failsafe state on TX module
@@ -490,14 +463,14 @@ void loop(void)
rxDeviceState.snr = 0;
rxDeviceState.flags = 0;
txDeviceState.roundtrip = 0;
qsp.deviceState = DEVICE_STATE_FAILSAFE;
platformNode.platformState = 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) {
if (platformNode.platformState == DEVICE_STATE_FAILSAFE) {
//Failsafe detected by TX
buzzerContinousMode(BUZZER_MODE_SLOW_BEEP, &buzzer);
} else if (txDeviceState.isReceiving && (rxDeviceState.flags & 0x1) == 1) {
@@ -511,24 +484,32 @@ void loop(void)
buzzerContinousMode(BUZZER_MODE_OFF, &buzzer);
}
#endif
/*
* Handle LED updates
*/
if (txDeviceState.nextLedUpdate < currentMillis) {
if (platformNode.nextLedUpdate < currentMillis) {
#ifdef DEVICE_MODE_TX
if (txDeviceState.isReceiving) {
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
txDeviceState.nextLedUpdate = currentMillis + 300;
platformNode.nextLedUpdate = currentMillis + 300;
} else if (txInput.isReceiving()) {
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
txDeviceState.nextLedUpdate = currentMillis + 100;
platformNode.nextLedUpdate = currentMillis + 100;
} else {
digitalWrite(LED_BUILTIN, HIGH);
txDeviceState.nextLedUpdate = currentMillis + 200;
platformNode.nextLedUpdate = currentMillis + 200;
}
#else
platformNode.nextLedUpdate = currentMillis + 200;
if (platformNode.platformState == DEVICE_STATE_FAILSAFE) {
digitalWrite(LED_BUILTIN, HIGH);
} else {
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
}
}
#endif
}
}
@@ -549,7 +530,7 @@ void onReceive(int packetSize)
*/
LoRa.sleep();
LoRa.receive();
radioNode.deviceState = RADIO_STATE_RX;
radioNode.radioState = RADIO_STATE_RX;
}
}
}

21
crossbow/platform_node.cpp Normal file
View File

@@ -0,0 +1,21 @@
#include "platform_node.h"
PlatformNode::PlatformNode(void) {
for (uint8_t i = 0; i < PLATFORM_TOTAL_CHANNEL_COUNT; i++) {
_channels[i] = PLATFORM_DEFAULT_CHANNEL_VALUE;
}
}
int PlatformNode::getRcChannel(uint8_t channel) {
if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
return _channels[channel];
} else {
return PLATFORM_DEFAULT_CHANNEL_VALUE;
}
}
void PlatformNode::setRcChannel(uint8_t channel, int value, int offset) {
if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) {
_channels[channel] = value + offset;
}
}

31
crossbow/platform_node.h Normal file
View File

@@ -0,0 +1,31 @@
#pragma once
#include "Arduino.h"
#ifndef PLATFORM_NODE_H
#define PLATFORM_NODE_H
#define PLATFORM_TOTAL_CHANNEL_COUNT 11 //Including RSSI channel and other
#define PLATFORM_CHANNEL_COUNT 10
#define PLATFORM_DEFAULT_CHANNEL_VALUE 1000
enum deviceStates {
DEVICE_STATE_OK,
DEVICE_STATE_FAILSAFE,
DEVICE_STATE_UNDETERMINED
};
class PlatformNode {
public:
PlatformNode(void);
int getRcChannel(uint8_t channel);
void setRcChannel(uint8_t channel, int value, int offset);
uint8_t bindKey[4];
uint32_t nextLedUpdate = 0;
uint8_t platformState = DEVICE_STATE_UNDETERMINED;
private:
volatile int _channels[PLATFORM_TOTAL_CHANNEL_COUNT];
};
#endif

49
crossbow/qsp.cpp
View File

@@ -2,38 +2,17 @@
#include "variables.h"
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) {
int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0};
//TODO fix it, baby :)
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]) & 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 are passed as is
//8bit channels needs to be shifted left 2 places
temporaryPpmOutput[4] = temporaryPpmOutput[4] << 2;
temporaryPpmOutput[5] = temporaryPpmOutput[5] << 2;
//4bit channels needs to be shifted left 6 places
temporaryPpmOutput[6] = temporaryPpmOutput[6] << 6;
temporaryPpmOutput[7] = temporaryPpmOutput[7] << 6;
temporaryPpmOutput[8] = temporaryPpmOutput[8] << 6;
temporaryPpmOutput[9] = temporaryPpmOutput[9] << 6;
/*
* Copy tremporary to real output and add missing 1000
*/
for (uint8_t i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++) {
rxDeviceSate->channels[i] = temporaryPpmOutput[i] + 1000;
}
platformNode.setRcChannel(0, (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03), 1000);
platformNode.setRcChannel(1, (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F), 1000);
platformNode.setRcChannel(2, (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F), 1000);
platformNode.setRcChannel(3, (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF), 1000);
platformNode.setRcChannel(4, ((int) qsp->payload[5]) << 2, 1000);
platformNode.setRcChannel(5, ((int) qsp->payload[6]) << 2, 1000);
platformNode.setRcChannel(6, ((int) ((qsp->payload[7] >> 4) & 0b00001111)) << 6, 1000);
platformNode.setRcChannel(7, ((int) (qsp->payload[7] & 0b00001111)) << 6, 1000);
platformNode.setRcChannel(8, ((int) ((qsp->payload[8] >> 4) & 0b00001111)) << 6, 1000);
platformNode.setRcChannel(9, ((int) (qsp->payload[8] & 0b00001111)) << 6, 1000);
}
uint8_t get10bitHighShift(uint8_t channel) {
@@ -71,7 +50,7 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
uint8_t flags = 0;
if (qsp->deviceState == DEVICE_STATE_FAILSAFE) {
if (platformNode.platformState == DEVICE_STATE_FAILSAFE) {
flags |= 0x01 << 0;
}
@@ -92,11 +71,11 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
/**
* Encode 10 RC channels
*/
void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels)
void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels)
{
for (uint8_t i = 0; i < noOfChannels; i++)
{
int cV = constrain(channels[i], 1000, 2000) - 1000;
int cV = constrain(platformNode.getRcChannel(i), 1000, 2000) - 1000;
uint16_t channelValue10 = cV & 0x03ff;
uint8_t channelValue8 = (cV >> 2) & 0xff;
@@ -157,7 +136,7 @@ void qspClearPayload(QspConfiguration_t *qsp)
void qspInitCrc(QspConfiguration_t *qsp) {
qsp->crc = 0;
for (uint8_t i = 0; i < 4; i++) {
qspComputeCrc(qsp, qsp->bindKey[i]);
qspComputeCrc(qsp, platformNode.bindKey[i]);
}
}

5
crossbow/qsp.h
View File

@@ -1,6 +1,9 @@
#include "Arduino.h"
#include "variables.h"
#include "radio_node.h"
#include "platform_node.h"
extern PlatformNode platformNode;
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate);
void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
@@ -9,7 +12,7 @@ uint8_t get10bitHighShift(uint8_t channel);
uint8_t get10bitLowShift(uint8_t channel);
void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr);
void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels);
void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels);
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,

45
crossbow/radio_node.cpp
View File

@@ -1,6 +1,18 @@
#include "radio_node.h"
#include "lora.h"
uint32_t getFrequencyForChannel(uint8_t channel) {
return RADIO_FREQUENCY_MIN + (RADIO_CHANNEL_WIDTH * channel);
}
uint8_t getNextChannel(uint8_t channel) {
return (channel + RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
}
uint8_t getPrevChannel(uint8_t channel) {
return (RADIO_CHANNEL_COUNT + channel - RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
}
RadioNode::RadioNode(void) {
}
@@ -29,7 +41,7 @@ void RadioNode::init(uint8_t ss, uint8_t rst, uint8_t di0, void(*callback)(int))
//Setup ISR callback and start receiving
LoRa.onReceive(callback);
LoRa.receive();
deviceState = RADIO_STATE_RX;
radioState = RADIO_STATE_RX;
}
void RadioNode::readRssi(void)
@@ -50,18 +62,6 @@ uint32_t RadioNode::getChannelEntryMillis(void) {
return _channelEntryMillis;
}
uint32_t RadioNode::getFrequencyForChannel(uint8_t channel) {
return RADIO_FREQUENCY_MIN + (RADIO_CHANNEL_WIDTH * channel);
}
uint8_t RadioNode::getNextChannel(uint8_t channel) {
return (channel + RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
}
uint8_t RadioNode::getPrevChannel(uint8_t channel) {
return (RADIO_CHANNEL_COUNT + channel - RADIO_HOP_OFFSET) % RADIO_CHANNEL_COUNT;
}
void RadioNode::readAndDecode(
QspConfiguration_t *qsp,
RxDeviceState_t *rxDeviceState,
@@ -82,7 +82,7 @@ void RadioNode::readAndDecode(
LoRa.sleep();
LoRa.receive();
deviceState = RADIO_STATE_RX;
radioState = RADIO_STATE_RX;
bytesToRead = NO_DATA_TO_READ;
}
}
@@ -91,15 +91,15 @@ void RadioNode::hopFrequency(bool forward, uint8_t fromChannel, uint32_t timesta
_channelEntryMillis = timestamp;
if (forward) {
_channel = RadioNode::getNextChannel(fromChannel);
_channel = getNextChannel(fromChannel);
} else {
_channel = RadioNode::getPrevChannel(fromChannel);
_channel = getPrevChannel(fromChannel);
}
// And set hardware
LoRa.sleep();
LoRa.setFrequency(
RadioNode::getFrequencyForChannel(_channel)
getFrequencyForChannel(_channel)
);
LoRa.idle();
}
@@ -124,7 +124,7 @@ void RadioNode::handleTxDoneState(bool hop) {
if (
currentMillis > nextTxCheckMillis &&
deviceState == RADIO_STATE_TX &&
radioState == RADIO_STATE_TX &&
!LoRa.isTransmitting()
) {
@@ -136,12 +136,17 @@ void RadioNode::handleTxDoneState(bool hop) {
}
LoRa.receive();
deviceState = RADIO_STATE_RX;
radioState = RADIO_STATE_RX;
nextTxCheckMillis = currentMillis + 1; //We check of TX done every 1ms
}
}
void RadioNode::handleTx(QspConfiguration_t *qsp) {
if (!canTransmit) {
return;
}
uint8_t size;
uint8_t tmpBuffer[MAX_PACKET_SIZE];
@@ -153,5 +158,5 @@ void RadioNode::handleTx(QspConfiguration_t *qsp) {
LoRa.endPacketAsync();
//Set state to be able to detect the moment when TX is done
deviceState = RADIO_STATE_TX;
radioState = RADIO_STATE_TX;
}

6
crossbow/radio_node.h
View File

@@ -21,9 +21,6 @@ class RadioNode {
void init(uint8_t ss, uint8_t rst, uint8_t di0, void(*callback)(int));
void readRssi(void);
void readSnr(void);
uint32_t getFrequencyForChannel(uint8_t channel);
uint8_t getNextChannel(uint8_t channel);
uint8_t getPrevChannel(uint8_t channel);
void hopFrequency(bool forward, uint8_t fromChannel, uint32_t timestamp);
void readAndDecode(
QspConfiguration_t *qsp,
@@ -36,7 +33,7 @@ class RadioNode {
void handleTxDoneState(bool hop);
void handleTx(QspConfiguration_t *qsp);
volatile int8_t bytesToRead = -1;
volatile uint8_t deviceState = RADIO_STATE_RX;
volatile uint8_t radioState = RADIO_STATE_RX;
uint8_t rssi = 0;
uint8_t snr = 0;
uint8_t lastReceivedChannel = 0;
@@ -45,6 +42,7 @@ class RadioNode {
uint8_t loraSpreadingFactor = 7;
uint8_t loraCodingRate = 6;
uint8_t loraTxPower = 17; // Defines output power of TX, defined in dBm range from 2-17
bool canTransmit = false;
private:
uint8_t _channel = 0;
uint32_t _channelEntryMillis = 0;

45
crossbow/sbus.cpp
View File

@@ -31,16 +31,16 @@ int mapSbusToChannel(int in) {
return (((long) in - 173l) * 1020l / 1638l) + 990;
}
void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe){
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){
static int output[SBUS_CHANNEL_NUMBER] = {0};
int output[SBUS_CHANNEL_NUMBER];
/*
* Map 1000-2000 with middle at 1500 chanel values to
* 173-1811 with middle at 992 S.BUS protocol requires
*/
for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
output[i] = mapChannelToSbus(channels[i]);
output[i] = mapChannelToSbus(platformNode.getRcChannel(i));
}
uint8_t stateByte = 0x00;
@@ -83,7 +83,6 @@ SbusInput::SbusInput(HardwareSerial &serial) : _serial(serial)
{
}
void SbusInput::loop(void)
{
if (_serial.available()) {
@@ -116,36 +115,26 @@ void SbusInput::recoverStuckFrames(void)
}
}
void sbusToChannels(volatile int16_t channels[], byte buffer[]) {
channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
void sbusToChannels(byte buffer[]) {
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
}
platformNode.setRcChannel(0, mapSbusToChannel((buffer[1] | buffer[2]<<8) & 0x07FF), 0);
platformNode.setRcChannel(1, mapSbusToChannel((buffer[2]>>3 | buffer[3]<<5) & 0x07FF), 0);
platformNode.setRcChannel(2, mapSbusToChannel((buffer[3]>>6 | buffer[4]<<2 | buffer[5]<<10) & 0x07FF), 0);
platformNode.setRcChannel(3, mapSbusToChannel((buffer[5]>>1 | buffer[6]<<7) & 0x07FF), 0);
platformNode.setRcChannel(4, mapSbusToChannel((buffer[6]>>4 | buffer[7]<<4) & 0x07FF), 0);
platformNode.setRcChannel(5, mapSbusToChannel((buffer[7]>>7 | buffer[8]<<1 |buffer[9]<<9) & 0x07FF), 0);
platformNode.setRcChannel(6, mapSbusToChannel((buffer[9]>>2 | buffer[10]<<6) & 0x07FF), 0);
platformNode.setRcChannel(7, mapSbusToChannel((buffer[10]>>5 | buffer[11]<<3) & 0x07FF), 0);
platformNode.setRcChannel(8, mapSbusToChannel((buffer[12] | buffer[13]<<8) & 0x07FF), 0);
platformNode.setRcChannel(9, mapSbusToChannel((buffer[13]>>3 | buffer[14]<<5) & 0x07FF), 0);
//We use only 10 channels, so the reset can be just ignored
}
void SbusInput::sbusRead() {
static byte buffer[25];
static byte buffer_index = 0;
static uint32_t _decoderErrorFrames;
static uint32_t _goodFrames;
while (_serial.available()) {
byte rx = _serial.read();
@@ -172,7 +161,7 @@ void SbusInput::sbusRead() {
) {
//We have full frame now
_frameDecodingEndedAt = millis();
sbusToChannels(channels, buffer);
sbusToChannels(buffer);
_protocolState = SBUS_DECODING_STATE_IDLE;
}
}

2
crossbow/sbus.h
View File

@@ -27,7 +27,7 @@ class SbusInput : public TxInput
void sbusRead(void);
};
void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe);
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe);
#endif

2
crossbow/tx_input.h
View File

@@ -10,12 +10,10 @@ class TxInput
{
public:
virtual ~TxInput() {}
int get(uint8_t channel) { return channels[channel]; };
virtual void start(void) {};
virtual void stop(void) {};
virtual bool isReceiving(void) { return false; };
virtual void loop(void) {};
volatile static int16_t channels[TX_INPUT_CHANNEL_COUNT];
};
#endif

77
crossbow/tx_oled.cpp
View File

@@ -12,12 +12,7 @@ void TxOled::init() {
_display.display();
}
void TxOled::loop(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState,
Tactile *button0,
Tactile *button1
) {
void TxOled::loop() {
bool update = false;
//Depending on page, things might be different
@@ -29,7 +24,7 @@ void TxOled::loop(
case TX_PAGE_STATS:
//Second button refreshes this page
if (button1->getState() == TACTILE_STATE_SHORT_PRESS) {
if (button1.getState() == TACTILE_STATE_SHORT_PRESS) {
update = true;
}
break;
@@ -37,54 +32,52 @@ void TxOled::loop(
}
//Short press of button0 always toggles no next page
if (button0->getState() == TACTILE_STATE_SHORT_PRESS) {
if (button0.getState() == TACTILE_STATE_SHORT_PRESS) {
_mainPageSequenceIndex++;
if (_mainPageSequenceIndex == TX_OLED_PAGE_COUNT) {
_mainPageSequenceIndex = 0;
}
update = true;
}
if (update) {
page(
rxDeviceState,
txDeviceState,
pageSequence[_mainPageSequenceIndex]
);
page(pageSequence[_mainPageSequenceIndex]);
}
}
void TxOled::page(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState,
int page
) {
void TxOled::page(uint8_t page) {
static uint32_t lastUpdate = 0;
//Do not allow for OLED to be updated too often
if (lastUpdate > 0 && millis() - lastUpdate < 200) {
return;
}
switch (page) {
case TX_PAGE_INIT:
renderPageInit(rxDeviceState, txDeviceState);
renderPageInit();
break;
case TX_PAGE_STATS:
renderPageStats(rxDeviceState, txDeviceState);
renderPageStats();
break;
case TX_PAGE_PWR:
renderPagePwr(rxDeviceState, txDeviceState);
renderPagePwr();
break;
case TX_PAGE_BIND:
renderPageBind(rxDeviceState, txDeviceState);
renderPageBind();
break;
case TX_PAGE_MODE:
renderPageMode(rxDeviceState, txDeviceState);
renderPageMode();
break;
}
_page = page;
lastUpdate = millis();
}
void TxOled::renderPagePwr(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
) {
void TxOled::renderPagePwr() {
_display.clearDisplay();
_display.setTextColor(WHITE, BLACK);
@@ -101,10 +94,7 @@ void TxOled::renderPagePwr(
_display.display();
}
void TxOled::renderPageBind(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
) {
void TxOled::renderPageBind() {
_display.clearDisplay();
_display.setTextColor(WHITE, BLACK);
@@ -117,10 +107,7 @@ void TxOled::renderPageBind(
_display.display();
}
void TxOled::renderPageMode(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
) {
void TxOled::renderPageMode() {
_display.clearDisplay();
_display.setTextColor(WHITE, BLACK);
@@ -135,10 +122,7 @@ void TxOled::renderPageMode(
_display.display();
}
void TxOled::renderPageStats(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
) {
void TxOled::renderPageStats() {
_display.clearDisplay();
_display.setTextColor(WHITE, BLACK);
@@ -152,23 +136,20 @@ void TxOled::renderPageStats(
_display.setCursor(74, 0);
_display.setTextSize(3);
_display.print(rxDeviceState->rssi);
_display.print(rxDeviceState.rssi);
_display.setCursor(92, 28);
_display.setTextSize(2);
_display.print(rxDeviceState->snr);
_display.print(rxDeviceState.snr);
_display.setCursor(54, 48);
_display.setTextSize(2);
_display.print(txDeviceState->roundtrip);
_display.print(txDeviceState.roundtrip);
_display.display();
}
void TxOled::renderPageInit(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
) {
void TxOled::renderPageInit() {
_display.clearDisplay();
_display.setTextColor(WHITE, BLACK);
_display.setTextSize(2);

42
crossbow/tx_oled.h
View File

@@ -10,6 +10,10 @@
#include "radio_node.h"
extern RadioNode radioNode;
extern RxDeviceState_t rxDeviceState;
extern TxDeviceState_t txDeviceState;
extern Tactile button0;
extern Tactile button1;
enum txOledPages {
TX_PAGE_NONE,
@@ -34,39 +38,15 @@ class TxOled {
public:
TxOled(void);
void init();
void loop(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState,
Tactile *button0,
Tactile *button1
);
void page(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState,
int page
);
void loop();
void page(uint8_t page);
private:
Adafruit_SSD1306 _display;
void renderPageInit(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
);
void renderPageStats(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
);
void renderPagePwr(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
);
void renderPageBind(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
);
void renderPageMode(
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
);
void renderPageInit();
void renderPageStats();
void renderPagePwr();
void renderPageBind();
void renderPageMode();
uint8_t _page = TX_PAGE_NONE;
uint8_t _mainPageSequenceIndex = 0;
};

24
crossbow/variables.h
View File

@@ -14,8 +14,6 @@
#define RX_FAILSAFE_DELAY (TX_TRANSMIT_SLOT_RATE * 8)
#define TX_FAILSAFE_DELAY (RX_FAILSAFE_DELAY * 4)
#define OLED_UPDATE_RATE 750
#define SBUS_UPDATE_RATE 15 //ms
#define SBUS_PACKET_LENGTH 25
@@ -25,7 +23,6 @@
#define RX_TASK_HEALTH 200 //5Hz should be enough
#define RSSI_CHANNEL 11
#define CHANNEL_ID 0x01
#define QSP_PAYLOAD_LENGTH 32
#define QSP_MAX_FRAME_DECODE_TIME 10 //max time that frame can be decoded in ms
@@ -60,25 +57,10 @@ enum dataStates {
QSP_STATE_CRC_RECEIVED
};
enum deviceStates {
DEVICE_STATE_OK,
DEVICE_STATE_FAILSAFE,
DEVICE_STATE_UNDETERMINED
};
enum debugConfigFlags {
DEBUG_FLAG_SERIAL = 0b00000001,
DEBUG_FLAG_LED = 0b00000010
};
#define PPM_INPUT_PIN 0 // Has to be one of Interrupt pins
#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
#define PPM_OUTPUT_MULTIPLIER 1 //1 for 8MHz RX, 2 for 16MHz RX
@@ -94,7 +76,6 @@ struct TxDeviceState_t {
uint8_t flags = 0;
uint32_t roundtrip = 0;
bool isReceiving = false; //Indicates that TX module is receiving frames from RX module
uint32_t nextLedUpdate = 0;
};
struct RxDeviceState_t {
@@ -104,12 +85,10 @@ struct RxDeviceState_t {
uint8_t a1Voltage = 0;
uint8_t a2Voltage = 0;
uint8_t flags = 0;
int16_t channels[16] = {};
int16_t indicatedRssi = 0;
};
struct QspConfiguration_t {
uint8_t bindKey[4] = {0, 0, 0, 0};
uint8_t protocolState = QSP_STATE_IDLE;
uint8_t crc = 0;
uint8_t payload[QSP_PAYLOAD_LENGTH] = {0};
@@ -118,12 +97,9 @@ struct QspConfiguration_t {
uint8_t frameId = 0;
uint32_t lastFrameReceivedAt[QSP_FRAME_COUNT] = {0};
uint32_t anyFrameRecivedAt = 0;
uint8_t deviceState = DEVICE_STATE_UNDETERMINED;
void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, uint8_t receivedChannel);
void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*);
bool canTransmit = false;
bool forcePongFrame = false;
uint8_t debugConfig = 0;
uint32_t frameDecodingStartedAt = 0;
uint32_t lastTxSlotTimestamp = 0;
bool transmitWindowOpen = false;