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

Sbus decoding

Browse Source
This commit is contained in:
Pawel Spychalski (DzikuVx)
2017-12-14 14:54:17 +01:00
parent c97c5f2b22
commit baf8e54d1a
8 changed files with 97 additions and 23 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": "COM4",
"output": "./build"
}

3
.vscode/c_cpp_properties.json vendored
View File

@@ -3,8 +3,7 @@
{
"name": "Win32",
"includePath": [
"C:\\Users\\pspyc\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\avr\\1.6.19\\cores\\arduino",
"C:\\Users\\pspyc\\Documents\\Arduino\\libraries\\PPMReader"
"C:\\Users\\pspyc\\AppData\\Local\\Arduino15\\packages\\arduino\\hardware\\avr\\1.6.19\\cores\\arduino"
],
"browse": {
"limitSymbolsToIncludedHeaders": false,

38
crossbow.ino
View File

@@ -1,10 +1,10 @@
#define DEVICE_MODE_TX
// #define DEVICE_MODE_RX
#define FEATURE_TX_OLED
// #define FORCE_TX_WITHOUT_INPUT
// #define FEATURE_TX_OLED
#define FORCE_TX_WITHOUT_INPUT
// #define DEBUG_SERIAL
#define DEBUG_SERIAL
// #define DEBUG_PING_PONG
// #define DEBUG_LED
@@ -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;
@@ -177,14 +179,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
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
@@ -214,6 +211,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;
@@ -292,6 +294,10 @@ void loop(void)
#ifdef DEVICE_MODE_TX
if (Serial1.available()) {
sbusRead(Serial1, &sbusInput);
}
if (
qsp.protocolState == QSP_STATE_IDLE &&
qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
@@ -300,9 +306,9 @@ void loop(void)
int8_t frameToSend = getFrameToTransmit(&qsp);
#ifndef FORCE_TX_WITHOUT_INPUT
if (frameToSend == QSP_FRAME_RC_DATA && !ppmReader.isReceiving()) {
//FIXME detect if taranis is really transmitting here
if (frameToSend == QSP_FRAME_RC_DATA && false) {
frameToSend = -1;
//FIXME uncomment to enable full Failsafe
}
#endif
@@ -317,7 +323,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

@@ -97,11 +97,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);
int cV = constrain(channels[i], 1000, 2000);
uint16_t channelValue10 = map(cV, 1000, 2000, 0, 1000) & 0x03ff;
uint8_t channelValue8 = map(cV, 1000, 2000, 0, 255) & 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, 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,

62
sbus.cpp
View File

@@ -21,6 +21,10 @@ int mapChannelToSbus(int in) {
return (((long) in * 1605l) / 1000l) - 1417;
}
int mapSbusToChannel(int in) {
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};
@@ -67,4 +71,62 @@ 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]);
}
//FIXME remove this debug
static uint32_t prev = 0;
sbusInput->lastChannelReceivedAt = millis();
Serial.println(sbusInput->lastChannelReceivedAt - prev);
prev = sbusInput->lastChannelReceivedAt;
}
}
}

4
sbus.h
View File

@@ -1,3 +1,5 @@
#include "Arduino.h"
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe);
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

@@ -120,4 +120,9 @@ struct QspConfiguration_t {
uint32_t frameDecodingStartedAt = 0;
uint32_t lastTxSlotTimestamp = 0;
bool transmitWindowOpen = false;
};
struct SbusInput_t {
int16_t channels[16] = {};
uint32_t lastChannelReceivedAt = 0;
};