Line data    Source code

       1             : #include "AP_Frsky_Backend.h"
       2             : 
       3             : #if AP_FRSKY_TELEM_ENABLED
       4             : 
       5             : #include <AP_Baro/AP_Baro.h>
       6             : #include <AP_AHRS/AP_AHRS.h>
       7             : #include <AP_RPM/AP_RPM.h>
       8             : #include <AP_RCTelemetry/AP_RCTelemetry.h>
       9             : 
      10             : extern const AP_HAL::HAL& hal;
      11             : 
      12           5 : bool AP_Frsky_Backend::init()
      13             : {
      14             :     // if SPort Passthrough is using external data then it will
      15             :     // override this to do nothing:
      16           5 :     return init_serial_port();
      17             : }
      18             : 
      19           5 : bool AP_Frsky_Backend::init_serial_port()
      20             : {
      21           5 :     if (!hal.scheduler->thread_create(
      22             :             FUNCTOR_BIND_MEMBER(&AP_Frsky_Backend::loop, void),
      23             :             "FrSky",
      24             :             1024,
      25             :             AP_HAL::Scheduler::PRIORITY_RCIN,
      26             :             1)) {
      27           0 :         return false;
      28             :     }
      29             :     // we don't want flow control for either protocol
      30           5 :     _port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
      31           5 :     return true;
      32             : }
      33             : 
      34             : /*
      35             :   thread to loop handling bytes
      36             :  */
      37           5 : void AP_Frsky_Backend::loop(void)
      38             : {
      39             :     // initialise uart (this must be called from within tick b/c the UART begin must be called from the same thread as it is used from)
      40           5 :     _port->begin(initial_baud(), 0, 0);
      41             : 
      42             :     while (true) {
      43       42812 :         hal.scheduler->delay(1);
      44       42807 :         send();
      45             :     }
      46             : }
      47             : 
      48             : /*
      49             :  * prepare altitude between vehicle and home location data
      50             :  * for FrSky D and SPort protocols
      51             :  */
      52         124 : void AP_Frsky_Backend::calc_nav_alt(void)
      53             : {
      54         124 :     _SPort_data.vario_vspd = (int32_t)(AP_RCTelemetry::get_vspeed_ms()*100); //convert to cm/s
      55             : 
      56         124 :     float current_height = AP_RCTelemetry::get_nav_alt_m();
      57         124 :     _SPort_data.alt_nav_meters = float_to_uint16(current_height);
      58         124 :     _SPort_data.alt_nav_cm = float_to_uint16((current_height - _SPort_data.alt_nav_meters) * 100);
      59         124 : }
      60             : 
      61             : /*
      62             :  * format the decimal latitude/longitude to the required degrees/minutes
      63             :  * for FrSky D and SPort protocols
      64             :  */
      65          34 : float AP_Frsky_Backend::format_gps(float dec)
      66             : {
      67          34 :     uint8_t dm_deg = (uint8_t) dec;
      68          34 :     return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
      69             : }
      70             : 
      71             : /*
      72             :  * prepare gps data
      73             :  * for FrSky D and SPort protocols
      74             :  */
      75          26 : void AP_Frsky_Backend::calc_gps_position(void)
      76             : {
      77          26 :     AP_AHRS &_ahrs = AP::ahrs();
      78             : 
      79          26 :     Location loc;
      80             : 
      81          26 :     if (_ahrs.get_location(loc)) {
      82          17 :         float lat = format_gps(fabsf(loc.lat/10000000.0f));
      83          17 :         _SPort_data.latdddmm = lat;
      84          17 :         _SPort_data.latmmmm = (lat - _SPort_data.latdddmm) * 10000;
      85          17 :         _SPort_data.lat_ns = (loc.lat < 0) ? 'S' : 'N';
      86             : 
      87          17 :         float lon = format_gps(fabsf(loc.lng/10000000.0f));
      88          17 :         _SPort_data.londddmm = lon;
      89          17 :         _SPort_data.lonmmmm = (lon - _SPort_data.londddmm) * 10000;
      90          17 :         _SPort_data.lon_ew = (loc.lng < 0) ? 'W' : 'E';
      91             : 
      92          17 :         float alt = loc.alt * 0.01f;
      93          17 :         _SPort_data.alt_gps_meters = float_to_uint16(alt);
      94          17 :         _SPort_data.alt_gps_cm = float_to_uint16((alt - _SPort_data.alt_gps_meters) * 100);
      95             : 
      96          17 :         const float speed = AP::ahrs().groundspeed();
      97          17 :         _SPort_data.speed_in_meter = float_to_int16(speed);
      98          17 :         _SPort_data.speed_in_centimeter = float_to_uint16((speed - _SPort_data.speed_in_meter) * 100);
      99             :     } else {
     100           9 :         _SPort_data.latdddmm = 0;
     101           9 :         _SPort_data.latmmmm = 0;
     102           9 :         _SPort_data.lat_ns = 0;
     103           9 :         _SPort_data.londddmm = 0;
     104           9 :         _SPort_data.lonmmmm = 0;
     105           9 :         _SPort_data.alt_gps_meters = 0;
     106           9 :         _SPort_data.alt_gps_cm = 0;
     107           9 :         _SPort_data.speed_in_meter = 0;
     108           9 :         _SPort_data.speed_in_centimeter = 0;
     109             :     }
     110             : 
     111          26 :     _SPort_data.yaw = uint16_t(_ahrs.get_yaw_deg()); // heading in degree based on AHRS and not GPS
     112          26 : }
     113             : 
     114             : /*
     115             :  * prepare rpm data
     116             :  * for FrSky D and SPort protocols
     117             :  */
     118          10 : bool AP_Frsky_Backend::calc_rpm(const uint8_t instance, int32_t &value) const
     119             : {
     120             : #if AP_RPM_ENABLED
     121          10 :     const AP_RPM* rpm = AP::rpm();
     122          10 :     if (rpm == nullptr) {
     123           0 :         return false;
     124             :     }
     125             : 
     126             :     float rpm_value;
     127          10 :     if (!rpm->get_rpm(instance, rpm_value)) {
     128           0 :         return false;
     129             :     }
     130          10 :     value = static_cast<int32_t>(roundf(rpm_value));
     131          10 :     return true;
     132             : #else
     133             :     return false;
     134             : #endif
     135             : }
     136             : 
     137             : #endif  // AP_FRSKY_TELEM_ENABLED