ardupilot/libraries/SITL/SIM_CRSF.cpp

82 lines
2.3 KiB
C++
Raw Normal View History

2020-06-04 15:00:16 -03:00
/*
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
(at your option) any later version.
This program is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with this program. If not, see <http://www.gnu.org/licenses/>.
*/
/*
Base class for CRSF telemetery
*/
#include "SIM_CRSF.h"
using namespace SITL;
extern const AP_HAL::HAL& hal;
const char *CRSF::dataid_string(DataID id, ssize_t& len)
{
switch (id) {
case DataID::VTX_FRAME: {
static const uint8_t vtx_frame[] = { 0xC8, 0x8, 0xF, 0xCE, 0x30, 0x8, 0x16, 0xE9, 0x0, 0x5F }; // VTX
len = sizeof(vtx_frame);
return (const char*)vtx_frame;
}
break;
case DataID::VTX_TELEM: {
static const uint8_t vtx_frame[] = { 0xC8, 0x7, 0x10, 0xCE, 0xE, 0x16, 0x65, 0x0, 0x1B }; // VTX Telem
len = sizeof(vtx_frame);
return (const char*)vtx_frame;
}
break;
case DataID::VTX_UNKNOWN: {
static const uint8_t vtx_frame[] = { 0xC8, 0x9, 0x8, 0x0, 0x9E, 0x0, 0x0, 0x0, 0x0, 0x0, 0x95}; // Battery 15.8v
len = sizeof(vtx_frame);
return (const char*)vtx_frame;
}
default:
break;
}
return "UNKNOWN";
}
void CRSF::update()
{
const ssize_t n = read_from_autopilot(&_buffer[_buflen], ARRAY_SIZE(_buffer) - _buflen - 1);
if (n != -1) {
_buflen += n;
}
// update every 400ms
uint32_t now = AP_HAL::millis();
if (now - _last_update_ms < 400) {
return;
}
_last_update_ms = now;
ssize_t len = 0;
ssize_t index = 0;
const char* bytes = dataid_string(DataID(_id), len);
while (len > 0) {
const ssize_t nwrite = write_to_autopilot(&bytes[index], len);
len -= nwrite;
index += nwrite;
}
_id = (_id + 1) % MAX_DATA_FRAMES;
if (_buflen == 0) {
return;
}
}