libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp

   1 /*
   2
   3    Inspired by work done here
   4    https://github.com/PX4/Firmware/tree/master/src/drivers/frsky_telemetry from Stefan Rado <px4@sradonia.net>
   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 AP_Frsky_Telem::AP_Frsky_Telem()
  45 {
  46     singleton = this;
  47 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
  48     _frsky_parameters = &AP::vehicle()->frsky_parameters;
  49 #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
  50 }
  51
  52
  53 AP_Frsky_Telem::~AP_Frsky_Telem(void)
  54 {
  55     singleton = nullptr;
  56 }
  57
  58 /*
  59  * init - perform required initialisation
  60  */
  61 bool AP_Frsky_Telem::init(bool use_external_data)
  62 {
  63     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     if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_D, 0))) {
  68 #if AP_FRSKY_D_TELEM_ENABLED
  69         _backend = NEW_NOTHROW AP_Frsky_D(port);
  70 #endif
  71     } else if ((port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_FrSky_SPort, 0))) {
  72 #if AP_FRSKY_SPORT_TELEM_ENABLED
  73         _backend = NEW_NOTHROW AP_Frsky_SPort(port);
  74 #endif
  75     } 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         _backend = NEW_NOTHROW AP_Frsky_SPort_Passthrough(port, use_external_data, _frsky_parameters);
  78 #endif
  79     }
  80
  81     if (_backend == nullptr) {
  82         return false;
  83     }
  84
  85     if (!_backend->init()) {
  86         delete _backend;
  87         _backend = nullptr;
  88         return false;
  89     }
  90
  91     return true;
  92 }
  93
  94 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     if (_backend == nullptr) {
  97         return false;
  98     }
  99     if (packet_array == nullptr) {
 100         return false;
 101     }
 102     return _backend->get_telem_data(packet_array, packet_count, max_size);
 103 }
 104
 105 #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
 106 bool AP_Frsky_Telem::_set_telem_data(uint8_t frame, uint16_t appid, uint32_t data)
 107 {
 108     if (_backend == nullptr) {
 109         return false;
 110     }
 111     return _backend->set_telem_data(frame, appid, data);
 112 }
 113 #endif
 114
 115 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     if (!singleton && !hal.util->get_soft_armed()) {
 119         NEW_NOTHROW AP_Frsky_Telem();
 120         // initialize the passthrough scheduler
 121         if (singleton) {
 122             singleton->init(true);
 123         }
 124     }
 125 }
 126
 127 /*
 128   fetch Sport data for an external transport, such as FPort
 129  */
 130 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     try_create_singleton_for_external_data();
 133     if (singleton == nullptr) {
 134         return false;
 135     }
 136     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 bool AP_Frsky_Telem::set_telem_data(const uint8_t frame, const uint16_t appid, const uint32_t data)
 144 {
 145     try_create_singleton_for_external_data();
 146     if (singleton == nullptr) {
 147         return false;
 148     }
 149     return singleton->_set_telem_data(frame, appid, data);
 150 }
 151 #endif
 152
 153 namespace AP
 154 {
 155 AP_Frsky_Telem *frsky_telem()
 156 {
 157     return AP_Frsky_Telem::get_singleton();
 158 }
 159 };
 160
 161 #endif  // AP_FRSKY_TELEM_ENABLED