From cc39f67ae3e700a0b8e4b1a74f7c12086410d7a8 Mon Sep 17 00:00:00 2001 From: Dmytro Date: Thu, 29 Aug 2024 12:11:46 +0300 Subject: [PATCH 1/2] Introduced the following callbacks: - on connection down - on connection up - raw data received Signed-off-by: Dmytro --- src/CRSFforArduino.cpp | 15 +++++++++ src/CRSFforArduino.hpp | 4 +++ src/SerialReceiver/SerialReceiver.cpp | 47 ++++++++++++++++++++++++--- src/SerialReceiver/SerialReceiver.hpp | 21 ++++++++++++ 4 files changed, 83 insertions(+), 4 deletions(-) diff --git a/src/CRSFforArduino.cpp b/src/CRSFforArduino.cpp index 2a936b0d..7f34c294 100644 --- a/src/CRSFforArduino.cpp +++ b/src/CRSFforArduino.cpp @@ -176,6 +176,21 @@ namespace sketchLayer #endif } + void CRSFforArduino::setRawDataCallback(void (*callback)(int8_t byteReceived)) + { + this->SerialReceiver::setRawDataCallback(callback); + } + + void CRSFforArduino::setLinkUpCallback(void (*callback)()) + { + this->SerialReceiver::setLinkUpCallback(callback); + } + + void CRSFforArduino::setLinkDownCallback(void (*callback)()) + { + this->SerialReceiver::setLinkDownCallback(callback); + } + void CRSFforArduino::setLinkStatisticsCallback(void (*callback)(serialReceiverLayer::link_statistics_t linkStatistics)) { #if CRSF_LINK_STATISTICS_ENABLED > 0 diff --git a/src/CRSFforArduino.hpp b/src/CRSFforArduino.hpp index a9f9695d..444474b2 100644 --- a/src/CRSFforArduino.hpp +++ b/src/CRSFforArduino.hpp @@ -56,6 +56,10 @@ namespace sketchLayer bool setFlightMode(serialReceiverLayer::flightModeId_t flightModeId, const char *flightModeName, uint8_t channel, uint16_t min, uint16_t max); bool setFlightMode(serialReceiverLayer::flightModeId_t flightMode, uint8_t channel, uint16_t min, uint16_t max); void setFlightModeCallback(void (*callback)(serialReceiverLayer::flightModeId_t flightMode)); + void setRawDataCallback(void (*callback)(int8_t byteReceived)); + + void setLinkUpCallback(void (*callback)()); + void setLinkDownCallback(void (*callback)()); // Telemetry functions. void telemetryWriteAttitude(int16_t roll, int16_t pitch, int16_t yaw); diff --git a/src/SerialReceiver/SerialReceiver.cpp b/src/SerialReceiver/SerialReceiver.cpp index 58649378..8c1dd983 100644 --- a/src/SerialReceiver/SerialReceiver.cpp +++ b/src/SerialReceiver/SerialReceiver.cpp @@ -33,7 +33,7 @@ using namespace hal; namespace serialReceiverLayer { - SerialReceiver::SerialReceiver() + SerialReceiver::SerialReceiver(): _linkIsUp(false), _lastChannelsPacket(0) { #if defined(ARDUINO_ARCH_STM32) #if defined(HAVE_HWSERIAL1) @@ -72,7 +72,7 @@ namespace serialReceiverLayer #endif } - SerialReceiver::SerialReceiver(HardwareSerial *hwUartPort) + SerialReceiver::SerialReceiver(HardwareSerial *hwUartPort) : _linkIsUp(false), _lastChannelsPacket(0) { _uart = hwUartPort; @@ -101,7 +101,7 @@ namespace serialReceiverLayer #endif } - SerialReceiver::SerialReceiver(HardwareSerial *hwUartPort, int8_t rxPin, int8_t txPin) + SerialReceiver::SerialReceiver(HardwareSerial *hwUartPort, int8_t rxPin, int8_t txPin) : _linkIsUp(false), _lastChannelsPacket(0) { _uart = hwUartPort; @@ -130,6 +130,8 @@ namespace serialReceiverLayer _rxPin = serialReceiver._rxPin; _txPin = serialReceiver._txPin; + _linkIsUp = serialReceiver._linkIsUp; + _lastChannelsPacket= serialReceiver._lastChannelsPacket; #if CRSF_RC_ENABLED > 0 _rcChannels = new rcChannels_t; @@ -159,6 +161,8 @@ namespace serialReceiverLayer _txPin = serialReceiver._txPin; crsf = serialReceiver.crsf; + _linkIsUp = serialReceiver._linkIsUp; + _lastChannelsPacket = serialReceiver._lastChannelsPacket; #if CRSF_TELEMETRY_ENABLED > 0 telemetry = serialReceiver.telemetry; @@ -341,7 +345,8 @@ namespace serialReceiverLayer { while (_uart->available() > 0) { - if (crsf->receiveFrames((uint8_t)_uart->read())) + uint8_t byteReceived = (uint8_t)_uart->read(); + if (crsf->receiveFrames(byteReceived)) { flushRemainingFrames(); @@ -368,11 +373,40 @@ namespace serialReceiverLayer _rcChannelsCallback(_rcChannels); } #endif + setLinkUp(); } + + if (_rawDataCallback != nullptr) + { + _rawDataCallback(byteReceived); + } + } + checkLinkDown(); } #endif +void SerialReceiver::setLinkDownCallback(linkDownCallback_t callback) { _linkDownCallback = callback; } +void SerialReceiver::setLinkUpCallback(linkUpCallback_t callback) { _linkUpCallback = callback; } + +bool SerialReceiver::isLinkUp() const { return _linkIsUp; } + +void SerialReceiver::setLinkUp() +{ + if (!_linkIsUp && _linkUpCallback) _linkUpCallback(); + _linkIsUp = true; + _lastChannelsPacket = millis(); +} + +void SerialReceiver::checkLinkDown() +{ + if (_linkIsUp && millis() - _lastChannelsPacket > CRSF_FAILSAFE_STAGE1_MS) { + if (_linkDownCallback) _linkDownCallback(); + _linkIsUp = false; + } +} + + #if CRSF_LINK_STATISTICS_ENABLED > 0 void SerialReceiver::setLinkStatisticsCallback(linkStatisticsCallback_t callback) { @@ -391,6 +425,11 @@ namespace serialReceiverLayer } #endif + void SerialReceiver::setRawDataCallback(rawDataCallback_t callback) + { + _rawDataCallback = callback; + } + #if CRSF_RC_ENABLED > 0 void SerialReceiver::setRcChannelsCallback(rcChannelsCallback_t callback) { diff --git a/src/SerialReceiver/SerialReceiver.hpp b/src/SerialReceiver/SerialReceiver.hpp index f28f075b..86950b47 100644 --- a/src/SerialReceiver/SerialReceiver.hpp +++ b/src/SerialReceiver/SerialReceiver.hpp @@ -71,10 +71,16 @@ namespace serialReceiverLayer typedef void (*rcChannelsCallback_t)(rcChannels_t *); typedef void (*flightModeCallback_t)(flightModeId_t); typedef void (*linkStatisticsCallback_t)(link_statistics_t); + typedef void (*rawDataCallback_t)(int8_t); + + typedef void (*linkUpCallback_t)(); + typedef void (*linkDownCallback_t)(); class SerialReceiver { public: + static const unsigned int CRSF_FAILSAFE_STAGE1_MS = 300; + SerialReceiver(); explicit SerialReceiver(HardwareSerial *hwUartPort); SerialReceiver(HardwareSerial *hwUartPort, int8_t rxPin, int8_t txPin); @@ -89,6 +95,8 @@ namespace serialReceiverLayer void processFrames(); #endif + + #if CRSF_LINK_STATISTICS_ENABLED > 0 void setLinkStatisticsCallback(linkStatisticsCallback_t callback); #endif @@ -108,6 +116,13 @@ namespace serialReceiverLayer #endif #endif + void setRawDataCallback(rawDataCallback_t callback); + void setLinkDownCallback(linkDownCallback_t callback); + void setLinkUpCallback(linkUpCallback_t callback); + bool isLinkUp() const; + void checkLinkDown(); + void setLinkUp(); + #if CRSF_TELEMETRY_ENABLED > 0 void telemetryWriteAttitude(int16_t roll, int16_t pitch, int16_t yaw); void telemetryWriteBaroAltitude(uint16_t altitude, int16_t vario); @@ -123,6 +138,8 @@ namespace serialReceiverLayer int8_t _rxPin = -1; int8_t _txPin = -1; + bool _linkIsUp; + uint32_t _lastChannelsPacket; #if CRSF_TELEMETRY_ENABLED > 0 Telemetry *telemetry = nullptr; @@ -158,5 +175,9 @@ namespace serialReceiverLayer #if CRSF_RC_ENABLED > 0 || CRSF_TELEMETRY_ENABLED > 0 void flushRemainingFrames(); #endif + + rawDataCallback_t _rawDataCallback = nullptr; + linkUpCallback_t _linkUpCallback = nullptr; + linkDownCallback_t _linkDownCallback = nullptr; }; } // namespace serialReceiverLayer From d11656cea3ee115a57107665d3775b845eb4d310 Mon Sep 17 00:00:00 2001 From: Dmytro Date: Sat, 31 Aug 2024 21:49:07 +0300 Subject: [PATCH 2/2] fix operator= issue --- src/SerialReceiver/SerialReceiver.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/SerialReceiver/SerialReceiver.cpp b/src/SerialReceiver/SerialReceiver.cpp index d0bfbb05..2a279a30 100644 --- a/src/SerialReceiver/SerialReceiver.cpp +++ b/src/SerialReceiver/SerialReceiver.cpp @@ -161,6 +161,9 @@ namespace serialReceiverLayer crsf = serialReceiver.crsf; _linkIsUp = serialReceiver._linkIsUp; _lastChannelsPacket = serialReceiver._lastChannelsPacket; + _rawDataCallback = serialReceiver._rawDataCallback; + _linkUpCallback = serialReceiver._linkUpCallback; + _linkDownCallback = serialReceiver._linkDownCallback; #if CRSF_TELEMETRY_ENABLED > 0 telemetry = serialReceiver.telemetry;