diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 7a982cbedb..338081b6c9 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 a90c29a5d1..7c8fc1810e 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[];