mirror of https://github.com/ArduPilot/ardupilot
82 lines
2.3 KiB
C++
82 lines
2.3 KiB
C++
|
/*
|
||
|
|
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;
|
||
|
|
}
|
||
|
|
}
|