Merge pull request #47 from unitware/conditional-ppm-input-on-tx

Hardware and configuration selections and travis build server setup
This commit is contained in:
Paweł Spychalski
2018-02-12 15:48:13 +01:00
committed by GitHub
18 changed files with 229 additions and 91 deletions

5
.gitignore vendored
View File

@@ -1 +1,4 @@
build build
ppm_reader.*
config_tx_ppm.h
.DS_Store

33
.travis.yml Normal file
View File

@@ -0,0 +1,33 @@
language: c
env:
matrix:
- BOARD="adafruit:samd:adafruit_feather_m0" CONFIG="tx_sbus"
- BOARD="adafruit:samd:adafruit_feather_m0" CONFIG="rx"
- BOARD="adafruit:avr:feather32u4" CONFIG="tx_sbus"
- BOARD="adafruit:avr:feather32u4" CONFIG="rx"
before_install:
- "/sbin/start-stop-daemon --start --quiet --pidfile /tmp/custom_xvfb_1.pid --make-pidfile --background --exec /usr/bin/Xvfb -- :1 -ac -screen 0 1280x1024x16"
- sleep 3
- export DISPLAY=:1.0
- wget http://downloads.arduino.cc/arduino-1.8.5-linux64.tar.xz
- tar xf arduino-1.8.5-linux64.tar.xz
- sudo mv arduino-1.8.5 /usr/local/share/arduino
- sudo ln -s /usr/local/share/arduino/arduino /usr/local/bin/arduino
- arduino --pref "boardsmanager.additional.urls=https://adafruit.github.io/arduino-board-index/package_adafruit_index.json" --save-prefs
- if [[ "$BOARD" =~ "adafruit:avr:" ]]; then
arduino --install-boards adafruit:avr > /tmp/install_boards.log || cat /tmp/install_boards.log ;
fi
- if [[ "$BOARD" =~ "adafruit:samd:" ]]; then
arduino --install-boards arduino:samd > /tmp/install_boards.log || cat /tmp/install_boards.log ;
arduino --install-boards adafruit:samd > /tmp/install_boards.log || cat /tmp/install_boards.log ;
fi
- arduino --pref "boardsmanager.additional.urls=" --save-prefs
install:
- arduino --install-library "Adafruit SSD1306","Adafruit GFX Library"
script:
- cp -f $PWD/crossbow/configurations/config_${CONFIG}.h $PWD/crossbow/config.h &&
arduino --verify --board $BOARD $PWD/crossbow/crossbow.ino
notifications:
email:
on_success: change
on_failure: change

17
crossbow/config.h Normal file
View File

@@ -0,0 +1,17 @@
#ifndef CONFIG_H
#define CONFIG_H
#define DEVICE_MODE_TX
// #define DEVICE_MODE_RX
#define FEATURE_TX_OLED
// #define FORCE_TX_WITHOUT_INPUT
#define FEATURE_TX_INPUT_PPM
// #define FEATURE_TX_INPUT_SBUS
// #define DEBUG_SERIAL
// #define DEBUG_PING_PONG
// #define DEBUG_LED
// #define DEBUG_TX_INPUT_ON_OLED
#endif

View File

@@ -0,0 +1,6 @@
#ifndef CONFIG_H
#define CONFIG_H
#define DEVICE_MODE_RX
#endif

View File

@@ -0,0 +1,9 @@
#ifndef CONFIG_H
#define CONFIG_H
#define DEVICE_MODE_TX
#define FEATURE_TX_OLED
#define FEATURE_TX_INPUT_SBUS
#endif

View File

@@ -1,12 +1,13 @@
#define DEVICE_MODE_TX /*
// #define DEVICE_MODE_RX This Source Code Form is subject to the terms of the Mozilla Public
License, v. 2.0. If a copy of the MPL was not distributed with this file,
You can obtain one at http://mozilla.org/MPL/2.0/.
// #define FEATURE_TX_OLED Copyright (c) 20xx, MPL Contributor1 contrib1@example.net
// #define FORCE_TX_WITHOUT_INPUT */
#include "config.h"
#define DEBUG_SERIAL
// #define DEBUG_PING_PONG
// #define DEBUG_LED
#include "lora.h" #include "lora.h"
#include "variables.h" #include "variables.h"
@@ -14,27 +15,44 @@
#include "qsp.h" #include "qsp.h"
#include "sbus.h" #include "sbus.h"
// LoRa32u4 ports #ifdef ARDUINO_AVR_FEATHER32U4
#define LORA32U4_SS_PIN 8 #define LORA_SS_PIN 8
#define LORA32U4_RST_PIN 4 #define LORA_RST_PIN 4
#define LORA32U4_DI0_PIN 7 #define LORA_DI0_PIN 7
#elif defined(ARDUINO_SAMD_FEATHER_M0)
#define LORA_SS_PIN 8
#define LORA_RST_PIN 4
#define LORA_DI0_PIN 3
#else
#error please select hardware
#endif
/* /*
* Main defines for device working in TX mode * Main defines for device working in TX mode
*/ */
#ifdef DEVICE_MODE_TX #ifdef DEVICE_MODE_TX
// #include <PPMReader.h>
// PPMReader ppmReader(PPM_INPUT_PIN, PPM_INPUT_INTERRUPT, true);
SbusInput_t sbusInput = {}; #ifdef FEATURE_TX_INPUT_PPM
#include "ppm_reader.h"
PPM_Reader txInput(PPM_INPUT_PIN, true);
#elif defined(FEATURE_TX_INPUT_SBUS)
#include "sbus.h"
SbusInput txInput(Serial1);
#else
#error please select tx input source
#endif
volatile int16_t TxInput::channels[TX_INPUT_CHANNEL_COUNT];
// FUTABA_SBUS sBus;
#include "txbuzzer.h" #include "txbuzzer.h"
BuzzerState_t buzzer; BuzzerState_t buzzer;
#ifdef FEATURE_TX_OLED #ifdef FEATURE_TX_OLED
#include "Wire.h"
#define OLED_RESET -1 #define OLED_RESET -1
#include <Adafruit_SSD1306.h> #include <Adafruit_SSD1306.h>
@@ -129,7 +147,7 @@ void setup(void)
#ifdef DEVICE_MODE_RX #ifdef DEVICE_MODE_RX
qsp.deviceState = DEVICE_STATE_FAILSAFE; qsp.deviceState = DEVICE_STATE_FAILSAFE;
#else #else
qsp.deviceState = DEVICE_STATE_OK; qsp.deviceState = DEVICE_STATE_OK;
#endif #endif
@@ -137,11 +155,11 @@ void setup(void)
* Setup hardware * Setup hardware
*/ */
LoRa.setPins( LoRa.setPins(
LORA32U4_SS_PIN, LORA_SS_PIN,
LORA32U4_RST_PIN, LORA_RST_PIN,
LORA32U4_DI0_PIN LORA_DI0_PIN
); );
if (!LoRa.begin(radioState.frequency)) if (!LoRa.begin(radioState.frequency))
{ {
#ifdef DEBUG_SERIAL #ifdef DEBUG_SERIAL
@@ -156,7 +174,7 @@ void setup(void)
LoRa.setCodingRate4(radioState.loraCodingRate); LoRa.setCodingRate4(radioState.loraCodingRate);
LoRa.setTxPower(radioState.loraTxPower); LoRa.setTxPower(radioState.loraTxPower);
LoRa.enableCrc(); LoRa.enableCrc();
//Setup ISR callback and start receiving //Setup ISR callback and start receiving
LoRa.onReceive(onReceive); LoRa.onReceive(onReceive);
LoRa.receive(); LoRa.receive();
@@ -182,7 +200,6 @@ void setup(void)
#ifdef DEVICE_MODE_TX #ifdef DEVICE_MODE_TX
#ifdef FEATURE_TX_OLED #ifdef FEATURE_TX_OLED
Wire.setClock(400000); Wire.setClock(400000);
display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32) display.begin(SSD1306_SWITCHCAPVCC, 0x3C); // initialize with the I2C addr 0x3C (for the 128x32)
@@ -205,7 +222,7 @@ void setup(void)
/* /*
* Prepare Serial1 for S.Bus processing * Prepare Serial1 for S.Bus processing
*/ */
Serial1.begin(100000, SERIAL_8N2); txInput.start();
#endif #endif
pinMode(LED_BUILTIN, OUTPUT); pinMode(LED_BUILTIN, OUTPUT);
@@ -228,17 +245,17 @@ void updateRxDeviceState(RxDeviceState_t *rxDeviceState) {
rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255); rxDeviceState->rxVoltage = map(analogRead(RX_ADC_PIN_1), 0, 1024, 0, 255);
rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255); rxDeviceState->a1Voltage = map(analogRead(RX_ADC_PIN_2), 0, 1024, 0, 255);
rxDeviceState->a2Voltage = map(analogRead(RX_ADC_PIN_3), 0, 1024, 0, 255); rxDeviceState->a2Voltage = map(analogRead(RX_ADC_PIN_3), 0, 1024, 0, 255);
} }
int8_t getFrameToTransmit(QspConfiguration_t *qsp) { int8_t getFrameToTransmit(QspConfiguration_t *qsp) {
if (qsp->forcePongFrame) { if (qsp->forcePongFrame) {
qsp->forcePongFrame = false; qsp->forcePongFrame = false;
return QSP_FRAME_PONG; return QSP_FRAME_PONG;
} }
int8_t retVal = rxSendSequence[currentSequenceIndex]; int8_t retVal = rxSendSequence[currentSequenceIndex];
currentSequenceIndex++; currentSequenceIndex++;
if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) { if (currentSequenceIndex >= TRANSMIT_SEQUENCE_COUNT) {
currentSequenceIndex = 0; currentSequenceIndex = 0;
@@ -263,9 +280,9 @@ int8_t getFrameToTransmit(QspConfiguration_t *qsp) {
#endif #endif
/* /*
* *
* Main loop starts here! * Main loop starts here!
* *
*/ */
void loop(void) void loop(void)
{ {
@@ -278,7 +295,7 @@ void loop(void)
*/ */
if ( if (
currentMillis > radioState.nextTxCheckMillis && currentMillis > radioState.nextTxCheckMillis &&
radioState.deviceState == RADIO_STATE_TX && radioState.deviceState == RADIO_STATE_TX &&
!LoRa.isTransmitting() !LoRa.isTransmitting()
) { ) {
LoRa.receive(); LoRa.receive();
@@ -290,9 +307,9 @@ void loop(void)
LoRa.read(tmpBuffer, radioState.bytesToRead); LoRa.read(tmpBuffer, radioState.bytesToRead);
for (int i = 0; i < radioState.bytesToRead; i++) { for (int i = 0; i < radioState.bytesToRead; i++) {
qspDecodeIncomingFrame(&qsp, tmpBuffer[i], &rxDeviceState, &txDeviceState, &radioState); qspDecodeIncomingFrame(&qsp, tmpBuffer[i], &rxDeviceState, &txDeviceState, &radioState);
} }
radioState.rssi = getRadioRssi(); radioState.rssi = getRadioRssi();
radioState.snr = getRadioSnr(); radioState.snr = getRadioSnr();
@@ -310,24 +327,22 @@ void loop(void)
* Watchdog for frame decoding stuck somewhere in the middle of a process * Watchdog for frame decoding stuck somewhere in the middle of a process
*/ */
if ( if (
qsp.protocolState != QSP_STATE_IDLE && qsp.protocolState != QSP_STATE_IDLE &&
qsp.frameDecodingStartedAt + QSP_MAX_FRAME_DECODE_TIME < currentMillis qsp.frameDecodingStartedAt + QSP_MAX_FRAME_DECODE_TIME < currentMillis
) { ) {
qsp.protocolState = QSP_STATE_IDLE; qsp.protocolState = QSP_STATE_IDLE;
} }
#ifdef DEVICE_MODE_TX #ifdef DEVICE_MODE_TX
if (Serial1.available()) { txInput.loop();
sbusRead(Serial1, &sbusInput);
}
if ( if (
radioState.deviceState == RADIO_STATE_RX && radioState.deviceState == RADIO_STATE_RX &&
qsp.protocolState == QSP_STATE_IDLE && qsp.protocolState == QSP_STATE_IDLE &&
qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis qsp.lastTxSlotTimestamp + TX_TRANSMIT_SLOT_RATE < currentMillis
) { ) {
int8_t frameToSend = getFrameToTransmit(&qsp); int8_t frameToSend = getFrameToTransmit(&qsp);
#ifndef FORCE_TX_WITHOUT_INPUT #ifndef FORCE_TX_WITHOUT_INPUT
@@ -335,7 +350,7 @@ void loop(void)
* If module is not receiving data from radio, do not send RC DATA * If module is not receiving data from radio, do not send RC DATA
* This is the only way to trigger failsafe in that case * This is the only way to trigger failsafe in that case
*/ */
if (frameToSend == QSP_FRAME_RC_DATA && !isReceivingSbus(&sbusInput)) { if (frameToSend == QSP_FRAME_RC_DATA && !txInput.isReceiving()) {
frameToSend = -1; frameToSend = -1;
} }
#endif #endif
@@ -351,7 +366,7 @@ void loop(void)
break; break;
case QSP_FRAME_RC_DATA: case QSP_FRAME_RC_DATA:
encodeRcDataPayload(&qsp, sbusInput.channels, PPM_INPUT_CHANNEL_COUNT); encodeRcDataPayload(&qsp, txInput.channels, PPM_INPUT_CHANNEL_COUNT);
break; break;
} }
@@ -381,7 +396,7 @@ void loop(void)
digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN)); digitalWrite(LED_BUILTIN, !digitalRead(LED_BUILTIN));
} }
} }
/* /*
* Main routine to answer to TX module * Main routine to answer to TX module
*/ */
@@ -443,10 +458,10 @@ void loop(void)
} }
#ifdef DEVICE_MODE_TX #ifdef DEVICE_MODE_TX
buzzerProcess(TX_BUZZER_PIN, currentMillis, &buzzer); buzzerProcess(TX_BUZZER_PIN, currentMillis, &buzzer);
// This routing enables when TX starts to receive signal from RX for a first time or after // This routing enables when TX starts to receive signal from RX for a first time or after
// failsafe // failsafe
if (txDeviceState.isReceiving == false && qsp.anyFrameRecivedAt != 0) { if (txDeviceState.isReceiving == false && qsp.anyFrameRecivedAt != 0) {
//TX module started to receive data //TX module started to receive data
@@ -457,7 +472,7 @@ void loop(void)
//Here we detect failsafe state on TX module //Here we detect failsafe state on TX module
if ( if (
txDeviceState.isReceiving && txDeviceState.isReceiving &&
qsp.anyFrameRecivedAt + TX_FAILSAFE_DELAY < currentMillis qsp.anyFrameRecivedAt + TX_FAILSAFE_DELAY < currentMillis
) { ) {
txDeviceState.isReceiving = false; txDeviceState.isReceiving = false;
@@ -472,7 +487,7 @@ void loop(void)
qsp.anyFrameRecivedAt = 0; qsp.anyFrameRecivedAt = 0;
} }
//FIXME rxDeviceState should be resetted also in RC_HEALT frame is not received in a long period //FIXME rxDeviceState should be resetted also in RC_HEALT frame is not received in a long period
//Handle audible alarms //Handle audible alarms
if (qsp.deviceState == DEVICE_STATE_FAILSAFE) { if (qsp.deviceState == DEVICE_STATE_FAILSAFE) {
@@ -494,36 +509,41 @@ void loop(void)
currentMillis - lastOledTaskTime > OLED_UPDATE_RATE currentMillis - lastOledTaskTime > OLED_UPDATE_RATE
) { ) {
lastOledTaskTime = currentMillis; lastOledTaskTime = currentMillis;
display.clearDisplay(); display.clearDisplay();
display.setTextColor(WHITE, BLACK); display.setTextColor(WHITE, BLACK);
display.setCursor(0, 0); display.setCursor(0, 0);
display.setTextSize(3); display.setTextSize(3);
display.print(radioState.rssi); display.print(radioState.rssi);
display.setCursor(18, 28); display.setCursor(18, 28);
display.setTextSize(2); display.setTextSize(2);
display.print(radioState.snr); display.print(radioState.snr);
display.setCursor(74, 0); display.setCursor(74, 0);
display.setTextSize(3); display.setTextSize(3);
display.print(rxDeviceState.rssi); display.print(rxDeviceState.rssi);
display.setCursor(92, 28); display.setCursor(92, 28);
display.setTextSize(2); display.setTextSize(2);
display.print(rxDeviceState.snr); display.print(rxDeviceState.snr);
#ifdef DEBUG_TX_INPUT_ON_OLED
display.setCursor(0, 48);
display.setTextSize(2);
display.print(txInput.channels[0]);
#endif
display.setCursor(54, 48); display.setCursor(54, 48);
display.setTextSize(2); display.setTextSize(2);
display.print(txDeviceState.roundtrip); display.print(txDeviceState.roundtrip);
display.display(); display.display();
} }
#endif #endif
#endif #endif
} }

View File

@@ -1,6 +1,5 @@
#include "Arduino.h" #include "Arduino.h"
#include "variables.h" #include "variables.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) { void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) {
int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0}; int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0};
@@ -93,7 +92,7 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta
/** /**
* Encode 10 RC channels * Encode 10 RC channels
*/ */
void encodeRcDataPayload(QspConfiguration_t *qsp, int channels[], uint8_t noOfChannels) void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels)
{ {
for (uint8_t i = 0; i < noOfChannels; i++) for (uint8_t i = 0; i < noOfChannels; i++)
{ {

View File

@@ -1,5 +1,4 @@
#include "Arduino.h" #include "Arduino.h"
#include <PPMReader.h>
void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate); void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate);
void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState); void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState);
@@ -8,7 +7,7 @@ uint8_t get10bitHighShift(uint8_t channel);
uint8_t get10bitLowShift(uint8_t channel); uint8_t get10bitLowShift(uint8_t channel);
void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte); void qspComputeCrc(QspConfiguration_t *qsp, uint8_t dataByte);
void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, volatile RadioState_t *radioState); void encodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceState, volatile RadioState_t *radioState);
void encodeRcDataPayload(QspConfiguration_t *qsp, int channels[], uint8_t noOfChannels); void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels);
void qspDecodeIncomingFrame( void qspDecodeIncomingFrame(
QspConfiguration_t *qsp, QspConfiguration_t *qsp,
uint8_t incomingByte, uint8_t incomingByte,

View File

@@ -1,5 +1,6 @@
#include "Arduino.h" #include "Arduino.h"
#include "variables.h" #include "variables.h"
#include "sbus.h"
#define SBUS_MIN_OFFSET 173 #define SBUS_MIN_OFFSET 173
#define SBUS_MID_OFFSET 992 #define SBUS_MID_OFFSET 992
@@ -28,7 +29,7 @@ int mapSbusToChannel(int in) {
return map(in, 173, 1811, 990, 2010); return map(in, 173, 1811, 990, 2010);
} }
void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool isFailsafe){ void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe){
static int output[SBUS_CHANNEL_NUMBER] = {0}; static int output[SBUS_CHANNEL_NUMBER] = {0};
@@ -76,7 +77,24 @@ void sbusPreparePacket(uint8_t packet[], int channels[], bool isSignalLoss, bool
packet[24] = SBUS_FRAME_FOOTER; //Footer packet[24] = SBUS_FRAME_FOOTER; //Footer
} }
void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput) { SbusInput::SbusInput(HardwareSerial &serial) : _serial(serial)
{
}
void SbusInput::loop(void)
{
if (_serial.available()) {
sbusRead();
}
}
void SbusInput::start(void)
{
_serial.begin(100000, SERIAL_8N2);
}
void SbusInput::sbusRead() {
static byte buffer[25]; static byte buffer[25];
static byte buffer_index = 0; static byte buffer_index = 0;
@@ -103,33 +121,32 @@ void sbusRead(HardwareSerial &_serial, SbusInput_t *sbusInput) {
} }
_goodFrames++; _goodFrames++;
sbusInput->channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF); channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF);
sbusInput->channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF); channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF);
sbusInput->channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF); channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF);
sbusInput->channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF); channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF);
sbusInput->channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF); channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF);
sbusInput->channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF); channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF);
sbusInput->channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF); channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF);
sbusInput->channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF); channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF);
sbusInput->channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF); channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF);
sbusInput->channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF); channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF);
sbusInput->channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF); channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF);
sbusInput->channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF); channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF);
sbusInput->channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF); channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF);
sbusInput->channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF); channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF);
sbusInput->channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF); channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF);
sbusInput->channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF); channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF);
for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) { for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) {
sbusInput->channels[channelIndex] = mapSbusToChannel(sbusInput->channels[channelIndex]); channels[channelIndex] = mapSbusToChannel(channels[channelIndex]);
} }
sbusInput->lastChannelReceivedAt = millis(); _lastChannelReceivedAt = millis();
} }
} }
} }
bool isReceivingSbus(SbusInput_t *sbusInput) { bool SbusInput::isReceiving() {
return !(millis() - sbusInput->lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD); return !(millis() - _lastChannelReceivedAt > SBUS_IS_RECEIVING_THRESHOLD);
} }

25
crossbow/sbus.h Normal file
View File

@@ -0,0 +1,25 @@
#ifndef SBUS_INPUT
#define SBUS_INPUT
#include "Arduino.h"
#include "tx_input.h"
class SbusInput : public TxInput
{
public:
SbusInput(HardwareSerial &serial);
void start(void);
void loop(void);
bool isReceiving(void);
private:
HardwareSerial &_serial;
uint32_t _lastChannelReceivedAt = 0;
void sbusRead(void);
};
void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe);
#endif

21
crossbow/tx_input.h Normal file
View File

@@ -0,0 +1,21 @@
#ifndef TX_INPUT_H
#define TX_INPUT_H
#include "Arduino.h"
#define TX_INPUT_CHANNEL_COUNT 16
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

View File

@@ -55,7 +55,6 @@ enum debugConfigFlags {
}; };
#define PPM_INPUT_PIN 0 // Has to be one of Interrupt pins #define PPM_INPUT_PIN 0 // Has to be one of Interrupt pins
#define PPM_INPUT_INTERRUPT 2 // For Pro Micro 1, For Pro Mini 0
#define PPM_INPUT_CHANNEL_COUNT 10 #define PPM_INPUT_CHANNEL_COUNT 10
#define PPM_OUTPUT_CHANNEL_COUNT 10 #define PPM_OUTPUT_CHANNEL_COUNT 10
@@ -125,8 +124,3 @@ struct QspConfiguration_t {
uint32_t lastTxSlotTimestamp = 0; uint32_t lastTxSlotTimestamp = 0;
bool transmitWindowOpen = false; bool transmitWindowOpen = false;
}; };
struct SbusInput_t {
int16_t channels[16] = {};
uint32_t lastChannelReceivedAt = 0;
};

5
sbus.h
View File

@@ -1,5 +0,0 @@
#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);