From 3ea05fa78b59520bd8127a746c9ddc6f9dfb3127 Mon Sep 17 00:00:00 2001
From: Andy Piper <github@andypiper.com>
Date: Wed, 18 Dec 2019 22:35:32 +0000
Subject: [PATCH] AP_Vehicle: add common parameter and scheduling management.
 Add runcam.

---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 48 ++++++++++++++++++++++++++++-
 libraries/AP_Vehicle/AP_Vehicle.h   | 17 +++++++++-
 2 files changed, 63 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index eb5b474184..cd29bb1fe8 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -1,4 +1,50 @@
-#include <AP_Vehicle/AP_Vehicle.h>
+#include "AP_Vehicle.h"
+
+#define SCHED_TASK(func, rate_hz, max_time_micros) SCHED_TASK_CLASS(AP_Vehicle, &vehicle, func, rate_hz, max_time_micros)
+
+/*
+  2nd group of parameters
+ */
+const AP_Param::GroupInfo AP_Vehicle::var_info[] = {
+#if HAL_RUNCAM_ENABLED
+    // @Group: CAM_RC_
+    // @Path: ../AP_Camera/AP_RunCam.cpp
+    AP_SUBGROUPINFO(runcam, "CAM_RC_", 1, AP_Vehicle, AP_RunCam),
+#endif
+
+    AP_GROUPEND
+};
+
+// reference to the vehicle. using AP::vehicle() here does not work on clang
+#if APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
+AP_Vehicle& vehicle = *AP_Vehicle::get_singleton();
+#else
+extern AP_Vehicle& vehicle;
+#endif
+
+/*
+  common scheduler table for fast CPUs - all common vehicle tasks
+  should be listed here, along with how often they should be called (in hz)
+  and the maximum time they are expected to take (in microseconds)
+ */
+const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
+#if HAL_RUNCAM_ENABLED
+    SCHED_TASK_CLASS(AP_RunCam,    &vehicle.runcam,              update,          50,  50),
+#endif
+};
+
+void AP_Vehicle::get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks)
+{
+    tasks = scheduler_tasks;
+    num_tasks = ARRAY_SIZE(scheduler_tasks);
+}
+
+// initialize the vehicle
+void AP_Vehicle::init_vehicle() {
+#if HAL_RUNCAM_ENABLED
+    runcam.init();
+#endif
+}
 
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index c953b87a9d..06b2510219 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -32,8 +32,10 @@
 #include <AP_RangeFinder/AP_RangeFinder.h>
 #include <AP_Relay/AP_Relay.h>                      // APM relay
 #include <AP_RSSI/AP_RSSI.h>                        // RSSI Library
+#include <AP_Scheduler/AP_Scheduler.h>
 #include <AP_SerialManager/AP_SerialManager.h>      // Serial manager library
 #include <AP_ServoRelayEvents/AP_ServoRelayEvents.h>
+#include <AP_Camera/AP_RunCam.h>
 
 class AP_Vehicle : public AP_HAL::HAL::Callbacks {
 
@@ -43,6 +45,7 @@ public:
         if (_singleton) {
             AP_HAL::panic("Too many Vehicles");
         }
+        AP_Param::setup_object_defaults(this, var_info);
         _singleton = this;
     }
 
@@ -107,6 +110,8 @@ public:
         AP_Int16 angle_max;
     };
 
+    void get_common_scheduler_tasks(const AP_Scheduler::Task*& tasks, uint8_t& num_tasks);
+
 protected:
 
     // board specific config
@@ -126,7 +131,9 @@ protected:
     RangeFinder rangefinder;
 
     AP_RSSI rssi;
-
+#if HAL_RUNCAM_ENABLED
+    AP_RunCam runcam;
+#endif
     AP_SerialManager serial_manager;
 
     AP_Relay relay;
@@ -146,6 +153,12 @@ protected:
     AP_AHRS_DCM ahrs;
 #endif
 
+    // initialize the vehicle
+    void init_vehicle();
+
+    static const struct AP_Param::GroupInfo var_info[];
+    static const struct AP_Scheduler::Task scheduler_tasks[];
+
 private:
 
     static AP_Vehicle *_singleton;
@@ -158,4 +171,6 @@ namespace AP {
 
 extern const AP_HAL::HAL& hal;
 
+extern const AP_Param::Info vehicle_var_info[];
+
 #include "AP_Vehicle_Type.h"