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

Merge branch 'master' into channel-processing-speedup

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2018-01-03 19:44:44 +01:00
parent fbbd5fc68e 34744ac5d7
commit c61a87361e
7 changed files with 100 additions and 26 deletions
Download Patch File Download Diff File Expand all files Collapse all files

11
.vscode/c_cpp_properties.json vendored
View File

@@ -3,19 +3,12 @@
{
"name": "Win32",
"includePath": [
"${workspaceRoot}",
"C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino",
"C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries",
"C:\\Users\\pspyc\\Documents\\Arduino\\libraries",
"C:\\Users\\pspyc\\Documents\\Arduino\\libraries\\PPMReader"
"${workspaceRoot}"
],
"browse": {
"limitSymbolsToIncludedHeaders": false,
"path": [
"C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\cores\\arduino",
"C:\\Program Files (x86)\\Arduino\\hardware\\arduino\\avr\\libraries",
"${workspaceRoot}",
"C:\\Users\\pspyc\\Documents\\Arduino\\libraries"
"${workspaceRoot}"
]
},
"intelliSenseMode": "msvc-x64"

35
crossbow.ino
View File

@@ -12,6 +12,7 @@
#include "variables.h"
#include "main_variables.h"
#include "qsp.h"
#include "sbus.h"
// LoRa32u4 ports
#define LORA32U4_SS_PIN 8
@@ -22,8 +23,12 @@
* Main defines for device working in TX mode
*/
#ifdef DEVICE_MODE_TX
#include <PPMReader.h>
PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
// #include <PPMReader.h>
// PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
SbusInput_t sbusInput = {};
// FUTABA_SBUS sBus;
#include "txbuzzer.h"
@@ -44,9 +49,6 @@ uint32_t lastOledTaskTime = 0;
* 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;
@@ -170,14 +172,9 @@ void setup(void)
pinMode(RX_ADC_PIN_1, INPUT);
pinMode(RX_ADC_PIN_2, INPUT);
pinMode(RX_ADC_PIN_3, INPUT);
Serial1.begin(100000, SERIAL_8E2);
#endif
#ifdef DEVICE_MODE_TX
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
@@ -210,6 +207,11 @@ void setup(void)
qsp.debugConfig |= DEBUG_FLAG_LED;
#endif
/*
* Prepare Serial1 for S.Bus processing
*/
Serial1.begin(100000, SERIAL_8N2);
// sBus.begin();
}
uint8_t currentSequenceIndex = 0;
@@ -311,6 +313,10 @@ void loop(void)
#ifdef DEVICE_MODE_TX
if (Serial1.available()) {
sbusRead(Serial1, &sbusInput);
}
if (
radioState.deviceState == RADIO_STATE_RX &&
qsp.protocolState == QSP_STATE_IDLE &&
@@ -320,9 +326,12 @@ void loop(void)
int8_t frameToSend = getFrameToTransmit(&qsp);
#ifndef FORCE_TX_WITHOUT_INPUT
if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) {
/*
* If module is not receiving data from radio, do not send RC DATA
* This is the only way to trigger failsafe in that case
*/
if (frameToSend == QSP_FRAME_RC_DATA && !isReceivingSbus(&sbusInput)) {
frameToSend = -1;
//FIXME uncomment to enable full Failsafe
}
#endif
@@ -337,7 +346,7 @@ void loop(void)
break;
case QSP_FRAME_RC_DATA:
encodeRcDataPayload(&qsp, &ppmReader, PPM_INPUT_CHANNEL_COUNT);
encodeRcDataPayload(&qsp, sbusInput.channels, PPM_INPUT_CHANNEL_COUNT);
break;
}

4
qsp.cpp
View File

@@ -93,11 +93,11 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
/**
* Encode 10 RC channels
*/
void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels)
void encodeRcDataPayload(QspConfiguration_t *qsp, int channels[], uint8_t noOfChannels)
{
for (uint8_t i = 0; i < noOfChannels; i++)
{
int cV = constrain(ppmSource->get(i), 1000, 2000) - 1000;
int cV = constrain(channels[i], 1000, 2000) - 1000;
uint16_t channelValue10 = cV & 0x03ff;
uint8_t channelValue8 = (cV >> 2) & 0xff;

2
qsp.h
View File

@@ -8,7 +8,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, volatile RadioState_t *radioState);
void encodeRcDataPayload(QspConfiguration_t *qsp, PPMReader *ppmSource, uint8_t noOfChannels);
void encodeRcDataPayload(QspConfiguration_t *qsp, int channels[], uint8_t noOfChannels);
void qspDecodeIncomingFrame(
QspConfiguration_t *qsp,
uint8_t incomingByte,

65
sbus.cpp
View File

@@ -12,6 +12,8 @@
#define SBUS_STATE_FAILSAFE 0x08
#define SBUS_STATE_SIGNALLOSS 0x04
#define SBUS_IS_RECEIVING_THRESHOLD 250 //If there is no SBUS input for 250ms, assume connection is broken
/*
Precomputed mapping from 990-2010 to 173:1811
equivalent to
@@ -21,6 +23,11 @@ int mapChannelToSbus(int in) {
return (((long) in * 1605l) / 1000l) - 1417;
}
int mapSbusToChannel(int in) {
//TODO, speed up this processing
return map(in, 173, 1811, 990, 2010);
}
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){
static int output[SBUS_CHANNEL_NUMBER] = {0};
@@ -68,3 +75,61 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool
packet[23] = stateByte; //Flags byte
packet[24] = SBUS_FRAME_FOOTER; //Footer
}
void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput) {
static byte buffer[25];
static byte buffer_index = 0;
static uint32_t _decoderErrorFrames;
static uint32_t _goodFrames;
while (_serial.available()) {
byte rx = _serial.read();
if (buffer_index == 0 && rx != SBUS_FRAME_HEADER) {
//incorrect start byte, out of sync
_decoderErrorFrames++;
continue;
}
buffer[buffer_index] = rx;
buffer_index++;
if (buffer_index == 25) {
buffer_index = 0;
if (buffer[24] != SBUS_FRAME_FOOTER) {
//incorrect end byte, out of sync
_decoderErrorFrames++;
continue;
}
_goodFrames++;
sbusInput->channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
sbusInput->channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
sbusInput->channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
sbusInput->channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
sbusInput->channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
sbusInput->channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
sbusInput->channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
sbusInput->channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
sbusInput->channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
sbusInput->channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
sbusInput->channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
sbusInput->channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
sbusInput->channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
sbusInput->channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
sbusInput->channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
sbusInput->channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
sbusInput->channels[channelIndex] = mapSbusToChannel(sbusInput->channels[channelIndex]);
}
sbusInput->lastChannelReceivedAt = millis();
}
}
}
bool isReceivingSbus(SbusInput_t *sbusInput) {
return !(millis() - sbusInput->lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD);
}

2
sbus.h
View File

@@ -1,3 +1,5 @@
#include "Arduino.h"
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe);
void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput);
bool isReceivingSbus(SbusInput_t *sbusInput);

5
variables.h
View File

@@ -125,3 +125,8 @@ struct QspConfiguration_t {
uint32_t lastTxSlotTimestamp = 0;
bool transmitWindowOpen = false;
};
struct SbusInput_t {
int16_t channels[16] = {};
uint32_t lastChannelReceivedAt = 0;
};