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_MAVlite_SPortToMAVlite.cpp
Views: 1798
1
#include "AP_Frsky_MAVlite_SPortToMAVlite.h"

2


3
#include "AP_Frsky_MAVlite.h"

4


5
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL

6
void AP_Frsky_MAVlite_SPortToMAVlite::reset(void)

7
{

8
    checksum = 0;

9


10
    expected_seq = 0;

11
    payload_next_byte = 0;

12
    parse_state = State::WANT_LEN;

13
}

14


15
void AP_Frsky_MAVlite_SPortToMAVlite::update_checksum(const uint8_t c)

16
{

17
    checksum += c; //0-1FF

18
    checksum += checksum >> 8;

19
    checksum &= 0xFF;

20
}

21


22
/*

23
 Parses sport packets and if successful fills the rxmsg mavlite struct

24
 */

25
bool AP_Frsky_MAVlite_SPortToMAVlite::process(AP_Frsky_MAVlite_Message &rxmsg, const AP_Frsky_SPort::sport_packet_t &packet)

26
{

27
    // the two skipped bytes in packet.raw here are sensor and frame.

28
    // appid and data are used to transport the mavlite message.

29


30
    // deal with packet sequence number:

31
    const uint8_t received_seq = (packet.raw[2] & 0x3F);

32
    // if the first byte of any sport passthrough packet is zero then we reset:

33
    if (received_seq == 0) {

34
        reset();

35
    }

36
    if (received_seq != expected_seq) {

37
        parse_state = State::ERROR;

38
        return false;

39
    }

40
    update_checksum(received_seq);

41
    expected_seq = received_seq + 1;

42


43
    // deal with the remainder (post-sequence) of the packet:

44
    for (uint8_t i=3; i<ARRAY_SIZE(packet.raw); i++) {

45
        parse(packet.raw[i]);

46
    }

47
    if (parse_state == State::MESSAGE_RECEIVED) {

48
        rxmsg = _rxmsg;

49
        return true;

50
    }

51
    return false;

52
}

53


54
void AP_Frsky_MAVlite_SPortToMAVlite::parse(uint8_t byte)

55
{

56
    switch (parse_state) {

57


58
    case State::IDLE:

59
        // it is an error to receive anything but offset==0 byte=0xx0

60
        // in this state

61
        parse_state = State::ERROR;

62
        return;

63


64
    case State::ERROR:

65
        // waiting for offset==0 && byte==0x00 to bump us into WANT_LEN

66
        return;

67


68
    case State::WANT_LEN:

69
        _rxmsg.len = byte;

70
        update_checksum(byte);

71
        parse_state = State::WANT_MSGID;

72
        return;

73


74
    case State::WANT_MSGID:

75
        _rxmsg.msgid = byte;

76
        update_checksum(byte);

77
        if (_rxmsg.len == 0) {

78
            parse_state = State::WANT_CHECKSUM;

79
        } else {

80
            parse_state = State::WANT_PAYLOAD;

81
        }

82
        return;

83


84
    case State::WANT_PAYLOAD:

85
        // add byte to payload

86
        _rxmsg.payload[payload_next_byte++] = byte;

87
        update_checksum(byte);

88


89
        if (payload_next_byte >= _rxmsg.len) {

90
            parse_state = State::WANT_CHECKSUM;

91
        }

92
        return;

93


94
    case State::WANT_CHECKSUM:

95
        if (checksum != byte) {

96
            parse_state = State::ERROR;

97
            return;

98
        }

99
        parse_state = State::MESSAGE_RECEIVED;

100
        return;

101


102
    case State::MESSAGE_RECEIVED:

103
        return;

104
    }

105
}

106
#endif

107