diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index fb61d8fd95..0bf2ed9a56 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -84,6 +84,7 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { SCHED_TASK(terrain_update, 5, 500), SCHED_TASK(update_is_flying_5Hz, 10, 100), SCHED_TASK(dataflash_periodic, 1, 300), + SCHED_TASK(adsb_update, 50, 500), }; void Plane::setup() @@ -360,6 +361,7 @@ void Plane::terrain_update(void) #endif } + void Plane::dataflash_periodic(void) { DataFlash.periodic_tasks(); diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 44780dfdd2..161c802294 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -1899,6 +1899,10 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg) (uint32_t)(new_home_loc.alt*0.01f)); break; } + + case MAVLINK_MSG_ID_ADSB_VEHICLE: + plane.adsb.update_vehicle(msg); + break; } // end switch } // end handle mavlink diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 5135704774..37b08ac0f5 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -1022,6 +1022,10 @@ const AP_Param::Info Plane::var_info[] = { GOBJECT(terrain, "TERRAIN_", AP_Terrain), #endif + // @Group: ADSB_ + // @Path: ../libraries/AP_ADSB/AP_ADSB.cpp + GOBJECT(adsb, "ADSB_", AP_ADSB), + // RC channel //----------- // @Group: RC1_ diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index 7f5bce81c6..b1d926c5a6 100644 --- a/ArduPlane/Parameters.h +++ b/ArduPlane/Parameters.h @@ -221,6 +221,7 @@ public: k_param_camera = 160, k_param_camera_mount, k_param_camera_mount2, // unused + k_param_adsb, // // Battery monitoring parameters diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 22a9e04a44..40216d70d0 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -94,6 +94,7 @@ #include // Optical Flow library #include // RSSI Library #include +#include // Configuration #include "config.h" @@ -559,6 +560,17 @@ private: AP_Terrain terrain {ahrs, mission, rally}; #endif + AP_ADSB adsb {ahrs}; + struct { + + // for Loiter_and_descend behavior, keeps track of rate changes + uint32_t time_last_alt_change_ms; + + // previous wp to restore to when switching between modes back to AUTO + Location prev_wp; + } adsb_state; + + // Outback Challenge Failsafe Support #if OBC_FAILSAFE == ENABLED APM_OBC obc {mission, barometer, gps, rcmap}; @@ -909,6 +921,8 @@ private: void update_logging1(void); void update_logging2(void); void terrain_update(void); + void adsb_update(void); + void adsb_handle_vehicle_threats(void); void update_flight_mode(void); void stabilize(); void set_servos_idle(void); diff --git a/ArduPlane/adsb.cpp b/ArduPlane/adsb.cpp new file mode 100644 index 0000000000..8fbf0cb4fd --- /dev/null +++ b/ArduPlane/adsb.cpp @@ -0,0 +1,101 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- +/* + * adsb.cpp + * Copyright (C) Tom Pittenger 2015 + * + * This file is free software: you can redistribute it and/or modify it + * under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along + * with this program. If not, see . + */ + +#include "Plane.h" + +/* + * this module deals with ADS-B handling for ArduPlane + * ADS-B is an RF based collision avoidance protocol to tell nearby aircraft your location + * https://en.wikipedia.org/wiki/Automatic_dependent_surveillance_%E2%80%93_broadcast + * + */ + +/* + handle periodic adsb database maintenance and handle threats + */ +void Plane::adsb_update(void) +{ + adsb.update(); + adsb_handle_vehicle_threats(); +} + +/* + * Handle ADS-B based threats which are platform dependent + */ +void Plane::adsb_handle_vehicle_threats(void) +{ + uint32_t now = millis(); + AP_ADSB::ADSB_BEHAVIOR behavior = adsb.get_behavior(); + + switch (control_mode) { + case AUTO: + if (mission.get_current_nav_cmd().id == MAV_CMD_NAV_TAKEOFF) { + // for testing purposes ignore ADS-B traffic until we get into the air so we don't screw up the sim takeoff + break; + } + switch(behavior) { + case AP_ADSB::ADSB_BEHAVIOR_NONE: + default: + break; + + case AP_ADSB::ADSB_BEHAVIOR_LOITER: + case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND: + if (adsb.get_another_vehicle_within_radius() && !adsb.get_is_evading_threat()) { + adsb.set_is_evading_threat(true); + gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat found, performing LOITER"); + adsb_state.prev_wp = prev_WP_loc; + set_mode(LOITER); + if (behavior == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND) { + adsb_state.time_last_alt_change_ms = now; + } + } + } // switch behavior + break; // case auto + + case LOITER: + switch(behavior) { + case AP_ADSB::ADSB_BEHAVIOR_NONE: + // TODO: recover from this + default: + break; + + case AP_ADSB::ADSB_BEHAVIOR_LOITER: + case AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND: + if (adsb.get_is_evading_threat()) { + if (!adsb.get_another_vehicle_within_radius()) { + adsb.set_is_evading_threat(false); + gcs_send_text(MAV_SEVERITY_CRITICAL, "ADS-B threat gone, continuing mission"); + set_mode(AUTO); + prev_WP_loc = adsb_state.prev_wp; + auto_state.no_crosstrack = false; + } else if (behavior == AP_ADSB::ADSB_BEHAVIOR_LOITER_AND_DESCEND && + now - adsb_state.time_last_alt_change_ms >= 1000) { + // slowly reduce altitude 1m/s while loitering. Drive into the ground if threat persists + adsb_state.time_last_alt_change_ms = now; + next_WP_loc.alt -= 100; + } // get_another_vehicle_within_radius + } // get_is_evading_threat + } // switch behavior + break; // case LOITER + + default: + break; + } // switch control_mode +} +