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

Some dependency pleanup

Browse Source
This commit is contained in:
Pawel Spychalski
2018-05-23 14:15:22 +02:00
parent 917b1514fd
commit bb2317b06a
9 changed files with 99 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
crossbow/crossbow.ino
View File

@@ -150,6 +150,16 @@ void onQspFailure(QspConfiguration_t *qsp, TxDeviceState_t *txDeviceState, RxDev
}
//I do not get function pointers to object methods, no way...
int getRcChannel_wrapper(uint8_t channel) {
return platformNode.getRcChannel(channel);
}
//Same here, wrapper just works
void setRcChannel_wrapper(uint8_t channel, int value, int offset) {
platformNode.setRcChannel(channel, value, offset);
}
void setup(void)
{
#ifdef DEBUG_SERIAL
@@ -158,11 +168,16 @@ void setup(void)
qsp.onSuccessCallback = onQspSuccess;
qsp.onFailureCallback = onQspFailure;
qsp.rcChannelGetCallback = getRcChannel_wrapper;
qsp.setRcChannelCallback = setRcChannel_wrapper;
#ifdef DEVICE_MODE_RX
platformNode.platformState = DEVICE_STATE_FAILSAFE;
#else
platformNode.platformState = DEVICE_STATE_OK;
txInput.setRcChannelCallback = setRcChannel_wrapper;
#endif
radioNode.init(LORA_SS_PIN, LORA_RST_PIN, LORA_DI0_PIN, onReceive);
@@ -274,7 +289,7 @@ void loop(void)
* This routine handles resync of TX/RX while hoppping frequencies
* When not in bind mode. Bind mode is single frequency operation
*/
if (!plaformNode.isBindMode) {
if (!platformNode.isBindMode) {
radioNode.handleChannelDwell();
}
@@ -314,7 +329,8 @@ void loop(void)
radioNode.readAndDecode(
&qsp,
&rxDeviceState,
&txDeviceState
&txDeviceState,
platformNode.bindKey
);
bool transmitPayload = false;
@@ -408,7 +424,13 @@ void loop(void)
break;
case QSP_FRAME_RX_HEALTH:
encodeRxHealthPayload(&qsp, &rxDeviceState, radioNode.rssi, radioNode.snr);
encodeRxHealthPayload(
&qsp,
&rxDeviceState,
radioNode.rssi,
radioNode.snr,
platformNode.platformState
);
break;
}
@@ -420,7 +442,7 @@ void loop(void)
if (currentMillis > sbusTime) {
platformNode.setRcChannel(RSSI_CHANNEL - 1, rxDeviceState.indicatedRssi, 0);
sbusPreparePacket(sbusPacket, false, (platformNode.platformState == DEVICE_STATE_FAILSAFE));
sbusPreparePacket(sbusPacket, false, (platformNode.platformState == DEVICE_STATE_FAILSAFE), getRcChannel_wrapper);
Serial1.write(sbusPacket, SBUS_PACKET_LENGTH);
sbusTime = currentMillis + SBUS_UPDATE_RATE;
}
@@ -437,7 +459,7 @@ void loop(void)
if (transmitPayload)
{
radioNode.handleTx(&qsp);
radioNode.handleTx(&qsp, platformNode.bindKey);
}
#ifdef DEVICE_MODE_TX

45
crossbow/qsp.cpp
View File

@@ -3,16 +3,16 @@
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) {
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);
qsp->setRcChannelCallback(0, (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03), 1000);
qsp->setRcChannelCallback(1, (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F), 1000);
qsp->setRcChannelCallback(2, (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F), 1000);
qsp->setRcChannelCallback(3, (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF), 1000);
qsp->setRcChannelCallback(4, ((int) qsp->payload[5]) << 2, 1000);
qsp->setRcChannelCallback(5, ((int) qsp->payload[6]) << 2, 1000);
qsp->setRcChannelCallback(6, ((int) ((qsp->payload[7] >> 4) & 0b00001111)) << 6, 1000);
qsp->setRcChannelCallback(7, ((int) (qsp->payload[7] & 0b00001111)) << 6, 1000);
qsp->setRcChannelCallback(8, ((int) ((qsp->payload[8] >> 4) & 0b00001111)) << 6, 1000);
qsp->setRcChannelCallback(9, ((int) (qsp->payload[8] & 0b00001111)) << 6, 1000);
}
uint8_t get10bitHighShift(uint8_t channel) {
@@ -41,7 +41,7 @@ void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte)
qsp->crc = crc8_dvb_s2(qsp->crc, dataByte);
}
void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr) {
void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr, uint8_t platformState) {
qsp->payload[0] = rssi;
qsp->payload[1] = snr;
qsp->payload[2] = rxDeviceState->rxVoltage;
@@ -50,7 +50,7 @@ void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
uint8_t flags = 0;
if (platformNode.platformState == DEVICE_STATE_FAILSAFE) {
if (platformState == DEVICE_STATE_FAILSAFE) {
flags |= 0x01 << 0;
}
@@ -75,7 +75,7 @@ void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels)
{
for (uint8_t i = 0; i < noOfChannels; i++)
{
int cV = constrain(platformNode.getRcChannel(i), 1000, 2000) - 1000;
int cV = constrain(qsp->rcChannelGetCallback(i), 1000, 2000) - 1000;
uint16_t channelValue10 = cV & 0x03ff;
uint8_t channelValue8 = (cV >> 2) & 0xff;
@@ -133,10 +133,10 @@ void qspClearPayload(QspConfiguration_t *qsp)
/**
* Init CRC with salt based on 4 byte bind key
*/
void qspInitCrc(QspConfiguration_t *qsp) {
void qspInitCrc(QspConfiguration_t *qsp, uint8_t bindKey[]) {
qsp->crc = 0;
for (uint8_t i = 0; i < 4; i++) {
qspComputeCrc(qsp, platformNode.bindKey[i]);
qspComputeCrc(qsp, bindKey[i]);
}
}
@@ -144,7 +144,8 @@ void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
TxDeviceState_t *txDeviceState,
uint8_t bindKey[]
) {
static uint8_t frameId;
static uint8_t payloadLength;
@@ -153,7 +154,7 @@ void qspDecodeIncomingFrame(
if (qsp->protocolState == QSP_STATE_IDLE)
{
qspInitCrc(qsp);
qspInitCrc(qsp, bindKey);
qspClearPayload(qsp);
receivedPayload = 0;
qsp->frameDecodingStartedAt = millis();
@@ -201,9 +202,15 @@ void qspDecodeIncomingFrame(
/**
* Encode frame is corrent format and write to hardware
*/
void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel) {
void qspEncodeFrame(
QspConfiguration_t *qsp,
uint8_t buffer[],
uint8_t *size,
uint8_t radioChannel,
uint8_t bindKey[]
) {
//Salt CRC with bind key
qspInitCrc(qsp);
qspInitCrc(qsp, bindKey);
//Write frame type and length
// We are no longer sending payload length, so 4 bits are now free for other usages

10
crossbow/qsp.h
View File

@@ -1,9 +1,6 @@
#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);
@@ -11,15 +8,16 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
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 encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, uint8_t rssi, uint8_t snr, uint8_t platformState);
void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels);
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
TxDeviceState_t *txDeviceState,
uint8_t bindKey[]
);
void qspClearPayload(QspConfiguration_t *qsp);
void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel);
void qspEncodeFrame(QspConfiguration_t *qsp, uint8_t buffer[], uint8_t *size, uint8_t radioChannel, uint8_t bindKey[]);
void encodePingPayload(QspConfiguration_t *qsp, uint32_t currentMicros);

9
crossbow/radio_node.cpp
View File

@@ -68,7 +68,8 @@ uint32_t RadioNode::getChannelEntryMillis(void) {
void RadioNode::readAndDecode(
QspConfiguration_t *qsp,
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
TxDeviceState_t *txDeviceState,
uint8_t bindKey[]
) {
uint8_t tmpBuffer[MAX_PACKET_SIZE];
/*
@@ -78,7 +79,7 @@ void RadioNode::readAndDecode(
LoRa.read(tmpBuffer, bytesToRead);
for (int i = 0; i < bytesToRead; i++) {
qspDecodeIncomingFrame(qsp, tmpBuffer[i], rxDeviceState, txDeviceState);
qspDecodeIncomingFrame(qsp, tmpBuffer[i], rxDeviceState, txDeviceState, bindKey);
}
//After reading, flush radio buffer, we have no need for whatever might be over there
@@ -144,7 +145,7 @@ void RadioNode::handleTxDoneState(bool hop) {
}
}
void RadioNode::handleTx(QspConfiguration_t *qsp) {
void RadioNode::handleTx(QspConfiguration_t *qsp, uint8_t bindKey[]) {
if (!canTransmit) {
return;
@@ -155,7 +156,7 @@ void RadioNode::handleTx(QspConfiguration_t *qsp) {
LoRa.beginPacket();
//Prepare packet
qspEncodeFrame(qsp, tmpBuffer, &size, getChannel());
qspEncodeFrame(qsp, tmpBuffer, &size, getChannel(), bindKey);
//Sent it to radio in one SPI transaction
LoRa.write(tmpBuffer, size);
LoRa.endPacketAsync();

7
crossbow/radio_node.h
View File

@@ -24,18 +24,19 @@ class RadioNode {
void readAndDecode(
QspConfiguration_t *qsp,
RxDeviceState_t *rxDeviceState,
TxDeviceState_t *txDeviceState
TxDeviceState_t *txDeviceState,
uint8_t bindKey[]
);
uint8_t getChannel(void);
uint32_t getChannelEntryMillis(void);
void handleChannelDwell(void);
void handleTxDoneState(bool hop);
void handleTx(QspConfiguration_t *qsp);
void handleTx(QspConfiguration_t *qsp, uint8_t bindKey[]);
void set(
uint8_t power,
long bandwidth,
uint8_t spreadingFactor,
uint8_t codingRate,
uint8_t codingRate,
long frequency
);
volatile int8_t bytesToRead = -1;

27
crossbow/sbus.cpp
View File

@@ -1,5 +1,4 @@
#include "Arduino.h"
#include "variables.h"
#include "sbus.h"
#define SBUS_MIN_OFFSET 173
@@ -31,7 +30,7 @@ int mapSbusToChannel(int in) {
return (((long) in - 173l) * 1020l / 1638l) + 990;
}
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe, int (* rcChannelGetCallback)(uint8_t)) {
int output[SBUS_CHANNEL_NUMBER];
@@ -40,7 +39,7 @@ void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){
* 173-1811 with middle at 992 S.BUS protocol requires
*/
for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) {
output[i] = mapChannelToSbus(platformNode.getRcChannel(i));
output[i] = mapChannelToSbus(rcChannelGetCallback(i));
}
uint8_t stateByte = 0x00;
@@ -115,18 +114,18 @@ void SbusInput::recoverStuckFrames(void)
}
}
void sbusToChannels(byte buffer[]) {
void SbusInput::sbusToChannels(byte buffer[]) {
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);
setRcChannelCallback(0, mapSbusToChannel((buffer[1] | buffer[2]<<8) & 0x07FF), 0);
setRcChannelCallback(1, mapSbusToChannel((buffer[2]>>3 | buffer[3]<<5) & 0x07FF), 0);
setRcChannelCallback(2, mapSbusToChannel((buffer[3]>>6 | buffer[4]<<2 | buffer[5]<<10) & 0x07FF), 0);
setRcChannelCallback(3, mapSbusToChannel((buffer[5]>>1 | buffer[6]<<7) & 0x07FF), 0);
setRcChannelCallback(4, mapSbusToChannel((buffer[6]>>4 | buffer[7]<<4) & 0x07FF), 0);
setRcChannelCallback(5, mapSbusToChannel((buffer[7]>>7 | buffer[8]<<1 |buffer[9]<<9) & 0x07FF), 0);
setRcChannelCallback(6, mapSbusToChannel((buffer[9]>>2 | buffer[10]<<6) & 0x07FF), 0);
setRcChannelCallback(7, mapSbusToChannel((buffer[10]>>5 | buffer[11]<<3) & 0x07FF), 0);
setRcChannelCallback(8, mapSbusToChannel((buffer[12] | buffer[13]<<8) & 0x07FF), 0);
setRcChannelCallback(9, mapSbusToChannel((buffer[13]>>3 | buffer[14]<<5) & 0x07FF), 0);
//We use only 10 channels, so the reset can be just ignored
}

6
crossbow/sbus.h
View File

@@ -19,15 +19,17 @@ class SbusInput : public TxInput
void loop(void);
bool isReceiving(void);
void recoverStuckFrames(void);
void (* setRcChannelCallback)(uint8_t channel, int value, int offset);
private:
HardwareSerial &_serial;
uint32_t _frameDecodingStartedAt = 0;
uint32_t _frameDecodingEndedAt = 0 ;
uint8_t _protocolState = SBUS_DECODING_STATE_IDLE;
void sbusRead(void);
void sbusRead(void);
void sbusToChannels(byte buffer[]);
};
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe);
void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe, int (* rcChannelGetCallback)(uint8_t));
#endif

3
crossbow/variables.h
View File

@@ -1,5 +1,6 @@
#include "Arduino.h"
#include "radio_node.h"
#include "platform_node.h"
#pragma once
@@ -101,6 +102,8 @@ struct QspConfiguration_t {
uint32_t anyFrameRecivedAt = 0;
void (* onSuccessCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*, uint8_t receivedChannel);
void (* onFailureCallback)(QspConfiguration_t*, TxDeviceState_t*, RxDeviceState_t*);
int (* rcChannelGetCallback)(uint8_t);
void (* setRcChannelCallback)(uint8_t channel, int value, int offset);
bool forcePongFrame = false;
uint32_t frameDecodingStartedAt = 0;
uint32_t lastTxSlotTimestamp = 0;