diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
new file mode 100644
index 0000000000..36bb7cd7ff
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
@@ -0,0 +1,463 @@
+/*
+   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/>.
+*/
+
+#include "AP_CRSF_Telem.h"
+#include "AP_VideoTX.h"
+#include <AP_HAL/utility/sparse-endian.h>
+#include <AP_BattMonitor/AP_BattMonitor.h>
+#include <AP_Common/AP_FWVersion.h>
+#include <AP_GPS/AP_GPS.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_AHRS/AP_AHRS.h>
+#include <AP_Notify/AP_Notify.h>
+#include <math.h>
+#include <stdio.h>
+
+#if HAL_CRSF_TELEM_ENABLED
+
+#define CRSF_DEBUG
+#ifdef CRSF_DEBUG
+# define debug(fmt, args...)	hal.console->printf("CRSF: " fmt "\n", ##args)
+#else
+# define debug(fmt, args...)	do {} while(0)
+#endif
+
+extern const AP_HAL::HAL& hal;
+
+AP_CRSF_Telem *AP_CRSF_Telem::singleton;
+
+AP_CRSF_Telem::AP_CRSF_Telem() : AP_RCTelemetry(0)
+{
+    singleton = this;
+}
+
+AP_CRSF_Telem::~AP_CRSF_Telem(void)
+{
+    singleton = nullptr;
+}
+
+bool AP_CRSF_Telem::init(void)
+{
+    // sanity check that we are using a UART for RC input
+    if (!AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_RCIN, 0)
+        && !AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) {
+        return false;
+    }
+    return AP_RCTelemetry::init();
+}
+
+/*
+  setup ready for passthrough telem
+ */
+void AP_CRSF_Telem::setup_wfq_scheduler(void)
+{
+    // initialize packet weights for the WFQ scheduler
+    // priority[i] = 1/_scheduler.packet_weight[i]
+    // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) )
+
+    // CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit
+    add_scheduler_entry(50, 100);   // heartbeat        10Hz
+    add_scheduler_entry(50, 120);   // Attitude and compass 8Hz
+    add_scheduler_entry(200, 1000); // parameters        1Hz
+    add_scheduler_entry(1300, 500); // battery           2Hz
+    add_scheduler_entry(550, 280);  // GPS               3Hz
+    add_scheduler_entry(550, 500);  // flight mode       2Hz
+}
+
+void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
+{
+}
+
+// WFQ scheduler
+bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
+{
+    switch (idx) {
+    case PARAMETERS:
+        return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
+    default:
+        return _enable_telemetry;
+    }
+}
+
+// WFQ scheduler
+void AP_CRSF_Telem::process_packet(uint8_t idx)
+{
+    // send packet
+    switch (idx) {
+        case HEARTBEAT: // HEARTBEAT
+            calc_heartbeat();
+            break;
+        case ATTITUDE:
+            calc_attitude();
+            break;
+        case PARAMETERS: // update various parameters
+            update_params();
+            break;
+        case BATTERY: // BATTERY
+            calc_battery();
+            break;
+        case GPS: // GPS
+            calc_gps();
+            break;
+        case FLIGHT_MODE: // GPS
+            calc_flight_mode();
+            break;
+        default:
+            break;
+    }
+}
+
+// Process a frame from the CRSF protocol decoder
+bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data) {
+    switch (frame_type) {
+    // this means we are connected to an RC receiver and can send telemetry
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED:
+    // the EVO sends battery frames and we should send telemetry back to populate the OS
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR:
+        _enable_telemetry = true;
+        break;
+
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VTX:
+        process_vtx_frame((VTXFrame*)data);
+        break;
+
+    case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VTX_TELEM:
+        process_vtx_telem_frame((VTXTelemetryFrame*)data);
+        break;
+
+    default:
+        break;
+    }
+    return true;
+}
+
+void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) {
+    vtx->user_frequency = be16toh(vtx->user_frequency);
+
+    debug("VTX: SmartAudio: %d, Avail: %d, FreqMode: %d, Band: %d, Channel: %d, Freq: %d, PitMode: %d, Pwr: %d, Pit: %d",
+        vtx->smart_audio_ver, vtx->is_vtx_available, vtx->is_in_user_frequency_mode,
+        vtx->band, vtx->channel, vtx->is_in_user_frequency_mode ? vtx->user_frequency : AP_VideoTX::get_frequency_mhz(vtx->band, vtx->channel),
+        vtx->is_in_pitmode, vtx->power, vtx->pitmode);
+    AP_VideoTX& apvtx = AP::vtx();
+
+    apvtx.set_enabled(vtx->is_vtx_available);
+    apvtx.set_band(vtx->band);
+    apvtx.set_channel(vtx->channel);
+    if (vtx->is_in_user_frequency_mode) {
+        apvtx.set_frequency_mhz(vtx->user_frequency);
+    } else {
+        apvtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx->band, vtx->channel));
+    }
+    // 14dBm (25mW), 20dBm (100mW), 26dBm (400mW), 29dBm (800mW)
+    switch (vtx->power) {
+        case 0:
+            apvtx.set_power_mw(25);
+            break;
+        case 1:
+            apvtx.set_power_mw(200);
+            break;
+        case 2:
+            apvtx.set_power_mw(500);
+            break;
+        case 3:
+            apvtx.set_power_mw(800);
+            break;
+    }
+    if (vtx->is_in_pitmode) {
+        apvtx.set_options(apvtx.get_options() | uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
+    } else {
+        apvtx.set_options(apvtx.get_options() & ~uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
+    }
+    // make sure the configured values now reflect reality
+    apvtx.set_defaults();
+
+    _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
+}
+
+void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) {
+    vtx->frequency = be16toh(vtx->frequency);
+    debug("VTXTelemetry: Freq: %d, PitMode: %d, Power: %d", vtx->frequency, vtx->pitmode, vtx->power);
+
+    AP_VideoTX& apvtx = AP::vtx();
+
+    apvtx.set_frequency_mhz(vtx->frequency);
+
+    AP_VideoTX::VideoBand band;
+    uint8_t channel;
+    if (AP_VideoTX::get_band_and_channel(vtx->frequency, band, channel)) {
+        apvtx.set_band(uint8_t(band));
+        apvtx.set_channel(channel);
+    }
+
+    apvtx.set_power_dbm(vtx->power);
+
+    if (vtx->pitmode) {
+        apvtx.set_options(apvtx.get_options() | uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
+    } else {
+        apvtx.set_options(apvtx.get_options() & ~uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
+    }
+    // make sure the configured values now reflect reality
+    apvtx.set_defaults();
+
+    _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
+}
+
+// process any changed settings and schedule for transmission
+void AP_CRSF_Telem::update()
+{
+}
+
+void AP_CRSF_Telem::update_params()
+{
+    AP_VideoTX& vtx = AP::vtx();
+
+    _vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || _vtx_freq_change_pending;
+    _vtx_power_change_pending = vtx.update_power() || _vtx_power_change_pending;
+    _vtx_options_change_pending = vtx.update_options() || _vtx_options_change_pending;
+
+    if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) {
+        debug("update_params(): freq %d, chan: %d->%d, band: %d->%d, pwr: %d->%d",
+            vtx.get_frequency_mhz(),
+            vtx.get_channel(), vtx.get_configured_channel(),
+            vtx.get_band(), vtx.get_configured_band(),
+            vtx.get_power_mw(), vtx.get_configured_power_mw());
+
+        _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND;
+        _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX;
+        _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+        _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX;
+
+
+        // make the desired frequency match the desired band and channel
+        if (_vtx_freq_change_pending) {
+            vtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()));
+        }
+
+        uint8_t len = 5;
+        if (_vtx_freq_change_pending && _vtx_freq_update) {
+            _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_FREQ;
+            _telem.ext.command.payload[1] = (vtx.get_frequency_mhz() & 0xFF00) >> 8;
+            _telem.ext.command.payload[2] = (vtx.get_frequency_mhz() & 0xFF);
+            _vtx_freq_update = false;
+            len++;
+        } else if (_vtx_freq_change_pending) {
+            _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_CHANNEL;
+            _telem.ext.command.payload[1] = vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel();
+            _vtx_freq_update = true;
+        } else if (_vtx_power_change_pending && _vtx_dbm_update) {
+            _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_POWER_DBM;
+            _telem.ext.command.payload[1] = vtx.get_configured_power_dbm();
+            _vtx_dbm_update = false;
+        } else if (_vtx_power_change_pending) {
+            _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_POWER;
+            if (vtx.get_configured_power_mw() < 26) {
+                vtx.set_configured_power_mw(25);
+                _telem.ext.command.payload[1] = 0;
+            } else if (vtx.get_configured_power_mw() < 201) {
+                if (vtx.get_configured_power_mw() < 101) {
+                    vtx.set_configured_power_mw(100);
+                } else {
+                    vtx.set_configured_power_mw(200);
+                }
+                _telem.ext.command.payload[1] = 1;
+            } else if (vtx.get_configured_power_mw() < 501) {
+                if (vtx.get_configured_power_mw() < 401) {
+                    vtx.set_configured_power_mw(400);
+                } else {
+                    vtx.set_configured_power_mw(500);
+                }
+                _telem.ext.command.payload[1] = 2;
+            } else {
+                vtx.set_configured_power_mw(800);
+                _telem.ext.command.payload[1] = 3;
+            }
+            _vtx_dbm_update = true;
+        } else if (_vtx_options_change_pending) {
+            _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_PITMODE;
+            _telem.ext.command.payload[1] = vtx.get_configured_options() & uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE);
+        }
+        _telem_pending = true;
+        // calculate command crc
+        uint8_t* crcptr = &_telem.ext.command.destination;
+        uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA);
+        for (uint8_t i = 0; i < len; i++) {
+            crc = crc8_dvb(crc, crcptr[i], 0xBA);
+        }
+        crcptr[len] = crc;
+        _telem_size = len + 1;
+    }
+}
+
+// prepare parameter ping data
+void AP_CRSF_Telem::calc_parameter_ping()
+{
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
+    _telem.ext.ping.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX;
+    _telem.ext.ping.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+    _telem_size = sizeof(ParameterPingFrame);
+    _telem_pending = true;
+}
+
+// prepare qos data - mandatory frame that must be sent periodically
+void AP_CRSF_Telem::calc_heartbeat()
+{
+    _telem.bcast.heartbeat.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
+    _telem_size = sizeof(HeartbeatFrame);
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_HEARTBEAT;
+    _telem_pending = true;
+}
+
+// prepare battery data
+void AP_CRSF_Telem::calc_battery()
+{
+    const AP_BattMonitor &_battery = AP::battery();
+
+    _telem.bcast.battery.voltage = htobe16(uint16_t(roundf(_battery.voltage(0) * 10.0f)));
+
+    float current;
+    if (!_battery.current_amps(current, 0)) {
+        current = 0;
+    }
+    _telem.bcast.battery.current = htobe16(int16_t(roundf(current * 10.0f)));
+
+    float used_mah;
+    if (!_battery.consumed_mah(used_mah, 0)) {
+        used_mah = 0;
+    }
+
+    _telem.bcast.battery.remaining = _battery.capacity_remaining_pct(0);
+
+    int32_t capacity = _battery.pack_capacity_mah(0);
+    _telem.bcast.battery.capacity[0] = (capacity & 0xFF0000) >> 16;
+    _telem.bcast.battery.capacity[1] = (capacity & 0xFF00) >> 8;
+    _telem.bcast.battery.capacity[2] = (capacity & 0xFF);
+
+    _telem_size = sizeof(BatteryFrame);
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR;
+
+    _telem_pending = true;
+}
+
+// prepare gps data
+void AP_CRSF_Telem::calc_gps()
+{
+    const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw)
+
+    _telem.bcast.gps.latitude = htobe32(loc.lat);
+    _telem.bcast.gps.longitude = htobe32(loc.lng);
+    _telem.bcast.gps.groundspeed = htobe16(roundf(AP::gps().ground_speed() * 100000 / 3600));
+    _telem.bcast.gps.altitude = htobe16(loc.alt + 1000);
+    _telem.bcast.gps.gps_heading = htobe16(roundf(AP::gps().ground_course() * 100.0f));
+    _telem.bcast.gps.satellites = AP::gps().num_sats();
+
+    _telem_size = sizeof(AP_CRSF_Telem::GPSFrame);
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_GPS;
+
+    _telem_pending = true;
+}
+
+// prepare attitude data
+void AP_CRSF_Telem::calc_attitude()
+{
+    AP_AHRS &_ahrs = AP::ahrs();
+    WITH_SEMAPHORE(_ahrs.get_semaphore());
+
+    _telem.bcast.attitude.roll_angle =  htobe16(roundf(_ahrs.roll * 10000.0f));
+    _telem.bcast.attitude.pitch_angle =  htobe16(roundf(_ahrs.pitch * 10000.0f));
+    _telem.bcast.attitude.yaw_angle =  htobe16(roundf(_ahrs.yaw * 10000.0f));
+
+    _telem_size = sizeof(AP_CRSF_Telem::AttitudeFrame);
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_ATTITUDE;
+
+    _telem_pending = true;
+}
+
+// prepare flight mode data
+void AP_CRSF_Telem::calc_flight_mode()
+{
+    AP_Notify * notify = AP_Notify::get_singleton();
+    if (notify) {
+        hal.util->snprintf(_telem.bcast.flightmode.flight_mode, 16, "%s", notify->get_flight_mode_str());
+
+        _telem_size = sizeof(AP_CRSF_Telem::FlightModeFrame);
+        _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_FLIGHT_MODE;
+        _telem_pending = true;
+    }
+}
+
+/*
+  fetch CRSF frame data
+ */
+bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data)
+{
+    memset(&_telem, 0, sizeof(TelemetryPayload));
+    run_wfq_scheduler();
+    if (!_telem_pending) {
+        return false;
+    }
+    memcpy(data->payload, &_telem, _telem_size);
+    data->device_address = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;  // sync byte
+    data->length = _telem_size + 2;
+    data->type = _telem_type;
+
+    _telem_pending = false;
+
+    return true;
+}
+
+/*
+  fetch data for an external transport, such as CRSF
+ */
+bool AP_CRSF_Telem::process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data)
+{
+    if (!get_singleton()) {
+        return false;
+    }
+    return singleton->_process_frame(frame_type, data);
+}
+
+/*
+  fetch data for an external transport, such as CRSF
+ */
+bool AP_CRSF_Telem::get_telem_data(AP_RCProtocol_CRSF::Frame* data)
+{
+    if (!get_singleton()) {
+        return false;
+    }
+    return singleton->_get_telem_data(data);
+}
+
+AP_CRSF_Telem *AP_CRSF_Telem::get_singleton(void) {
+    if (!singleton && !hal.util->get_soft_armed()) {
+        // if telem data is requested when we are disarmed and don't
+        // yet have a AP_CRSF_Telem object then try to allocate one
+        new AP_CRSF_Telem();
+        // initialize the passthrough scheduler
+        if (singleton) {
+            singleton->init();
+        }
+    }
+    return singleton;
+}
+
+namespace AP {
+    AP_CRSF_Telem *crsf_telem() {
+        return AP_CRSF_Telem::get_singleton();
+    }
+};
+
+#endif
\ No newline at end of file
diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h
new file mode 100644
index 0000000000..5ae7c6c0c9
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h
@@ -0,0 +1,212 @@
+/*
+   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/>.
+*/
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+
+#ifndef HAL_CRSF_TELEM_ENABLED
+#define HAL_CRSF_TELEM_ENABLED !HAL_MINIMIZE_FEATURES
+#endif
+
+#if HAL_CRSF_TELEM_ENABLED
+
+#include <AP_Notify/AP_Notify.h>
+#include <AP_SerialManager/AP_SerialManager.h>
+#include <AP_HAL/utility/RingBuffer.h>
+#include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
+#include "AP_RCTelemetry.h"
+#include <AP_HAL/utility/sparse-endian.h>
+
+class AP_CRSF_Telem : public AP_RCTelemetry {
+public:
+    AP_CRSF_Telem();
+    ~AP_CRSF_Telem() override;
+
+    /* Do not allow copies */
+    AP_CRSF_Telem(const AP_CRSF_Telem &other) = delete;
+    AP_CRSF_Telem &operator=(const AP_CRSF_Telem&) = delete;
+
+    // init - perform required initialisation
+    virtual bool init() override;
+
+    static AP_CRSF_Telem *get_singleton(void);
+
+    // Broadcast frame definitions courtesy of TBS
+    struct GPSFrame {   // curious fact, calling this GPS makes sizeof(GPS) return 1!
+        int32_t latitude; // ( degree / 10`000`000 )
+        int32_t longitude; // (degree / 10`000`000 )
+        uint16_t groundspeed; // ( km/h / 100 )
+        uint16_t gps_heading; // ( degree / 100 )
+        uint16_t altitude; // ( meter - 1000m offset )
+        uint8_t satellites; // in use ( counter )
+    } PACKED;
+
+    struct HeartbeatFrame {
+        uint8_t origin; // Device addres
+    };
+
+    struct BatteryFrame {
+        uint16_t voltage; // ( mV * 100 )
+        uint16_t current; // ( mA * 100 )
+        uint8_t capacity[3]; // ( mAh )
+        uint8_t remaining; // ( percent )
+    } PACKED;
+
+    struct VTXFrame {
+#if __BYTE_ORDER != __LITTLE_ENDIAN
+#error "Only supported on little-endian architectures"
+#endif
+        uint8_t origin; // address
+        // status
+        uint8_t is_in_pitmode : 1;
+        uint8_t is_in_user_frequency_mode : 1;
+        uint8_t unused : 2;
+        uint8_t is_vtx_available : 1;
+        uint8_t smart_audio_ver : 3;    // SmartAudio_V1 = 0, SmartAudio_V2 = 1
+        // band / channel
+        uint8_t channel : 3;            // 1x-8x
+        uint8_t band : 5;               // A, B, E, AirWave, Race
+        uint16_t user_frequency;
+        uint8_t power : 4;              // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3
+        uint8_t pitmode : 4;            // off = 0, In_Band = 1, Out_Band = 2;
+    } PACKED;
+
+    struct VTXTelemetryFrame {
+        uint8_t origin; // address
+        uint8_t power;              // power in dBm
+        uint16_t frequency;         // frequency in Mhz
+        bool pitmode;               // disable 0, enable 1
+    } PACKED;
+
+    struct LinkStatisticsFrame {
+        uint8_t uplink_rssi_ant1; // ( dBm * -1 )
+        uint8_t uplink_rssi_ant2; // ( dBm * -1 )
+        uint8_t uplink_status; // Package success rate / Link quality ( % )
+        int8_t uplink_snr; // ( db )
+        uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
+        uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
+        uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
+        uint8_t downlink_rssi; // ( dBm * -1 )
+        uint8_t downlink_status; // Downlink package success rate / Link quality ( % ) ● int8_t Downlink SNR ( db )
+    } PACKED;
+
+    struct AttitudeFrame {
+        int16_t pitch_angle; // ( rad / 10000 )
+        int16_t roll_angle; // ( rad / 10000 )
+        int16_t yaw_angle; // ( rad / 10000 )
+    } PACKED;
+
+    struct FlightModeFrame {
+        char flight_mode[16]; // ( Null-terminated string )
+    } PACKED;
+
+    // CRSF_FRAMETYPE_COMMAND
+    struct CommandFrame {
+        uint8_t destination;
+        uint8_t origin;
+        uint8_t command_id;
+        uint8_t payload[9]; // 8 maximum for LED command + crc8
+    } PACKED;
+
+    // CRSF_FRAMETYPE_PARAM_DEVICE_PING
+    struct ParameterPingFrame {
+        uint8_t destination;
+        uint8_t origin;
+    } PACKED;
+
+    union BroadcastFrame {
+        GPSFrame gps;
+        HeartbeatFrame heartbeat;
+        BatteryFrame battery;
+        VTXFrame vtx;
+        LinkStatisticsFrame link;
+        AttitudeFrame attitude;
+        FlightModeFrame flightmode;
+    } PACKED;
+
+    union ExtendedFrame {
+        CommandFrame command;
+        ParameterPingFrame ping;
+    } PACKED;
+
+    union TelemetryPayload {
+        BroadcastFrame bcast;
+        ExtendedFrame ext;
+    } PACKED;
+
+    // Process a frame from the CRSF protocol decoder
+    static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
+    // process any changed settings and schedule for transmission
+    void update();
+    // get next telemetry data for external consumers of SPort data
+    static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame);
+
+private:
+
+    enum SensorType {
+        HEARTBEAT,
+        ATTITUDE,
+        PARAMETERS,
+        BATTERY,
+        GPS,
+        FLIGHT_MODE,
+        NUM_SENSORS
+    };
+
+    // passthrough WFQ scheduler
+    bool is_packet_ready(uint8_t idx, bool queue_empty) override;
+    void process_packet(uint8_t idx) override;
+    void adjust_packet_weight(bool queue_empty) override;
+
+    void calc_parameter_ping();
+    void calc_heartbeat();
+    void calc_battery();
+    void calc_gps();
+    void calc_attitude();
+    void calc_flight_mode();
+    void update_params();
+
+    void process_vtx_frame(VTXFrame* vtx);
+    void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
+
+     // setup ready for passthrough operation
+    void setup_wfq_scheduler(void) override;
+
+     // get next telemetry data for external consumers
+    bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data);
+    bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
+
+    TelemetryPayload _telem;
+    uint8_t _telem_size;
+    uint8_t _telem_type;
+
+    bool _telem_pending;
+    bool _enable_telemetry;
+
+    // vtx state
+    bool _vtx_freq_update;  // update using the frequency method or not
+    bool _vtx_dbm_update; // update using the dbm method or not
+    bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
+    bool _vtx_power_change_pending;
+    bool _vtx_options_change_pending;
+
+    static AP_CRSF_Telem *singleton;
+};
+
+namespace AP {
+    AP_CRSF_Telem *crsf_telem();
+};
+
+#endif
diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
index 003899d121..2316ab7794 100644
--- a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
+++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
@@ -89,7 +89,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
     update_avg_packet_rate();
 
     uint32_t now = AP_HAL::millis();
-    uint8_t max_delay_idx = 0;
+    int8_t max_delay_idx = -1;
     
     float max_delay = 0;
     float delay = 0;
@@ -125,6 +125,10 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
             }
         }
     }
+
+    if (max_delay_idx < 0) {  // nothing was ready
+        return;
+    }
     now = AP_HAL::millis();
 #ifdef TELEM_DEBUG
     _scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2;
diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.h b/libraries/AP_RCTelemetry/AP_RCTelemetry.h
index 43056340f3..18b1ec9dba 100644
--- a/libraries/AP_RCTelemetry/AP_RCTelemetry.h
+++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.h
@@ -84,10 +84,10 @@ private:
 
     // passthrough WFQ scheduler
     virtual void setup_wfq_scheduler() = 0;
-    virtual bool get_next_msg_chunk(void) = 0;
-    virtual bool is_packet_ready(uint8_t idx, bool queue_empty) = 0;
+    virtual bool get_next_msg_chunk(void) { return false; }
+    virtual bool is_packet_ready(uint8_t idx, bool queue_empty) { return true; }
     virtual void process_packet(uint8_t idx) = 0;
-    virtual void adjust_packet_weight(bool queue_empty) = 0;
+    virtual void adjust_packet_weight(bool queue_empty) {};
 
     void update_avg_packet_rate();
 
diff --git a/libraries/AP_RCTelemetry/AP_VideoTX.cpp b/libraries/AP_RCTelemetry/AP_VideoTX.cpp
new file mode 100644
index 0000000000..2e4a73177b
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_VideoTX.cpp
@@ -0,0 +1,233 @@
+/*
+   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/>.
+*/
+
+#include "AP_VideoTX.h"
+#include "AP_CRSF_Telem.h"
+#include <GCS_MAVLink/GCS.h>
+
+extern const AP_HAL::HAL& hal;
+
+AP_VideoTX *AP_VideoTX::singleton;
+
+const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
+
+    // @Param: ENABLE
+    // @DisplayName: Is the Video Transmitter enabled or not
+    // @Description: Toggles the Video Transmitter on and off
+    // @Values: 0:Disable,1:Enable
+    AP_GROUPINFO_FLAGS("ENABLE", 1, AP_VideoTX, _enabled, 0, AP_PARAM_FLAG_ENABLE),
+
+    // @Param: POWER
+    // @DisplayName: Video Transmitter Power Level
+    // @Description: Video Transmitter Power Level. Different VTXs support different power levels, the power level chosen will be rounded down to the nearest supported power level
+    // @Range: 1 1000
+    AP_GROUPINFO("POWER",    2, AP_VideoTX, _power_mw, 0),
+
+    // @Param: CHANNEL
+    // @DisplayName: Video Transmitter Channel
+    // @Description: Video Transmitter Channel
+    // @User: Standard
+    // @Range: 0 7
+    AP_GROUPINFO("CHANNEL",  3, AP_VideoTX, _channel, 0),
+
+    // @Param: BAND
+    // @DisplayName: Video Transmitter Band
+    // @Description: Video Transmitter Band
+    // @User: Standard
+    // @Values: 0:Band A,1:Band B,2:Band E,3:Airwave,4:RaceBand,5:Low RaceBand
+    AP_GROUPINFO("BAND",  4, AP_VideoTX, _band, 0),
+
+    // @Param: FREQ
+    // @DisplayName: Video Transmitter Frequency
+    // @Description: Video Transmitter Frequency. The frequency is derived from the setting of BAND and CHANNEL
+    // @User: Standard
+    // @ReadOnly: True
+    // @Range: 5000 6000
+    AP_GROUPINFO("FREQ",  5, AP_VideoTX, _frequency_mhz, 0),
+
+    // @Param: OPTIONS
+    // @DisplayName: Video Transmitter Options
+    // @Description: Video Transmitter Options.
+    // @User: Advanced
+    // @Bitmask: 0:Pitmode
+    AP_GROUPINFO("OPTIONS",  6, AP_VideoTX, _options, 0),
+
+    AP_GROUPEND
+};
+
+extern const AP_HAL::HAL& hal;
+
+const uint16_t AP_VideoTX::VIDEO_CHANNELS[AP_VideoTX::MAX_BANDS][VTX_MAX_CHANNELS] =
+{
+    { 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725}, /* Band A */
+    { 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, /* Band B */
+    { 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945}, /* Band E */
+    { 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880}, /* Airwave */
+    { 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917}, /* Race */
+    { 5621, 5584, 5547, 5510, 5473, 5436, 5399, 5362}  /* LO Race */
+};
+
+AP_VideoTX::AP_VideoTX()
+{
+    if (singleton) {
+        AP_HAL::panic("Too many VTXs");
+        return;
+    }
+    singleton = this;
+
+    AP_Param::setup_object_defaults(this, var_info);
+}
+
+AP_VideoTX::~AP_VideoTX(void)
+{
+    singleton = nullptr;
+}
+
+bool AP_VideoTX::init(void)
+{
+    if (_initialized) {
+        return false;
+    }
+
+    _current_power = _power_mw;
+    _current_band = _band;
+    _current_channel = _channel;
+    _current_options = _options;
+    _current_enabled = _enabled;
+    _initialized = true;
+
+    return true;
+}
+
+bool AP_VideoTX::get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel)
+{
+    for (uint8_t i = 0; i < AP_VideoTX::MAX_BANDS; i++) {
+        for (uint8_t j = 0; j < VTX_MAX_CHANNELS; j++) {
+            if (VIDEO_CHANNELS[i][j] == freq) {
+                band = VideoBand(i);
+                channel = j;
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+// set the current power
+void AP_VideoTX::set_configured_power_mw(uint16_t power) {
+    _power_mw.set_and_save(power);
+}
+
+// set the power in dbm, rounding appropriately
+void AP_VideoTX::set_power_dbm(uint8_t power) {
+    switch (power) {
+    case 14:
+        _current_power = 25;
+        break;
+    case 20:
+        _current_power = 100;
+        break;
+    case 23:
+        _current_power = 200;
+        break;
+    case 26:
+        _current_power = 400;
+        break;
+    case 27:
+        _current_power = 500;
+        break;
+    case 29:
+        _current_power = 800;
+        break;
+    default:
+        _current_power = uint16_t(roundf(powf(10, power / 10.0f)));
+        break;
+    }
+}
+
+// get the power in dbm, rounding appropriately
+uint8_t AP_VideoTX::get_configured_power_dbm() const {
+    switch (_power_mw.get()) {
+    case 25:    return 14;
+    case 100:   return 20;
+    case 200:   return 23;
+    case 400:   return 26;
+    case 500:   return 27;
+    case 800:   return 29;
+    default:
+        return uint8_t(roundf(10.0f * log10f(_power_mw)));
+    }
+}
+
+// set the current channel
+void AP_VideoTX::set_enabled(bool enabled) {
+    _current_enabled = enabled;
+    if (!_enabled.configured_in_storage()) {
+        _enabled.set_and_save(enabled);
+    }
+}
+
+// peiodic update
+void AP_VideoTX::update(void)
+{
+#if HAL_CRSF_TELEM_ENABLED
+    AP_CRSF_Telem* crsf = AP::crsf_telem();
+
+    if (crsf != nullptr) {
+        crsf->update();
+    }
+#endif
+}
+
+bool AP_VideoTX::have_params_changed() const
+{
+    return update_power()
+        || update_band()
+        || update_channel()
+        || update_options();
+}
+
+// set the current configured values if not currently set in storage
+// this is necessary so that the current settings can be seen
+void AP_VideoTX::set_defaults()
+{
+    if (_defaults_set) {
+        return;
+    }
+
+    if (!_options.configured()) {
+        _options.set_and_save(_current_options);
+    }
+    if (!_channel.configured()) {
+        _channel.set_and_save(_current_channel);
+    }
+    if (!_band.configured()) {
+        _band.set_and_save(_current_band);
+    }
+    if (!_power_mw.configured()) {
+        _power_mw.set_and_save(_current_power);
+    }
+
+    _defaults_set = true;
+    // Output a friendly message so the user knows the VTX has been detected
+    GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VTX: Freq: %dMHz, Power: %dmw, Band: %d, Chan: %d",
+        _frequency_mhz.get(), _power_mw.get(), _band.get() + 1, _channel.get() + 1);
+}
+
+namespace AP {
+    AP_VideoTX& vtx() {
+        return *AP_VideoTX::get_singleton();
+    }
+};
\ No newline at end of file
diff --git a/libraries/AP_RCTelemetry/AP_VideoTX.h b/libraries/AP_RCTelemetry/AP_VideoTX.h
new file mode 100644
index 0000000000..cb512d2de3
--- /dev/null
+++ b/libraries/AP_RCTelemetry/AP_VideoTX.h
@@ -0,0 +1,128 @@
+/*
+   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/>.
+*/
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_Param/AP_Param.h>
+
+#define VTX_MAX_CHANNELS 8
+
+class AP_VideoTX {
+public:
+    AP_VideoTX();
+    ~AP_VideoTX();
+
+    /* Do not allow copies */
+    AP_VideoTX(const AP_VideoTX &other) = delete;
+    AP_VideoTX &operator=(const AP_VideoTX&) = delete;
+
+    // init - perform required initialisation
+    bool init();
+
+    // run any required updates
+    void update();
+
+    static AP_VideoTX *get_singleton(void) {
+        return singleton;
+    }
+    static const struct AP_Param::GroupInfo var_info[];
+
+    enum class VideoOptions {
+        VTX_PITMODE = 1 << 0
+    };
+
+    enum VideoBand {
+        VTX_BAND_A,
+        VTX_BAND_B,
+        VTX_BAND_E,
+        VTX_BAND_FATSHARK,
+        VTX_BAND_RACEBAND,
+        VTX_BAND_LOW_RACEBAND,
+        MAX_BANDS
+    };
+
+    static const uint16_t VIDEO_CHANNELS[MAX_BANDS][VTX_MAX_CHANNELS];
+
+    static uint16_t get_frequency_mhz(uint8_t band, uint8_t channel) { return VIDEO_CHANNELS[band][channel]; }
+    static bool get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel);
+
+    void set_frequency_mhz(uint16_t freq) { _frequency_mhz.set_and_save(freq); }
+    uint16_t get_frequency_mhz() const { return _frequency_mhz; }
+    // get / set power level
+    void set_power_mw(uint16_t power) { _current_power = power; }
+    void set_power_dbm(uint8_t power);
+    void set_configured_power_mw(uint16_t power);
+    uint16_t get_configured_power_mw() const { return _power_mw; }
+    uint16_t get_power_mw() const { return _current_power; }
+    uint8_t get_configured_power_dbm() const;
+    bool update_power() const { return _defaults_set && _power_mw != _current_power; }
+    // get / set the frequency band
+    void set_band(uint8_t band) { _current_band = band; }
+    uint8_t get_configured_band() const { return _band; }
+    uint8_t get_band() const { return _current_band; }
+    bool update_band() const { return _defaults_set && _band != _current_band; }
+    // get / set the frequency channel
+    void set_channel(uint8_t channel) { _current_channel = channel; }
+    uint8_t get_configured_channel() const { return _channel; }
+    uint8_t get_channel() const { return _current_channel; }
+    bool update_channel() const { return _defaults_set && _channel != _current_channel; }
+    // get / set vtx option
+    void set_options(uint8_t options) { _current_options = options; }
+    uint8_t get_configured_options() const { return _options; }
+    uint8_t get_options() const { return _current_options; }
+    bool update_options() const { return _defaults_set && _options != _current_options; }
+    // get / set whether the vtx is enabled
+    void set_enabled(bool enabled);
+    bool get_enabled() const { return _enabled; }
+    bool update_enabled() const { return _defaults_set && _enabled != _current_enabled; }
+
+    // have the parameters been updated
+    bool have_params_changed() const;
+    // set configured defaults from current settings
+    void set_defaults();
+
+    static AP_VideoTX *singleton;
+
+private:
+    // channel frequency
+    AP_Int16 _frequency_mhz;
+    
+    // power output in mw
+    AP_Int16 _power_mw;
+    uint16_t _current_power;
+
+    // frequency band
+    AP_Int8 _band;
+    uint16_t _current_band;
+
+    // frequency channel
+    AP_Int8 _channel;
+    uint8_t _current_channel;
+
+    // vtx options
+    AP_Int8 _options;
+    uint8_t _current_options;
+
+    AP_Int8 _enabled;
+    bool _current_enabled;
+
+    bool _initialized;
+    // when defaults have been configured
+    bool _defaults_set;
+};
+
+namespace AP {
+    AP_VideoTX& vtx();
+};
\ No newline at end of file