From 820d6654af03d683b9fe50312097fc67f6785ce8 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 28 May 2020 21:50:07 +0100 Subject: [PATCH] AP_Vehicle: add VideoTX --- libraries/AP_Vehicle/AP_Vehicle.cpp | 6 ++++++ libraries/AP_Vehicle/AP_Vehicle.h | 2 ++ 2 files changed, 8 insertions(+) diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 8e4040c4cc..6b8eaca3f5 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -27,6 +27,10 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = { AP_SUBGROUPINFO(visual_odom, "VISO", 3, AP_Vehicle, AP_VisualOdom), #endif + // @Group: VTX_ + // @Path: ../AP_RCTelemetry/AP_VideoTX.cpp + AP_SUBGROUPINFO(vtx, "VTX_", 4, AP_Vehicle, AP_VideoTX), + AP_GROUPEND }; @@ -102,6 +106,7 @@ void AP_Vehicle::setup() // init library used for visual position estimation visual_odom.init(); #endif + vtx.init(); #if AP_PARAM_KEY_DUMP AP_Param::show_all(hal.console, true); @@ -138,6 +143,7 @@ const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_GyroFFT, &vehicle.gyro_fft, update_parameters, 1, 50), #endif SCHED_TASK(update_dynamic_notch, 200, 200), + SCHED_TASK_CLASS(AP_VideoTX, &vehicle.vtx, update, 2, 100), SCHED_TASK(send_watchdog_reset_statustext, 0.1, 20), }; diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h index ac150bdc04..afd6a2d595 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.h +++ b/libraries/AP_Vehicle/AP_Vehicle.h @@ -41,6 +41,7 @@ #include #include #include +#include class AP_Vehicle : public AP_HAL::HAL::Callbacks { @@ -221,6 +222,7 @@ protected: #if HAL_GYROFFT_ENABLED AP_GyroFFT gyro_fft; #endif + AP_VideoTX vtx; AP_SerialManager serial_manager; AP_Relay relay;