SITL: add CRSF simulation driver

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 <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;
+    }
+}

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 <http://www.gnu.org/licenses/>.
+ */
+/*
+  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 <SITL/SITL.h>
+#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;
+};
+
+}