Line data    Source code

       1             : /*
       2             : 
       3             :    Inspired by work done here
       4             :    https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <[email protected]>
       5             :    https://github.com/opentx/opentx/tree/2.3/radio/src/telemetry from the OpenTX team
       6             : 
       7             :    This program is free software: you can redistribute it and/or modify
       8             :    it under the terms of the GNU General Public License as published by
       9             :    the Free Software Foundation, either version 3 of the License, or
      10             :    (at your option) any later version.
      11             : 
      12             :    This program is distributed in the hope that it will be useful,
      13             :    but WITHOUT ANY WARRANTY; without even the implied warranty of
      14             :    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
      15             :    GNU General Public License for more details.
      16             : 
      17             :    You should have received a copy of the GNU General Public License
      18             :    along with this program.  If not, see <http://www.gnu.org/licenses/>.
      19             : */
      20             : 
      21             : /* 
      22             :    FRSKY Telemetry library
      23             : */
      24             : 
      25             : #include "AP_Frsky_config.h"
      26             : 
      27             : #if AP_FRSKY_TELEM_ENABLED
      28             : 
      29             : #include "AP_Frsky_Telem.h"
      30             : #include "AP_Frsky_Parameters.h"
      31             : 
      32             : #include <AP_SerialManager/AP_SerialManager.h>
      33             : 
      34             : #include <AP_Vehicle/AP_Vehicle.h>
      35             : 
      36             : #include "AP_Frsky_D.h"
      37             : #include "AP_Frsky_SPort.h"
      38             : #include "AP_Frsky_SPort_Passthrough.h"
      39             : 
      40             : extern const AP_HAL::HAL& hal;
      41             : 
      42             : AP_Frsky_Telem *AP_Frsky_Telem::singleton;
      43             : 
      44        2094 : AP_Frsky_Telem::AP_Frsky_Telem()
      45             : {
      46        2094 :     singleton = this;
      47             : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
      48        2094 :     _frsky_parameters = &AP::vehicle()->frsky_parameters;
      49             : #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
      50        2094 : }
      51             : 
      52             : 
      53        2089 : AP_Frsky_Telem::~AP_Frsky_Telem(void)
      54             : {
      55        2089 :     singleton = nullptr;
      56        2089 : }
      57             : 
      58             : /*
      59             :  * init - perform required initialisation
      60             :  */
      61        2094 : bool AP_Frsky_Telem::init(bool use_external_data)
      62             : {
      63        2094 :     const AP_SerialManager &serial_manager = AP::serialmanager();
      64             : 
      65             :     // check for protocol configured for a serial port - only the first serial port with one of these protocols will then run (cannot have FrSky on multiple serial ports)
      66             :     AP_HAL::UARTDriver *port;
      67        2094 :     if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) {
      68             : #if AP_FRSKY_D_TELEM_ENABLED
      69           1 :         _backend = NEW_NOTHROW AP_Frsky_D(port);
      70             : #endif
      71        2093 :     } else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
      72             : #if AP_FRSKY_SPORT_TELEM_ENABLED
      73           1 :         _backend = NEW_NOTHROW AP_Frsky_SPort(port);
      74             : #endif
      75        2092 :     } else if (use_external_data || (port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough, 0))) {
      76             : #if AP_FRSKY_SPORT_PASSTHROUGH_ENABLED
      77           3 :         _backend = NEW_NOTHROW AP_Frsky_SPort_Passthrough(port, use_external_data, _frsky_parameters);
      78             : #endif
      79             :     }
      80             : 
      81        2094 :     if (_backend == nullptr) {
      82        2089 :         return false;
      83             :     }
      84             : 
      85           5 :     if (!_backend->init()) {
      86           0 :         delete _backend;
      87           0 :         _backend = nullptr;
      88           0 :         return false;
      89             :     }
      90             : 
      91           5 :     return true;
      92             : }
      93             : 
      94           0 : bool AP_Frsky_Telem::_get_telem_data(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
      95             : {
      96           0 :     if (_backend == nullptr) {
      97           0 :         return false;
      98             :     }
      99           0 :     if (packet_array == nullptr) {
     100           0 :         return false;
     101             :     }
     102           0 :     return _backend->get_telem_data(packet_array, packet_count, max_size);
     103             : }
     104             : 
     105             : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     106           0 : bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data)
     107             : {
     108           0 :     if (_backend == nullptr) {
     109           0 :         return false;
     110             :     }
     111           0 :     return _backend->set_telem_data(frame, appid, data);
     112             : }
     113             : #endif
     114             : 
     115           0 : void AP_Frsky_Telem::try_create_singleton_for_external_data()
     116             : {
     117             :     // try to allocate an AP_Frsky_Telem object only if we are disarmed
     118           0 :     if (!singleton && !hal.util->get_soft_armed()) {
     119           0 :         NEW_NOTHROW AP_Frsky_Telem();
     120             :         // initialize the passthrough scheduler
     121           0 :         if (singleton) {
     122           0 :             singleton->init(true);
     123             :         }
     124             :     }
     125           0 : }
     126             : 
     127             : /*
     128             :   fetch Sport data for an external transport, such as FPort
     129             :  */
     130           0 : bool AP_Frsky_Telem::get_telem_data(AP_Frsky_Backend::sport_packet_t* packet_array, uint8_t &packet_count, const uint8_t max_size)
     131             : {
     132           0 :     try_create_singleton_for_external_data();
     133           0 :     if (singleton == nullptr) {
     134           0 :         return false;
     135             :     }
     136           0 :     return singleton->_get_telem_data(packet_array, packet_count, max_size);
     137             : }
     138             : 
     139             : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
     140             : /*
     141             :   allow external transports (e.g. FPort), to supply telemetry data
     142             :  */
     143           0 : bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
     144             : {
     145           0 :     try_create_singleton_for_external_data();
     146           0 :     if (singleton == nullptr) {
     147           0 :         return false;
     148             :     }
     149           0 :     return singleton->_set_telem_data(frame, appid, data);
     150             : }
     151             : #endif
     152             : 
     153             : namespace AP
     154             : {
     155      262928 : AP_Frsky_Telem *frsky_telem()
     156             : {
     157      262928 :     return AP_Frsky_Telem::get_singleton();
     158             : }
     159             : };
     160             : 
     161             : #endif  // AP_FRSKY_TELEM_ENABLED