From baf8e54d1aa34b7552fd981d13ab4f35500e2a9d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 14 Dec 2017 14:54:17 +0100 Subject: [PATCH 1/2] Sbus decoding --- .vscode/arduino.json | 2 +- .vscode/c_cpp_properties.json | 3 +-- crossbow.ino | 38 +++++++++++++++----------- qsp.cpp | 4 +-- qsp.h | 2 +- sbus.cpp | 62 +++++++++++++++++++++++++++++++++++++++++++ sbus.h | 4 ++- variables.h | 5 ++++ 8 files changed, 97 insertions(+), 23 deletions(-) diff --git a/.vscode/arduino.json b/.vscode/arduino.json index e0b924b..488acc1 100644 --- a/.vscode/arduino.json +++ b/.vscode/arduino.json @@ -1,6 +1,6 @@ { "board": "bsfrance:avr:lora32u4", "sketch": "crossbow.ino", - "port": "COM17", + "port": "COM4", "output": "./build" } \ No newline at end of file diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 00c9b0c..74b2bcb 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -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, diff --git a/crossbow.ino b/crossbow.ino index 39fc969..7a41750 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -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 ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true); +// #include +// 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; } diff --git a/qsp.cpp b/qsp.cpp index 3315d12..7b504be 100644 --- a/qsp.cpp +++ b/qsp.cpp @@ -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; diff --git a/qsp.h b/qsp.h index e80f72f..e591628 100644 --- a/qsp.h +++ b/qsp.h @@ -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, diff --git a/sbus.cpp b/sbus.cpp index f8ebff6..b613b7f 100644 --- a/sbus.cpp +++ b/sbus.cpp @@ -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; + } + + } } \ No newline at end of file diff --git a/sbus.h b/sbus.h index 4004277..d9ac2c1 100644 --- a/sbus.h +++ b/sbus.h @@ -1,3 +1,5 @@ #include "Arduino.h" -void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe); \ No newline at end of file +void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe); +void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput); +bool isReceivingSbus(SbusInput_t *sbusInput); \ No newline at end of file diff --git a/variables.h b/variables.h index 2202804..0bf480a 100644 --- a/variables.h +++ b/variables.h @@ -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; }; \ No newline at end of file From 365df5b26979c7ef13370b646920dd4b5722a48f Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 3 Jan 2018 19:41:59 +0100 Subject: [PATCH 2/2] Improved failsafe state detection when TX module is not receiving input --- .vscode/c_cpp_properties.json | 11 ++--------- crossbow.ino | 7 +++++-- sbus.cpp | 11 +++++++---- 3 files changed, 14 insertions(+), 15 deletions(-) diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json index 705e4b4..56b262c 100644 --- a/.vscode/c_cpp_properties.json +++ b/.vscode/c_cpp_properties.json @@ -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" diff --git a/crossbow.ino b/crossbow.ino index 43f638a..16d44a7 100644 --- a/crossbow.ino +++ b/crossbow.ino @@ -323,8 +323,11 @@ void loop(void) int8_t frameToSend = getFrameToTransmit(&qsp); #ifndef FORCE_TX_WITHOUT_INPUT - //FIXME detect if taranis is really transmitting here - if (frameToSend == QSP_FRAME_RC_DATA && false) { + /* + * 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; } #endif diff --git a/sbus.cpp b/sbus.cpp index b613b7f..b9e08a2 100644 --- a/sbus.cpp +++ b/sbus.cpp @@ -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 @@ -22,6 +24,7 @@ int mapChannelToSbus(int in) { } int mapSbusToChannel(int in) { + //TODO, speed up this processing return map(in, 173, 1811, 990, 2010); } @@ -121,12 +124,12 @@ void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput) { 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; } } +} + +bool isReceivingSbus(SbusInput_t *sbusInput) { + return !(millis() - sbusInput->lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD); } \ No newline at end of file