diff --git a/src/CRSFforArduino.cpp b/src/CRSFforArduino.cpp index 66e40b1f..596425e5 100644 --- a/src/CRSFforArduino.cpp +++ b/src/CRSFforArduino.cpp @@ -190,6 +190,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 75a33ea5..5626ae51 100644 --- a/src/CRSFforArduino.hpp +++ b/src/CRSFforArduino.hpp @@ -54,6 +54,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 501334d2..2a279a30 100644 --- a/src/SerialReceiver/SerialReceiver.cpp +++ b/src/SerialReceiver/SerialReceiver.cpp @@ -31,7 +31,7 @@ using namespace hal; namespace serialReceiverLayer { - SerialReceiver::SerialReceiver() + SerialReceiver::SerialReceiver(): _linkIsUp(false), _lastChannelsPacket(0) { #if defined(ARDUINO_ARCH_STM32) #if defined(HAVE_HWSERIAL1) @@ -70,7 +70,7 @@ namespace serialReceiverLayer #endif } - SerialReceiver::SerialReceiver(HardwareSerial *hwUartPort) + SerialReceiver::SerialReceiver(HardwareSerial *hwUartPort) : _linkIsUp(false), _lastChannelsPacket(0) { _uart = hwUartPort; @@ -99,7 +99,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; @@ -128,6 +128,8 @@ namespace serialReceiverLayer _rxPin = serialReceiver._rxPin; _txPin = serialReceiver._txPin; + _linkIsUp = serialReceiver._linkIsUp; + _lastChannelsPacket= serialReceiver._lastChannelsPacket; #if CRSF_RC_ENABLED > 0 _rcChannels = new rcChannels_t; @@ -157,6 +159,11 @@ namespace serialReceiverLayer _txPin = serialReceiver._txPin; 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; @@ -522,7 +529,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(); @@ -549,11 +557,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) { @@ -572,6 +609,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 082e335c..03a93e1d 100644 --- a/src/SerialReceiver/SerialReceiver.hpp +++ b/src/SerialReceiver/SerialReceiver.hpp @@ -69,10 +69,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); @@ -87,6 +93,8 @@ namespace serialReceiverLayer void processFrames(); #endif + + #if CRSF_LINK_STATISTICS_ENABLED > 0 void setLinkStatisticsCallback(linkStatisticsCallback_t callback); #endif @@ -106,6 +114,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); @@ -121,6 +136,8 @@ namespace serialReceiverLayer int8_t _rxPin = -1; int8_t _txPin = -1; + bool _linkIsUp; + uint32_t _lastChannelsPacket; #if CRSF_TELEMETRY_ENABLED > 0 Telemetry *telemetry = nullptr; @@ -156,5 +173,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