CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_Frsky_Telem/AP_Frsky_D.cpp
Views: 1798
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
void AP_Frsky_D::send_byte(uint8_t byte)

14
{

15
    if (byte == START_STOP_D) {

16
        _port->write(0x5D);

17
        _port->write(0x3E);

18
    } else if (byte == BYTESTUFF_D) {

19
        _port->write(0x5D);

20
        _port->write(0x3D);

21
    } else {

22
        _port->write(byte);

23
    }

24
}

25


26
/*

27
 * send one uint16 frame of FrSky data - for FrSky D protocol (D-receivers)

28
 */

29
void AP_Frsky_D::send_uint16(uint16_t id, uint16_t data)

30
{

31
    _port->write(START_STOP_D);    // send a 0x5E start byte

32
    uint8_t *bytes = (uint8_t*)&id;

33
    send_byte(bytes[0]);

34
    bytes = (uint8_t*)&data;

35
    send_byte(bytes[0]); // LSB

36
    send_byte(bytes[1]); // MSB

37
}

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.yaw_sensor heading (instead of GPS heading)

43
 * for FrSky D protocol (D-receivers)

44
 */

45
void AP_Frsky_D::send(void)

46
{

47
    const AP_BattMonitor &_battery = AP::battery();

48
    uint32_t now = AP_HAL::millis();

49
    // send frame1 every 200ms

50
    if (now - _D.last_200ms_frame >= 200) {

51
        _D.last_200ms_frame = now;

52
        send_uint16(DATA_ID_TEMP2, (uint16_t)(AP::gps().num_sats() * 10 + AP::gps().status())); // send GPS status and number of satellites as num_sats*10 + status (to fit into a uint8_t)

53
        send_uint16(DATA_ID_TEMP1, gcs().custom_mode()); // send flight mode

54
        uint8_t percentage = 0;

55
        IGNORE_RETURN(_battery.capacity_remaining_pct(percentage));

56
        send_uint16(DATA_ID_FUEL, (uint16_t)roundf(percentage)); // send battery remaining

57
        send_uint16(DATA_ID_VFAS, (uint16_t)roundf(_battery.voltage() * 10.0f)); // send battery voltage

58
        float current;

59
        if (!_battery.current_amps(current)) {

60
            current = 0;

61
        }

62
        send_uint16(DATA_ID_CURRENT, (uint16_t)roundf(current * 10.0f)); // send current consumption

63
        calc_nav_alt();

64
        send_uint16(DATA_ID_BARO_ALT_BP, _SPort_data.alt_nav_meters); // send nav altitude integer part

65
        send_uint16(DATA_ID_BARO_ALT_AP, _SPort_data.alt_nav_cm); // send nav altitude decimal part

66
    }

67
    // send frame2 every second

68
    if (now - _D.last_1000ms_frame >= 1000) {

69
        _D.last_1000ms_frame = now;

70
        AP_AHRS &_ahrs = AP::ahrs();

71
        send_uint16(DATA_ID_GPS_COURS_BP, (uint16_t)((_ahrs.yaw_sensor / 100) % 360)); // send heading in degree based on AHRS and not GPS

72
        calc_gps_position();

73
        if (AP::gps().status() >= 3) {

74
            send_uint16(DATA_ID_GPS_LAT_BP, _SPort_data.latdddmm); // send gps latitude degree and minute integer part

75
            send_uint16(DATA_ID_GPS_LAT_AP, _SPort_data.latmmmm); // send gps latitude minutes decimal part

76
            send_uint16(DATA_ID_GPS_LAT_NS, _SPort_data.lat_ns); // send gps North / South information

77
            send_uint16(DATA_ID_GPS_LONG_BP, _SPort_data.londddmm); // send gps longitude degree and minute integer part

78
            send_uint16(DATA_ID_GPS_LONG_AP, _SPort_data.lonmmmm); // send gps longitude minutes decimal part

79
            send_uint16(DATA_ID_GPS_LONG_EW, _SPort_data.lon_ew); // send gps East / West information

80
            send_uint16(DATA_ID_GPS_SPEED_BP, _SPort_data.speed_in_meter); // send gps speed integer part

81
            send_uint16(DATA_ID_GPS_SPEED_AP, _SPort_data.speed_in_centimeter); // send gps speed decimal part

82
            send_uint16(DATA_ID_GPS_ALT_BP, _SPort_data.alt_gps_meters); // send gps altitude integer part

83
            send_uint16(DATA_ID_GPS_ALT_AP, _SPort_data.alt_gps_cm); // send gps altitude decimal part

84
        }

85
    }

86
}

87


88
#endif  // AP_FRSKY_D_TELEM_ENABLED

89


90