Line data    Source code

       1             : #include "AP_Frsky_D.h"
       2             : 
       3             : #if AP_FRSKY_D_TELEM_ENABLED
       4             : 
       5             : #include <AP_AHRS/AP_AHRS.h>
       6             : #include <AP_BattMonitor/AP_BattMonitor.h>
       7             : #include <AP_GPS/AP_GPS.h>
       8             : #include <GCS_MAVLink/GCS.h>
       9             : 
      10             : /*
      11             :   send 1 byte and do byte stuffing
      12             : */
      13        3021 : void AP_Frsky_D::send_byte(uint8_t byte)
      14             : {
      15        3021 :     if (byte == START_STOP_D) {
      16           0 :         _port->write(0x5D);
      17           0 :         _port->write(0x3E);
      18        3021 :     } else if (byte == BYTESTUFF_D) {
      19           2 :         _port->write(0x5D);
      20           2 :         _port->write(0x3D);
      21             :     } else {
      22        3019 :         _port->write(byte);
      23             :     }
      24        3021 : }
      25             : 
      26             : /*
      27             :  * send one uint16 frame of FrSky data - for FrSky D protocol (D-receivers)
      28             :  */
      29        1007 : void AP_Frsky_D::send_uint16(uint16_t id, uint16_t data)
      30             : {
      31        1007 :     _port->write(START_STOP_D);    // send a 0x5E start byte
      32        1007 :     uint8_t *bytes = (uint8_t*)&id;
      33        1007 :     send_byte(bytes[0]);
      34        1007 :     bytes = (uint8_t*)&data;
      35        1007 :     send_byte(bytes[0]); // LSB
      36        1007 :     send_byte(bytes[1]); // MSB
      37        1007 : }
      38             : 
      39             : /*
      40             :  * send frame1 and frame2 telemetry data
      41             :  * one frame (frame1) is sent every 200ms with baro alt, nb sats, batt volts and amp, control_mode
      42             :  * a second frame (frame2) is sent every second (1000ms) with gps position data, and ahrs.get_yaw_deg() heading (instead of GPS heading)
      43             :  * for FrSky D protocol (D-receivers)
      44             :  */
      45        5089 : void AP_Frsky_D::send(void)
      46             : {
      47        5089 :     const AP_BattMonitor &_battery = AP::battery();
      48        5089 :     uint32_t now = AP_HAL::millis();
      49             :     // send frame1 every 200ms
      50        5089 :     if (now - _D.last_200ms_frame >= 200) {
      51         119 :         _D.last_200ms_frame = now;
      52         119 :         send_uint16(DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + (uint8_t)AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)
      53         119 :         send_uint16(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode
      54         119 :         uint8_t percentage = 0;
      55         119 :         IGNORE_RETURN(_battery.capacity_remaining_pct(percentage));
      56         119 :         send_uint16(DATA_ID_FUEL, (uint16_t)roundf(percentage)); // send battery remaining
      57         119 :         send_uint16(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage
      58             :         float current;
      59         119 :         if (!_battery.current_amps(current)) {
      60           0 :             current = 0;
      61             :         }
      62         119 :         send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption
      63         119 :         calc_nav_alt();
      64         119 :         send_uint16(DATA_ID_BARO_ALT_BP, _SPort_data.alt_nav_meters); // send nav altitude integer part
      65         119 :         send_uint16(DATA_ID_BARO_ALT_AP, _SPort_data.alt_nav_cm); // send nav altitude decimal part
      66             :     }
      67             :     // send frame2 every second
      68        5089 :     if (now - _D.last_1000ms_frame >= 1000) {
      69          24 :         _D.last_1000ms_frame = now;
      70          24 :         AP_AHRS &_ahrs = AP::ahrs();
      71          24 :         send_uint16(DATA_ID_GPS_COURS_BP, (uint16_t)_ahrs.get_yaw_deg()); // send heading in degree based on AHRS and not GPS
      72          24 :         calc_gps_position();
      73          24 :         if (AP::gps().status() >= AP_GPS_FixType::FIX_3D) {
      74          15 :             send_uint16(DATA_ID_GPS_LAT_BP, _SPort_data.latdddmm); // send gps latitude degree and minute integer part
      75          15 :             send_uint16(DATA_ID_GPS_LAT_AP, _SPort_data.latmmmm); // send gps latitude minutes decimal part
      76          15 :             send_uint16(DATA_ID_GPS_LAT_NS, _SPort_data.lat_ns); // send gps North / South information
      77          15 :             send_uint16(DATA_ID_GPS_LONG_BP, _SPort_data.londddmm); // send gps longitude degree and minute integer part
      78          15 :             send_uint16(DATA_ID_GPS_LONG_AP, _SPort_data.lonmmmm); // send gps longitude minutes decimal part
      79          15 :             send_uint16(DATA_ID_GPS_LONG_EW, _SPort_data.lon_ew); // send gps East / West information
      80          15 :             send_uint16(DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer part
      81          15 :             send_uint16(DATA_ID_GPS_SPEED_AP, _SPort_data.speed_in_centimeter); // send gps speed decimal part
      82          15 :             send_uint16(DATA_ID_GPS_ALT_BP, _SPort_data.alt_gps_meters); // send gps altitude integer part
      83          15 :             send_uint16(DATA_ID_GPS_ALT_AP, _SPort_data.alt_gps_cm); // send gps altitude decimal part
      84             :         }
      85             :     }
      86        5089 : }
      87             : 
      88             : #endif  // AP_FRSKY_D_TELEM_ENABLED