diff --git a/libraries/SITL/SIM_CRSF.cpp b/libraries/SITL/SIM_CRSF.cpp new file mode 100644 index 0000000000..c7999d0c76 --- /dev/null +++ b/libraries/SITL/SIM_CRSF.cpp @@ -0,0 +1,82 @@ +/* + 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 . + */ +/* + 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; + } +} \ No newline at end of file diff --git a/libraries/SITL/SIM_CRSF.h b/libraries/SITL/SIM_CRSF.h new file mode 100644 index 0000000000..854edad1e7 --- /dev/null +++ b/libraries/SITL/SIM_CRSF.h @@ -0,0 +1,60 @@ +/* + 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 . + */ +/* + Simulated CRSF device + +./Tools/autotest/sim_vehicle.py --gdb --debug -v ArduCopter -A --uartF=sim:crsf --speedup=1 + +param set SERIAL5_PROTOCOL 23 +reboot + +arm throttle +rc 3 1600 + +*/ + +#pragma once + +#include "SIM_Aircraft.h" +#include +#include "SIM_SerialDevice.h" + +namespace SITL { + +class CRSF : public SerialDevice { +public: + + CRSF() {}; + + // update state + void update(); + +protected: + enum DataID { + VTX_FRAME = 0x00, + VTX_TELEM = 0x01, + VTX_UNKNOWN = 0x02, + MAX_DATA_FRAMES = 0x03 + }; + + const char* dataid_string(DataID id, ssize_t& len); + + char _buffer[64]; + uint16_t _buflen = 0; + uint8_t _id; + uint32_t _last_update_ms; +}; + +}