From ee273da50dfcfeb02efa5da83b4ed67b04352ce4 Mon Sep 17 00:00:00 2001
From: Joshua Henderson <hendjoshsr71@gmail.com>
Date: Mon, 3 Jan 2022 16:17:01 -0500
Subject: [PATCH] AP_Vehicle: move Airspeed to AP_Vehicle

---
 libraries/AP_Vehicle/AP_Vehicle.cpp | 22 +++++++++++++++++++++-
 libraries/AP_Vehicle/AP_Vehicle.h   |  4 ++++
 2 files changed, 25 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp
index 7a982cbedb5..338081b6c9b 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.cpp
+++ b/libraries/AP_Vehicle/AP_Vehicle.cpp
@@ -67,6 +67,12 @@ const AP_Param::GroupInfo AP_Vehicle::var_info[] = {
     AP_SUBGROUPINFO(efi, "EFI", 9, AP_Vehicle, AP_EFI),
 #endif
 
+#if AP_AIRSPEED_ENABLED
+    // @Group: ARSPD
+    // @Path: ../AP_Airspeed/AP_Airspeed.cpp
+    AP_SUBGROUPINFO(airspeed, "ARSPD", 10, AP_Vehicle, AP_Airspeed),
+#endif
+
     AP_GROUPEND
 };
 
@@ -146,6 +152,18 @@ void AP_Vehicle::setup()
     // init_ardupilot is where the vehicle does most of its initialisation.
     init_ardupilot();
 
+#if AP_AIRSPEED_ENABLED
+    airspeed.init();
+    if (airspeed.enabled()) {
+        airspeed.calibrate(true);
+    } 
+#if APM_BUILD_TYPE(APM_BUILD_ArduPlane)
+    else {
+        GCS_SEND_TEXT(MAV_SEVERITY_WARNING,"No airspeed sensor present or enabled");
+    }
+#endif
+#endif  // AP_AIRSPEED_ENABLED
+
 #if !APM_BUILD_TYPE(APM_BUILD_Replay)
     SRV_Channels::init();
 #endif
@@ -256,6 +274,9 @@ SCHED_TASK_CLASS arguments:
 
  */
 const AP_Scheduler::Task AP_Vehicle::scheduler_tasks[] = {
+#if AP_AIRSPEED_ENABLED
+    SCHED_TASK_CLASS(AP_Airspeed,  &vehicle.airspeed,       update,                   10, 100, 41),    // NOTE: the priority number here should be right before Plane's calc_airspeed_errors
+#endif
 #if HAL_RUNCAM_ENABLED
     SCHED_TASK_CLASS(AP_RunCam,    &vehicle.runcam,         update,                   50, 50, 200),
 #endif
@@ -486,7 +507,6 @@ void AP_Vehicle::accel_cal_update()
 #endif // HAL_INS_ENABLED
 }
 
-
 AP_Vehicle *AP_Vehicle::_singleton = nullptr;
 
 AP_Vehicle *AP_Vehicle::get_singleton()
diff --git a/libraries/AP_Vehicle/AP_Vehicle.h b/libraries/AP_Vehicle/AP_Vehicle.h
index a90c29a5d11..7c8fc1810e3 100644
--- a/libraries/AP_Vehicle/AP_Vehicle.h
+++ b/libraries/AP_Vehicle/AP_Vehicle.h
@@ -367,6 +367,10 @@ protected:
     AP_EFI efi;
 #endif
 
+#if AP_AIRSPEED_ENABLED
+    AP_Airspeed airspeed;
+#endif
+
     static const struct AP_Param::GroupInfo var_info[];
     static const struct AP_Scheduler::Task scheduler_tasks[];