ardupilot/libraries/AP_Frsky_Telem/AP_Frsky_MAVlite_Message.cpp

56 lines
1.6 KiB
C++

#include "AP_Frsky_MAVlite_Message.h"
#include <AP_Math/AP_Math.h>
bool AP_Frsky_MAVlite_Message::get_bytes(uint8_t *bytes, const uint8_t offset, const uint8_t count) const
{
if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
return false;
}
memcpy(bytes, &payload[offset], count);
return true;
}
bool AP_Frsky_MAVlite_Message::set_bytes(const uint8_t *bytes, const uint8_t offset, const uint8_t count)
{
if (offset + count > MAVLITE_MAX_PAYLOAD_LEN) {
return false;
}
memcpy(&payload[offset], bytes, count);
len += count;
return true;
}
bool AP_Frsky_MAVlite_Message::get_string(char* value, const uint8_t offset) const
{
if (get_bytes((uint8_t*)value, offset, MIN((uint8_t)16, len - offset))) {
value[MIN((uint8_t)16, len - offset)] = 0x00; // terminator
return true;
}
return false;
}
bool AP_Frsky_MAVlite_Message::set_string(const char* value, const uint8_t offset)
{
return set_bytes((uint8_t*)value, offset, MIN((uint8_t)16, strlen(value)));
}
uint8_t AP_Frsky_MAVlite_Message::bit8_unpack(const uint8_t value, const uint8_t bit_count, const uint8_t bit_offset)
{
uint8_t mask = 0;
for (uint8_t i=bit_offset; i<=bit_count; i++) {
mask |= 1 << i;
}
return (value & mask) >> bit_offset;
}
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)
{
uint8_t mask = 0;
for (uint8_t i=bit_offset; i<=bit_count; i++) {
mask |= 1 << i;
}
value |= (bit_value<<bit_offset) & mask;
}