diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.cpp b/libraries/AP_RCProtocol/AP_RCProtocol.cpp index df9e43f30d..210cb3b8fe 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol.cpp @@ -257,6 +257,14 @@ uint16_t AP_RCProtocol::read(uint8_t chan) return 0; } +int16_t AP_RCProtocol::get_RSSI(void) const +{ + if (_detected_protocol != AP_RCProtocol::NONE) { + return backend[_detected_protocol]->get_RSSI(); + } + return -1; +} + /* ask for bind start on supported receivers (eg spektrum satellite) */ diff --git a/libraries/AP_RCProtocol/AP_RCProtocol.h b/libraries/AP_RCProtocol/AP_RCProtocol.h index 523ec37b9d..c42ea90ffc 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol.h @@ -63,6 +63,7 @@ public: uint16_t read(uint8_t chan); bool new_input(); void start_bind(void); + int16_t get_RSSI(void) const; // return protocol name as a string static const char *protocol_name_from_protocol(rcprotocol_t protocol); diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp index ee920ac503..f4b6fb393f 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.cpp @@ -49,7 +49,7 @@ uint16_t AP_RCProtocol_Backend::read(uint8_t chan) /* provide input from a backend */ -void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe) +void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool in_failsafe, int16_t _rssi) { num_values = MIN(num_values, MAX_RCIN_CHANNELS); memcpy(_pwm_values, values, num_values*sizeof(uint16_t)); @@ -63,4 +63,5 @@ void AP_RCProtocol_Backend::add_input(uint8_t num_values, uint16_t *values, bool if (!in_failsafe) { rc_input_count++; } + rssi = _rssi; } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h index 499d9b85aa..684325f64d 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_Backend.h @@ -50,9 +50,14 @@ public: uint32_t get_rc_input_count(void) const { return rc_input_count; } + + // get RSSI + int16_t get_RSSI(void) const { + return rssi; + } protected: - void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe); + void add_input(uint8_t num_channels, uint16_t *values, bool in_failsafe, int16_t rssi=-1); private: AP_RCProtocol &frontend; @@ -62,4 +67,5 @@ private: uint16_t _pwm_values[MAX_RCIN_CHANNELS]; uint8_t _num_channels; + int16_t rssi = -1; }; diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp index 8b739560a2..73f3962f96 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_FPort.cpp @@ -14,6 +14,7 @@ */ #include "AP_RCProtocol_FPort.h" +#include #define FRAME_HEAD 0x7E #define FRAME_LEN_CONTROL 0x19 @@ -104,7 +105,7 @@ void AP_RCProtocol_FPort::decode_control(const FPort_Frame &frame) } bool failsafe = ((frame.control.flags & (1 << FLAGS_FAILSAFE_BIT)) != 0); - add_input(MAX_CHANNELS, values, failsafe); + add_input(MAX_CHANNELS, values, failsafe, frame.control.rssi); } // decode a full FPort downlink frame