mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andy Piper
0e921d3c90
Gate the AP_VideoTX include and the extra MSP_TIME_SLOT_MAX slot on AP_MSP_VIDEOTX_ENABLED, whose default now lives here, and label the matching #endifs. sbuf_read_u8/u16 flag an exhausted buffer as an internal error rather than quietly returning 0: callers guard every read, so an underflow is a bug worth surfacing. msp_vtx_set_pitmode uses the new AP_VideoTX::set_option_enabled.
63 lines
1.2 KiB
C++
63 lines
1.2 KiB
C++
#include <AP_HAL/utility/sparse-endian.h>
|
|
#include <AP_InternalError/AP_InternalError.h>
|
|
|
|
#include "msp.h"
|
|
#include "msp_sbuf.h"
|
|
|
|
#include <string.h>
|
|
|
|
#if HAL_MSP_ENABLED
|
|
|
|
uint8_t* MSP::sbuf_ptr(sbuf_t *buf)
|
|
{
|
|
return buf->ptr;
|
|
}
|
|
|
|
uint16_t MSP::sbuf_bytes_remaining(const sbuf_t *buf)
|
|
{
|
|
return buf->end - buf->ptr;
|
|
}
|
|
|
|
bool MSP::sbuf_check_bounds(const sbuf_t *buf, const uint8_t len)
|
|
{
|
|
if (sbuf_bytes_remaining(buf) < len) {
|
|
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
void MSP::sbuf_switch_to_reader(sbuf_t *buf, uint8_t *base)
|
|
{
|
|
buf->end = buf->ptr;
|
|
buf->ptr = base;
|
|
}
|
|
|
|
void MSP::sbuf_write_data(sbuf_t *dst, const void *data, int len)
|
|
{
|
|
if (!sbuf_check_bounds(dst, len)) {
|
|
return;
|
|
}
|
|
memcpy(dst->ptr, data, len);
|
|
dst->ptr += len;
|
|
}
|
|
|
|
uint8_t MSP::sbuf_read_u8(sbuf_t *src)
|
|
{
|
|
if (!sbuf_check_bounds(src, 1)) {
|
|
return 0;
|
|
}
|
|
return *src->ptr++;
|
|
}
|
|
|
|
uint16_t MSP::sbuf_read_u16(sbuf_t *src)
|
|
{
|
|
if (!sbuf_check_bounds(src, 2)) {
|
|
return 0;
|
|
}
|
|
uint16_t val = le16toh_ptr(src->ptr);
|
|
src->ptr += 2;
|
|
return val;
|
|
}
|
|
|
|
#endif //HAL_MSP_ENABLED
|