From 5ff36034b87ee7e6f153e3b8a4399e9113897df4 Mon Sep 17 00:00:00 2001 From: Pawel Spychalski Date: Thu, 17 May 2018 14:13:41 +0200 Subject: [PATCH] platform node --- .vscode/settings.json | 3 ++- crossbow/crossbow.ino | 15 +++++---------- crossbow/platform_node.cpp | 21 +++++++++++++++++++++ crossbow/platform_node.h | 22 ++++++++++++++++++++++ crossbow/qsp.cpp | 45 ++++++++++++--------------------------------- crossbow/qsp.h | 5 ++++- crossbow/sbus.cpp | 42 +++++++++++++++++------------------------- crossbow/sbus.h | 2 +- crossbow/tx_input.h | 2 -- crossbow/variables.h | 2 -- 10 files changed, 84 insertions(+), 75 deletions(-) create mode 100644 crossbow/platform_node.cpp create mode 100644 crossbow/platform_node.h diff --git a/.vscode/settings.json b/.vscode/settings.json index 72a79e6..fb7bbfe 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -8,7 +8,8 @@ "iterator": "cpp", "rope": "c", "__locale": "c", - "string": "c" + "string": "c", + "ios": "cpp" }, "files.exclude": { "**/build": true diff --git a/crossbow/crossbow.ino b/crossbow/crossbow.ino index 4cb0d94..06fef37 100644 --- a/crossbow/crossbow.ino +++ b/crossbow/crossbow.ino @@ -14,6 +14,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net #include "main_variables.h" #include "qsp.h" #include "sbus.h" +#include "platform_node.h" #ifdef ARDUINO_AVR_FEATHER32U4 #define LORA_SS_PIN 8 @@ -34,6 +35,7 @@ Copyright (c) 20xx, MPL Contributor1 contrib1@example.net #endif RadioNode radioNode; +PlatformNode platformNode; /* * Main defines for device working in TX mode @@ -52,8 +54,6 @@ RadioNode radioNode; #error please select tx input source #endif -volatile int16_t TxInput::channels[TX_INPUT_CHANNEL_COUNT]; - #include "txbuzzer.h" BuzzerState_t buzzer; @@ -163,11 +163,6 @@ void setup(void) radioNode.init(LORA_SS_PIN, LORA_RST_PIN, LORA_DI0_PIN, onReceive); #ifdef DEVICE_MODE_RX - //initiallize default ppm values - for (int i = 0; i < 16; i++) - { - rxDeviceState.channels[i] = PPM_CHANNEL_DEFAULT_VALUE; - } pinMode(RX_ADC_PIN_1, INPUT); pinMode(RX_ADC_PIN_2, INPUT); @@ -373,7 +368,7 @@ void loop(void) break; case QSP_FRAME_RC_DATA: - encodeRcDataPayload(&qsp, txInput.channels, PPM_INPUT_CHANNEL_COUNT); + encodeRcDataPayload(&qsp, PLATFORM_CHANNEL_COUNT); break; } @@ -434,9 +429,9 @@ void loop(void) } if (currentMillis > sbusTime) { - rxDeviceState.channels[RSSI_CHANNEL - 1] = rxDeviceState.indicatedRssi; + platformNode.setRcChannel(RSSI_CHANNEL - 1, rxDeviceState.indicatedRssi, 0); - sbusPreparePacket(sbusPacket, rxDeviceState.channels, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); + sbusPreparePacket(sbusPacket, false, (qsp.deviceState == DEVICE_STATE_FAILSAFE)); Serial1.write(sbusPacket, SBUS_PACKET_LENGTH); sbusTime = currentMillis + SBUS_UPDATE_RATE; } diff --git a/crossbow/platform_node.cpp b/crossbow/platform_node.cpp new file mode 100644 index 0000000..d822fdd --- /dev/null +++ b/crossbow/platform_node.cpp @@ -0,0 +1,21 @@ +#include "platform_node.h" + +PlatformNode::PlatformNode(void) { + for (uint8_t i = 0; i < PLATFORM_TOTAL_CHANNEL_COUNT; i++) { + _channels[i] = PLATFORM_DEFAULT_CHANNEL_VALUE; + } +} + +int PlatformNode::getRcChannel(uint8_t channel) { + if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) { + return _channels[channel]; + } else { + return PLATFORM_DEFAULT_CHANNEL_VALUE; + } +} + +void PlatformNode::setRcChannel(uint8_t channel, int value, int offset) { + if (channel < PLATFORM_TOTAL_CHANNEL_COUNT) { + _channels[channel] = value + offset; + } +} \ No newline at end of file diff --git a/crossbow/platform_node.h b/crossbow/platform_node.h new file mode 100644 index 0000000..15b147a --- /dev/null +++ b/crossbow/platform_node.h @@ -0,0 +1,22 @@ +#pragma once + +#include "Arduino.h" + +#ifndef PLATFORM_NODE_H +#define PLATFORM_NODE_H + +#define PLATFORM_TOTAL_CHANNEL_COUNT 11 //Including RSSI channel and other +#define PLATFORM_CHANNEL_COUNT 10 +#define PLATFORM_DEFAULT_CHANNEL_VALUE 1000 + +class PlatformNode { + + public: + PlatformNode(void); + int getRcChannel(uint8_t channel); + void setRcChannel(uint8_t channel, int value, int offset); + private: + volatile int _channels[PLATFORM_TOTAL_CHANNEL_COUNT]; +}; + +#endif \ No newline at end of file diff --git a/crossbow/qsp.cpp b/crossbow/qsp.cpp index fa4dc8d..51f00f7 100644 --- a/crossbow/qsp.cpp +++ b/crossbow/qsp.cpp @@ -2,38 +2,17 @@ #include "variables.h" void qspDecodeRcDataFrame(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSate) { - int temporaryPpmOutput[PPM_OUTPUT_CHANNEL_COUNT] = {0}; - //TODO fix it, baby :) - temporaryPpmOutput[0] = (uint16_t) (((uint16_t) qsp->payload[0] << 2) & 0x3fc) | ((qsp->payload[1] >> 6) & 0x03); - temporaryPpmOutput[1] = (uint16_t) (((uint16_t) qsp->payload[1] << 4) & 0x3f0) | ((qsp->payload[2] >> 4) & 0x0F); - temporaryPpmOutput[2] = (uint16_t) (((uint16_t) qsp->payload[2] << 6) & 0x3c0) | ((qsp->payload[3] >> 2) & 0x3F); - temporaryPpmOutput[3] = (uint16_t) (((uint16_t) qsp->payload[3] << 8) & 0x300) | ((qsp->payload[4]) & 0xFF); - temporaryPpmOutput[4] = qsp->payload[5]; - temporaryPpmOutput[5] = qsp->payload[6]; - temporaryPpmOutput[6] = (qsp->payload[7] >> 4) & 0b00001111; - temporaryPpmOutput[7] = qsp->payload[7] & 0b00001111; - temporaryPpmOutput[8] = (qsp->payload[8] >> 4) & 0b00001111; - temporaryPpmOutput[9] = qsp->payload[8] & 0b00001111; - - //10bit channels are passed as is - - //8bit channels needs to be shifted left 2 places - temporaryPpmOutput[4] = temporaryPpmOutput[4] << 2; - temporaryPpmOutput[5] = temporaryPpmOutput[5] << 2; - - //4bit channels needs to be shifted left 6 places - temporaryPpmOutput[6] = temporaryPpmOutput[6] << 6; - temporaryPpmOutput[7] = temporaryPpmOutput[7] << 6; - temporaryPpmOutput[8] = temporaryPpmOutput[8] << 6; - temporaryPpmOutput[9] = temporaryPpmOutput[9] << 6; - - /* - * Copy tremporary to real output and add missing 1000 - */ - for (uint8_t i = 0; i < PPM_OUTPUT_CHANNEL_COUNT; i++) { - rxDeviceSate->channels[i] = temporaryPpmOutput[i] + 1000; - } + 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); } uint8_t get10bitHighShift(uint8_t channel) { @@ -92,11 +71,11 @@ void decodeRxHealthPayload(QspConfiguration_t *qsp, RxDeviceState_t *rxDeviceSta /** * Encode 10 RC channels */ -void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels) +void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels) { for (uint8_t i = 0; i < noOfChannels; i++) { - int cV = constrain(channels[i], 1000, 2000) - 1000; + int cV = constrain(platformNode.getRcChannel(i), 1000, 2000) - 1000; uint16_t channelValue10 = cV & 0x03ff; uint8_t channelValue8 = (cV >> 2) & 0xff; diff --git a/crossbow/qsp.h b/crossbow/qsp.h index 8d6d29b..45fe66c 100644 --- a/crossbow/qsp.h +++ b/crossbow/qsp.h @@ -1,6 +1,9 @@ #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); @@ -9,7 +12,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, uint8_t rssi, uint8_t snr); -void encodeRcDataPayload(QspConfiguration_t *qsp, volatile int16_t channels[], uint8_t noOfChannels); +void encodeRcDataPayload(QspConfiguration_t *qsp, uint8_t noOfChannels); void qspDecodeIncomingFrame( QspConfiguration_t *qsp, uint8_t incomingByte, diff --git a/crossbow/sbus.cpp b/crossbow/sbus.cpp index dac4fe0..732572c 100644 --- a/crossbow/sbus.cpp +++ b/crossbow/sbus.cpp @@ -31,16 +31,16 @@ int mapSbusToChannel(int in) { return (((long) in - 173l) * 1020l / 1638l) + 990; } -void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe){ +void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe){ - static int output[SBUS_CHANNEL_NUMBER] = {0}; + int output[SBUS_CHANNEL_NUMBER]; /* * Map 1000-2000 with middle at 1500 chanel values to * 173-1811 with middle at 992 S.BUS protocol requires */ for (uint8_t i = 0; i < SBUS_CHANNEL_NUMBER; i++) { - output[i] = mapChannelToSbus(channels[i]); + output[i] = mapChannelToSbus(platformNode.getRcChannel(i)); } uint8_t stateByte = 0x00; @@ -83,7 +83,6 @@ SbusInput::SbusInput(HardwareSerial &serial) : _serial(serial) { } - void SbusInput::loop(void) { if (_serial.available()) { @@ -116,27 +115,20 @@ void SbusInput::recoverStuckFrames(void) } } -void sbusToChannels(volatile int16_t channels[], byte buffer[]) { - channels[0] = ((buffer[1] |buffer[2]<<8) & 0x07FF); - channels[1] = ((buffer[2]>>3 |buffer[3]<<5) & 0x07FF); - channels[2] = ((buffer[3]>>6 |buffer[4]<<2 |buffer[5]<<10) & 0x07FF); - channels[3] = ((buffer[5]>>1 |buffer[6]<<7) & 0x07FF); - channels[4] = ((buffer[6]>>4 |buffer[7]<<4) & 0x07FF); - channels[5] = ((buffer[7]>>7 |buffer[8]<<1 |buffer[9]<<9) & 0x07FF); - channels[6] = ((buffer[9]>>2 |buffer[10]<<6) & 0x07FF); - channels[7] = ((buffer[10]>>5|buffer[11]<<3) & 0x07FF); - channels[8] = ((buffer[12] |buffer[13]<<8) & 0x07FF); - channels[9] = ((buffer[13]>>3|buffer[14]<<5) & 0x07FF); - channels[10] = ((buffer[14]>>6|buffer[15]<<2|buffer[16]<<10) & 0x07FF); - channels[11] = ((buffer[16]>>1|buffer[17]<<7) & 0x07FF); - channels[12] = ((buffer[17]>>4|buffer[18]<<4) & 0x07FF); - channels[13] = ((buffer[18]>>7|buffer[19]<<1|buffer[20]<<9) & 0x07FF); - channels[14] = ((buffer[20]>>2|buffer[21]<<6) & 0x07FF); - channels[15] = ((buffer[21]>>5|buffer[22]<<3) & 0x07FF); +void sbusToChannels(byte buffer[]) { - for (uint8_t channelIndex = 0; channelIndex < SBUS_CHANNEL_NUMBER; channelIndex++) { - channels[channelIndex] = mapSbusToChannel(channels[channelIndex]); - } + 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); + + //We use only 10 channels, so the reset can be just ignored } void SbusInput::sbusRead() { @@ -169,7 +161,7 @@ void SbusInput::sbusRead() { ) { //We have full frame now _frameDecodingEndedAt = millis(); - sbusToChannels(channels, buffer); + sbusToChannels(buffer); _protocolState = SBUS_DECODING_STATE_IDLE; } } diff --git a/crossbow/sbus.h b/crossbow/sbus.h index e6a5b3b..29ab03b 100644 --- a/crossbow/sbus.h +++ b/crossbow/sbus.h @@ -27,7 +27,7 @@ class SbusInput : public TxInput void sbusRead(void); }; -void sbusPreparePacket(uint8_t packet[], int16_t channels[], bool isSignalLoss, bool isFailsafe); +void sbusPreparePacket(uint8_t packet[], bool isSignalLoss, bool isFailsafe); #endif diff --git a/crossbow/tx_input.h b/crossbow/tx_input.h index 5e56099..9043fd6 100644 --- a/crossbow/tx_input.h +++ b/crossbow/tx_input.h @@ -10,12 +10,10 @@ 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 diff --git a/crossbow/variables.h b/crossbow/variables.h index e71dc4a..140259a 100644 --- a/crossbow/variables.h +++ b/crossbow/variables.h @@ -69,7 +69,6 @@ enum deviceStates { #define PPM_INPUT_PIN 0 // Has to be one of Interrupt pins #define PPM_INPUT_CHANNEL_COUNT 10 -#define PPM_OUTPUT_CHANNEL_COUNT 10 #define TX_BUZZER_PIN A5 @@ -99,7 +98,6 @@ struct RxDeviceState_t { uint8_t a1Voltage = 0; uint8_t a2Voltage = 0; uint8_t flags = 0; - int16_t channels[16] = {}; int16_t indicatedRssi = 0; };