diff --git a/boards/bitcraze/crazyflie21/default.px4board b/boards/bitcraze/crazyflie21/default.px4board index 0c268b6983a6..13c44d61d964 100644 --- a/boards/bitcraze/crazyflie21/default.px4board +++ b/boards/bitcraze/crazyflie21/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/diatone/mamba-f405-mk2/default.px4board b/boards/diatone/mamba-f405-mk2/default.px4board index e8ee5ec656c8..14e8219028ff 100644 --- a/boards/diatone/mamba-f405-mk2/default.px4board +++ b/boards/diatone/mamba-f405-mk2/default.px4board @@ -36,6 +36,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/flywoo/gn-f405/default.px4board b/boards/flywoo/gn-f405/default.px4board index 54d17325bc92..bd7ea9015d2a 100644 --- a/boards/flywoo/gn-f405/default.px4board +++ b/boards/flywoo/gn-f405/default.px4board @@ -27,6 +27,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/boards/holybro/kakutef7/default.px4board b/boards/holybro/kakutef7/default.px4board index 8aaa037cc55a..49b8f9e0d5e5 100644 --- a/boards/holybro/kakutef7/default.px4board +++ b/boards/holybro/kakutef7/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/omnibus/f4sd/default.px4board b/boards/omnibus/f4sd/default.px4board index 8d3c58a7f0f5..470b95db5db1 100644 --- a/boards/omnibus/f4sd/default.px4board +++ b/boards/omnibus/f4sd/default.px4board @@ -31,6 +31,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/px4/fmu-v2/default.px4board b/boards/px4/fmu-v2/default.px4board index b796d1bfbf49..479e394f6f55 100644 --- a/boards/px4/fmu-v2/default.px4board +++ b/boards/px4/fmu-v2/default.px4board @@ -40,6 +40,7 @@ CONFIG_MODULES_MC_ATT_CONTROL=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y # CONFIG_SENSORS_VEHICLE_AIRSPEED is not set diff --git a/boards/raspberrypi/pico/default.px4board b/boards/raspberrypi/pico/default.px4board index 410f8d4788a9..d481cf3478c4 100644 --- a/boards/raspberrypi/pico/default.px4board +++ b/boards/raspberrypi/pico/default.px4board @@ -32,6 +32,7 @@ CONFIG_MODULES_MC_HOVER_THRUST_ESTIMATOR=y CONFIG_MODULES_MC_POS_CONTROL=y CONFIG_MODULES_MC_RATE_CONTROL=y CONFIG_MODULES_NAVIGATOR=y +CONFIG_NAVIGATOR_ADSB=n CONFIG_MODULES_RC_UPDATE=y CONFIG_MODULES_SENSORS=y CONFIG_SYSTEMCMDS_DMESG=y diff --git a/src/modules/navigator/Kconfig b/src/modules/navigator/Kconfig index 83e42cc594db..429acd60e763 100644 --- a/src/modules/navigator/Kconfig +++ b/src/modules/navigator/Kconfig @@ -19,3 +19,11 @@ menuconfig MODE_NAVIGATOR_VTOL_TAKEOFF Add VTOL takeoff mode to enable support for MAV_CMD_NAV_VTOL_TAKEOFF. The VTOL takes off in MC mode and transition to FW. The mode ends with an infinite loiter + +menuconfig NAVIGATOR_ADSB + bool "Include traffic reporting and avoidance" + default y + depends on MODULES_NAVIGATOR + ---help--- + Add support for acting on ADSB transponder_report or ADSB_VEHICLE MAVLink messages. + Actions are warnings, Loiter, Land and RTL without climb. diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h index b3057e53753f..5e30d22ccc95 100644 --- a/src/modules/navigator/navigator.h +++ b/src/modules/navigator/navigator.h @@ -57,8 +57,11 @@ #include "GeofenceBreachAvoidance/geofence_breach_avoidance.h" +#if CONFIG_NAVIGATOR_ADSB #include +#endif // CONFIG_NAVIGATOR_ADSB #include +#include #include #include #include @@ -136,14 +139,14 @@ class Navigator : public ModuleBase, public ModuleParams */ void publish_vehicle_cmd(vehicle_command_s *vcmd); +#if CONFIG_NAVIGATOR_ADSB /** * Check nearby traffic for potential collisions */ void check_traffic(); - void take_traffic_conflict_action(); - void run_fake_traffic(); +#endif // CONFIG_NAVIGATOR_ADSB /** * Setters @@ -365,7 +368,10 @@ class Navigator : public ModuleBase, public ModuleParams Land _land; /**< class for handling land commands */ PrecLand _precland; /**< class for handling precision land commands */ RTL _rtl; /**< class that handles RTL */ +#if CONFIG_NAVIGATOR_ADSB AdsbConflict _adsb_conflict; /**< class that handles ADSB conflict avoidance */ + traffic_buffer_s _traffic_buffer{}; +#endif // CONFIG_NAVIGATOR_ADSB NavigatorMode *_navigation_mode{nullptr}; /**< abstract pointer to current navigation mode class */ NavigatorMode *_navigation_mode_array[NAVIGATOR_MODE_ARRAY_SIZE] {}; /**< array of navigation modes */ @@ -381,8 +387,6 @@ class Navigator : public ModuleBase, public ModuleParams float _cruising_speed_current_mode{-1.0f}; float _mission_throttle{NAN}; - traffic_buffer_s _traffic_buffer{}; - bool _is_capturing_images{false}; // keep track if we need to stop capturing images diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 5f397786fbe4..a3a10c9464c6 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -52,7 +52,6 @@ #include #include #include -#include #include #include #include @@ -143,9 +142,11 @@ void Navigator::params_update() } _mission.set_command_timeout(_param_mis_command_tout.get()); +#if CONFIG_NAVIGATOR_ADSB _adsb_conflict.set_conflict_detection_params(_param_nav_traff_a_hor_ct.get(), _param_nav_traff_a_ver.get(), _param_nav_traff_collision_time.get(), _param_nav_traff_avoid.get()); +#endif // CONFIG_NAVIGATOR_ADSB } void Navigator::run() @@ -752,8 +753,10 @@ void Navigator::run() } } +#if CONFIG_NAVIGATOR_ADSB /* Check for traffic */ check_traffic(); +#endif // CONFIG_NAVIGATOR_ADSB /* Check geofence violation */ geofence_breach_check(); @@ -1222,6 +1225,7 @@ void Navigator::load_fence_from_file(const char *filename) _geofence.loadFromFile(filename); } +#if CONFIG_NAVIGATOR_ADSB void Navigator::take_traffic_conflict_action() { @@ -1251,12 +1255,10 @@ void Navigator::take_traffic_conflict_action() } } - } void Navigator::run_fake_traffic() { - _adsb_conflict.run_fake_traffic(get_global_position()->lat, get_global_position()->lon, get_global_position()->alt); } @@ -1283,6 +1285,7 @@ void Navigator::check_traffic() _adsb_conflict.remove_expired_conflicts(); } +#endif // CONFIG_NAVIGATOR_ADSB bool Navigator::abort_landing() { @@ -1325,11 +1328,13 @@ int Navigator::custom_command(int argc, char *argv[]) get_instance()->load_fence_from_file(GEOFENCE_FILENAME); return 0; +#if CONFIG_NAVIGATOR_ADSB } else if (!strcmp(argv[0], "fake_traffic")) { get_instance()->run_fake_traffic(); return 0; +#endif // CONFIG_NAVIGATOR_ADSB } return print_usage("unknown command");