Line data    Source code

       1             : #include "AP_Frsky_MAVlite_Message.h"
       2             : 
       3             : #include <AP_Math/AP_Math.h>
       4             : 
       5             : #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL
       6          17 : bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const
       7             : {
       8          17 :     if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
       9           0 :         return false;
      10             :     }
      11          17 :     memcpy(bytes, &payload[offset], count);
      12          17 :     return true;
      13             : }
      14             : 
      15          12 : bool AP_Frsky_MAVlite_Message::set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count)
      16             : {
      17          12 :     if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
      18           0 :         return false;
      19             :     }
      20          12 :     memcpy(&payload[offset], bytes, count);
      21          12 :     len += count;
      22          12 :     return true;
      23             : }
      24             : 
      25           2 : bool AP_Frsky_MAVlite_Message::get_string(char* value, const uint8_t offset) const
      26             : {
      27           2 :     if (get_bytes((uint8_t*)value, offset, MIN((uint8_t)16, len - offset))) {
      28           2 :         value[MIN((uint8_t)16, len - offset)] = 0x00; // terminator
      29           2 :         return true;
      30             :     }
      31           0 :     return false;
      32             : }
      33             : 
      34           2 : bool AP_Frsky_MAVlite_Message::set_string(const char* value, const uint8_t offset)
      35             : {
      36           2 :     return set_bytes((uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));
      37             : }
      38             : 
      39             : 
      40           4 : uint8_t AP_Frsky_MAVlite_Message::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset)
      41             : {
      42           4 :     uint8_t mask = 0;
      43          20 :     for (uint8_t i=bit_offset; i<=bit_count; i++) {
      44          16 :         mask |= 1 << i;
      45             :     }
      46           4 :     return (value & mask) >> bit_offset;
      47             : }
      48             : 
      49           0 : 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           0 :     uint8_t mask = 0;
      52           0 :     for (uint8_t i=bit_offset; i<=bit_count; i++) {
      53           0 :         mask |= 1 << i;
      54             :     }
      55           0 :     value |= (bit_value<<bit_offset) & mask;
      56           0 : }
      57             : #endif