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

2


3
#include <AP_Math/AP_Math.h>

4


5
#if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL

6
bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const

7
{

8
    if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {

9
        return false;

10
    }

11
    memcpy(bytes, &payload[offset], count);

12
    return true;

13
}

14


15
bool AP_Frsky_MAVlite_Message::set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count)

16
{

17
    if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {

18
        return false;

19
    }

20
    memcpy(&payload[offset], bytes, count);

21
    len += count;

22
    return true;

23
}

24


25
bool AP_Frsky_MAVlite_Message::get_string(char* value, const uint8_t offset) const

26
{

27
    if (get_bytes((uint8_t*)value, offset, MIN((uint8_t)16, len - offset))) {

28
        value[MIN((uint8_t)16, len - offset)] = 0x00; // terminator

29
        return true;

30
    }

31
    return false;

32
}

33


34
bool AP_Frsky_MAVlite_Message::set_string(const char* value, const uint8_t offset)

35
{

36
    return set_bytes((uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));

37
}

38


39


40
uint8_t AP_Frsky_MAVlite_Message::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset)

41
{

42
    uint8_t mask = 0;

43
    for (uint8_t i=bit_offset; i<=bit_count; i++) {

44
        mask |= 1 << i;

45
    }

46
    return (value & mask) >> bit_offset;

47
}

48


49
void AP_Frsky_MAVlite_Message::bit8_pack(uint8_t &value, const uint8_t bit_value, const uint8_t bit_count, const uint8_t bit_offset)

50
{

51
    uint8_t mask = 0;

52
    for (uint8_t i=bit_offset; i<=bit_count; i++) {

53
        mask |= 1 << i;

54
    }

55
    value |= (bit_value<<bit_offset) & mask;

56
}

57
#endif

58