From c057be8ed156ad455a5cfb4c9b720e6e3bfd4c50 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Sat, 19 Sep 2020 09:38:20 +0100 Subject: [PATCH] Plane: conditionally compile ADSB support --- ArduPlane/ArduPlane.cpp | 6 ++++-- ArduPlane/GCS_Mavlink.cpp | 6 ++++++ ArduPlane/Parameters.cpp | 2 ++ ArduPlane/Plane.h | 5 ++++- ArduPlane/avoidance_adsb.cpp | 3 +++ ArduPlane/is_flying.cpp | 2 ++ ArduPlane/mode.cpp | 3 ++- 7 files changed, 23 insertions(+), 4 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 19e0796a62..3064c8595c 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -92,7 +92,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400), #endif SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50), +#if HAL_ADSB_ENABLED SCHED_TASK(avoidance_adsb_update, 10, 100), +#endif SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200), SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100), #if STATS_ENABLED == ENABLED @@ -265,10 +267,10 @@ void Plane::one_second_loop() // make it possible to change orientation at runtime ahrs.update_orientation(); - +#if HAL_ADSB_ENABLED adsb.set_stall_speed_cm(aparm.airspeed_min); adsb.set_max_speed(aparm.airspeed_max); - +#endif ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2)); // sync MAVLink system ID diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index c397a79a70..c051d8bd40 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -434,8 +434,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) break; case MSG_ADSB_VEHICLE: +#if HAL_ADSB_ENABLED CHECK_PAYLOAD_SIZE(ADSB_VEHICLE); plane.adsb.send_adsb_vehicle(chan); +#endif break; case MSG_AOA_SSA: @@ -708,7 +710,9 @@ MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlin void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status, const mavlink_message_t &msg) { +#if HAL_ADSB_ENABLED plane.avoidance_adsb.handle_msg(msg); +#endif GCS_MAVLINK::packetReceived(status, msg); } @@ -1475,7 +1479,9 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG: case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC: case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT: +#if HAL_ADSB_ENABLED plane.adsb.handle_message(chan, msg); +#endif break; default: diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index ea5ae47eb0..a1e81f1953 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -911,6 +911,7 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(terrain, "TERRAIN_", AP_Terrain), #endif +#if HAL_ADSB_ENABLED // @Group: ADSB_ // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp GOBJECT(adsb, "ADSB_", AP_ADSB), @@ -918,6 +919,7 @@ const AP_Param::Info Plane::var_info[] = { // @Group: AVD_ // @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Plane), +#endif // @Group: Q_ // @Path: quadplane.cpp diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 486c5bca48..56de153687 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -108,7 +108,9 @@ #include "RC_Channel.h" // RC Channel Library #include "Parameters.h" +#if HAL_ADSB_ENABLED #include "avoidance_adsb.h" +#endif #include "AP_Arming.h" #if CONFIG_HAL_BOARD == HAL_BOARD_SITL @@ -632,11 +634,12 @@ private: FUNCTOR_BIND_MEMBER(&Plane::adjusted_relative_altitude_cm, int32_t), FUNCTOR_BIND_MEMBER(&Plane::disarm_if_autoland_complete, void), FUNCTOR_BIND_MEMBER(&Plane::update_flight_stage, void)}; - +#if HAL_ADSB_ENABLED AP_ADSB adsb; // avoidance of adsb enabled vehicles (normally manned vheicles) AP_Avoidance_Plane avoidance_adsb{adsb}; +#endif // Outback Challenge Failsafe Support #if ADVANCED_FAILSAFE == ENABLED diff --git a/ArduPlane/avoidance_adsb.cpp b/ArduPlane/avoidance_adsb.cpp index 52ca298ea0..a7ca1530d8 100644 --- a/ArduPlane/avoidance_adsb.cpp +++ b/ArduPlane/avoidance_adsb.cpp @@ -2,6 +2,7 @@ #include #include "Plane.h" +#if HAL_ADSB_ENABLED void Plane::avoidance_adsb_update(void) { adsb.update(); @@ -202,3 +203,5 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl return false; } +#endif + diff --git a/ArduPlane/is_flying.cpp b/ArduPlane/is_flying.cpp index 819c158b7e..5fe09e2e2c 100644 --- a/ArduPlane/is_flying.cpp +++ b/ArduPlane/is_flying.cpp @@ -154,7 +154,9 @@ void Plane::update_is_flying_5Hz(void) } } previous_is_flying = new_is_flying; +#if HAL_ADSB_ENABLED adsb.set_is_flying(new_is_flying); +#endif #if PARACHUTE == ENABLED parachute.set_is_flying(new_is_flying); #endif diff --git a/ArduPlane/mode.cpp b/ArduPlane/mode.cpp index e660c7f5c7..74ac0138d4 100644 --- a/ArduPlane/mode.cpp +++ b/ArduPlane/mode.cpp @@ -72,8 +72,9 @@ bool Mode::enter() // start with throttle suppressed in auto_throttle modes plane.throttle_suppressed = plane.auto_throttle_mode; - +#if HAL_ADSB_ENABLED plane.adsb.set_is_auto_mode(plane.auto_navigation_mode); +#endif // reset steering integrator on mode change plane.steerController.reset_I();