diff --git a/libraries/AP_Avoidance/AP_Avoidance.cpp b/libraries/AP_Avoidance/AP_Avoidance.cpp new file mode 100644 index 0000000000..ce4a39d390 --- /dev/null +++ b/libraries/AP_Avoidance/AP_Avoidance.cpp @@ -0,0 +1,675 @@ +#include "AP_Avoidance.h" + +extern const AP_HAL::HAL& hal; + +#include +#include + +#define AVOIDANCE_DEBUGGING 0 + +#if AVOIDANCE_DEBUGGING +#include +#define debug(fmt, args ...) do {::fprintf(stderr,"%s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0) +#else +#define debug(fmt, args ...) +#endif + +// table of user settable parameters +const AP_Param::GroupInfo AP_Avoidance::var_info[] = { + + // @Param: ENABLE + // @DisplayName: Enable Avoidance using ADSB + // @Description: Enable Avoidance using ADSB + // @Values: 0:Disabled,1:Enabled + // @User: Advanced + AP_GROUPINFO("ENABLE", 1, AP_Avoidance, _enabled, 0), + + // @Param: F_ACTION + // @DisplayName: Collision Avoidance Behavior + // @Description: Specifies aircraft behaviour when a collision is imminent + // The following values should come from the mavlink COLLISION_ACTION enum + // @Values: 0:None,1:Report,2:Climb Or Descend,3:Move Horizontally,4:Move Perpendicularly in 3D,5:RTL,6:Hover + // @User: Advanced + AP_GROUPINFO("F_ACTION", 2, AP_Avoidance, _fail_action, MAV_COLLISION_ACTION_REPORT), + + // @Param: W_ACTION + // @DisplayName: Collision Avoidance Behavior - Warn + // @Description: Specifies aircraft behaviour when a collision may occur + // The following values should come from the mavlink COLLISION_ACTION enum + // @Values: 0:None,1:Report + // @User: Advanced + AP_GROUPINFO("W_ACTION", 3, AP_Avoidance, _warn_action, MAV_COLLISION_ACTION_REPORT), + + // @Param: F_RCVRY + // @DisplayName: Recovery behaviour after a fail event + // @Description: Determines what the aircraft will do after a fail event is resolved + // @Values: 0:Continue failsafe action,1:Resume previous flight mode + // @User: Advanced + AP_GROUPINFO("F_RCVRY", 4, AP_Avoidance, _fail_recovery, AP_AVOIDANCE_RECOVERY_NONE), + + // @Param: OBS_MAX + // @DisplayName: Maximum number of obstacles to track + // @Description: Maximum number of obstacles to track + // @User: Advanced + AP_GROUPINFO("OBS_MAX", 5, AP_Avoidance, _obstacles_max, 20), + + // @Param: W_TIME + // @DisplayName: Time Horizon Warn + // @Description: Aircraft velocity vectors are multiplied by this time to determine closest approach. If this results in an approach closer than W_DIST_XY or W_DIST_Z then W_ACTION is undertaken (assuming F_ACTION is not undertaken) + // @User: Advanced + AP_GROUPINFO("W_TIME", 6, AP_Avoidance, _warn_time_horizon, 30), + + // @Param: F_TIME + // @DisplayName: Time Horizon Fail + // @Description: Aircraft velocity vectors are multiplied by this time to determine closest approach. If this results in an approach closer than F_DIST_XY or F_DIST_Z then F_ACTION is undertaken + // @User: Advanced + AP_GROUPINFO("F_TIME", 7, AP_Avoidance, _fail_time_horizon, 30), + + // @Param: W_DIST_XY + // @DisplayName: Distance Warn XY + // @Description: Closest allowed projected distance before W_ACTION is undertaken + // @User: Advanced + AP_GROUPINFO("W_DIST_XY", 8, AP_Avoidance, _warn_distance_xy, 300), + + // @Param: F_DIST_XY + // @DisplayName: Distance Fail XY + // @Description: Closest allowed projected distance before F_ACTION is undertaken + // @User: Advanced + AP_GROUPINFO("F_DIST_XY", 9, AP_Avoidance, _fail_distance_xy, 100), + + // @Param: W_DIST_Z + // @DisplayName: Distance Warn Z + // @Description: Closest allowed projected distance before BEHAVIOUR_W is undertaken + // @User: Advanced + AP_GROUPINFO("W_DIST_Z", 10, AP_Avoidance, _warn_distance_z, 300), + + // @Param: F_DIST_Z + // @DisplayName: Distance Fail Z + // @Description: Closest allowed projected distance before BEHAVIOUR_F is undertaken + // @User: Advanced + AP_GROUPINFO("F_DIST_Z", 11, AP_Avoidance, _fail_distance_z, 100), + + AP_GROUPEND +}; + +AP_Avoidance::AP_Avoidance(AP_AHRS &ahrs, AP_ADSB &adsb) : + _ahrs(ahrs), + _adsb(adsb) +{ + AP_Param::setup_object_defaults(this, var_info); +} + +/* + * Initialize variables and allocate memory for array + */ +void AP_Avoidance::init(void) +{ + debug("ADSB initialisation: %d obstacles", _obstacles_max.get()); + if (_obstacles == NULL) { + _obstacles = new AP_Avoidance::Obstacle[_obstacles_max]; + + if (_obstacles == NULL) { + // dynamic RAM allocation of _obstacles[] failed, disable gracefully + hal.console->printf("Unable to initialize Avoidance obstacle list\n"); + // disable ourselves to avoid repeated allocation attempts + _enabled.set(0); + return; + } + _obstacles_allocated = _obstacles_max; + } + _obstacle_count = 0; + _last_state_change_ms = 0; + _threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + _gcs_cleared_messages_first_sent = std::numeric_limits::max(); + _current_most_serious_threat = -1; +} + +/* + * de-initialize and free up some memory + */ +void AP_Avoidance::deinit(void) +{ + if (_obstacles != nullptr) { + delete [] _obstacles; + _obstacles = nullptr; + _obstacles_allocated = 0; + } + _obstacle_count = 0; +} + +bool AP_Avoidance::check_startup() +{ + if (!_enabled) { + if (_obstacles != nullptr) { + deinit(); + } + // nothing to do + return false; + } + if (_obstacles == nullptr) { + init(); + } + return _obstacles != nullptr; +} + +// vel is north/east/down! +void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms, + const MAV_COLLISION_SRC src, + const uint32_t src_id, + const Location &loc, + const Vector3f &vel_ned) +{ + if (! check_startup()) { + return; + } + uint32_t oldest_timestamp = std::numeric_limits::max(); + uint8_t oldest_index = 255; // avoid compiler warning with initialisation + int16_t index = -1; + uint8_t i; + for (i=0; i<_obstacle_count; i++) { + if (_obstacles[i].src_id == src_id && + _obstacles[i].src == src) { + // pre-existing obstacle found; we will update its information + index = i; + break; + } + if (_obstacles[i].timestamp_ms < oldest_timestamp) { + oldest_timestamp = _obstacles[i].timestamp_ms; + oldest_index = i; + } + } + if (index == -1) { + // existing obstacle not found. See if we can store it anyway: + if (i <_obstacles_allocated) { + // have room to store more vehicles... + index = _obstacle_count++; + } else if (oldest_timestamp < obstacle_timestamp_ms) { + // replace this very old entry with this new data + index = oldest_index; + } + _obstacles[index].src = src; + _obstacles[index].src_id = src_id; + } + + if (index == -1) { + // no room for this (old?!) data + return; + } + _obstacles[index]._location = loc; + _obstacles[index]._velocity = vel_ned; + _obstacles[index].timestamp_ms = obstacle_timestamp_ms; +} + +void AP_Avoidance::add_obstacle(const uint32_t obstacle_timestamp_ms, + const MAV_COLLISION_SRC src, + const uint32_t src_id, + const Location &loc, + const float cog, + const float hspeed, + const float vspeed) +{ + Vector3f vel; + vel[0] = hspeed * cosf(radians(cog)); + vel[1] = hspeed * sinf(radians(cog)); + vel[2] = vspeed; + // debug("cog=%f hspeed=%f veln=%f vele=%f", cog, hspeed, vel[0], vel[1]); + return add_obstacle(obstacle_timestamp_ms, src, src_id, loc, vel); +} + +uint32_t AP_Avoidance::src_id_for_adsb_vehicle(AP_ADSB::adsb_vehicle_t vehicle) const +{ + // TODO: need to include squawk code and callsign + return vehicle.info.ICAO_address; +} + +void AP_Avoidance::get_adsb_samples() +{ + AP_ADSB::adsb_vehicle_t vehicle; + while (_adsb.next_sample(vehicle)) { + uint32_t src_id = src_id_for_adsb_vehicle(vehicle); + Location loc = vehicle.get_location(); + add_obstacle(vehicle.last_update_ms, + MAV_COLLISION_SRC_ADSB, + src_id, + loc, + vehicle.info.heading/100.0f, + vehicle.info.hor_velocity/100.0f, + -vehicle.info.ver_velocity/1000.0f); // convert mm-up to m-down + } +} + +float closest_approach_xy(const Location &my_loc, + const Vector3f &my_vel, + const Location &obstacle_loc, + const Vector3f &obstacle_vel, + const uint8_t time_horizon) +{ + + Vector2f delta_vel_ne = Vector2f(obstacle_vel[0] - my_vel[0], obstacle_vel[1] - my_vel[1]); + Vector2f delta_pos_ne = location_diff(obstacle_loc, my_loc); + + Vector2f line_segment_ne = delta_vel_ne * time_horizon; + + float ret = Vector2::closest_distance_between_radial_and_point + (line_segment_ne, + delta_pos_ne); + + debug(" time_horizon: (%d)", time_horizon); + debug(" delta pos: (y=%f,x=%f)", delta_pos_ne[0], delta_pos_ne[1]); + debug(" delta vel: (y=%f,x=%f)", delta_vel_ne[0], delta_vel_ne[1]); + debug(" line segment: (y=%f,x=%f)", line_segment_ne[0], line_segment_ne[1]); + debug(" closest: (%f)", ret); + + return ret; +} + +// returns the closest these objects will get in the body z axis (in metres) +float closest_approach_z(const Location &my_loc, + const Vector3f &my_vel, + const Location &obstacle_loc, + const Vector3f &obstacle_vel, + const uint8_t time_horizon) +{ + + float delta_vel_d = obstacle_vel[2] - my_vel[2]; + float delta_pos_d = obstacle_loc.alt - my_loc.alt; + + float ret; + if (delta_pos_d >= 0 && delta_vel_d >= 0) { + ret = delta_pos_d; + } else if (delta_pos_d <= 0 && delta_vel_d <= 0) { + ret = fabs(delta_pos_d); + } else { + ret = fabs(delta_pos_d - delta_vel_d * time_horizon); + } + + debug(" time_horizon: (%d)", time_horizon); + debug(" delta pos: (%f) metres", delta_pos_d/100.0f); + debug(" delta vel: (%f) m/s", delta_vel_d); + debug(" closest: (%f) metres", ret/100.0f); + + return ret/100.0f; +} + +void AP_Avoidance::update_threat_level(const Location &my_loc, + const Vector3f &my_vel, + AP_Avoidance::Obstacle &obstacle) +{ + + Location &obstacle_loc = obstacle._location; + Vector3f &obstacle_vel = obstacle._velocity; + + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + + const uint32_t obstacle_age = AP_HAL::millis() - obstacle.timestamp_ms; + float closest_xy = closest_approach_xy(my_loc, my_vel, obstacle_loc, obstacle_vel, _fail_time_horizon + obstacle_age/1000); + if (closest_xy < _fail_distance_xy) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_HIGH; + } else { + closest_xy = closest_approach_xy(my_loc, my_vel, obstacle_loc, obstacle_vel, _warn_time_horizon + obstacle_age/1000); + if (closest_xy < _warn_distance_xy) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_LOW; + } + } + + // check for vertical separation; our threat level is the minimum + // of vertical and horizontal threat levels + float closest_z = closest_approach_z(my_loc, my_vel, obstacle_loc, obstacle_vel, _warn_time_horizon + obstacle_age/1000); + if (obstacle.threat_level != MAV_COLLISION_THREAT_LEVEL_NONE) { + if (closest_z > _warn_distance_z) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + } else { + closest_z = closest_approach_z(my_loc, my_vel, obstacle_loc, obstacle_vel, _fail_time_horizon + obstacle_age/1000); + if (closest_z > _fail_distance_z) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_LOW; + } + } + } + + // If we haven't heard from a vehicle then assume it is no threat + if (obstacle_age > MAX_OBSTACLE_AGE_MS) { + obstacle.threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + } + + // could optimise this to not calculate a lot of this if threat + // level is none - but only *once the GCS has been informed*! + obstacle.closest_approach_xy = closest_xy; + obstacle.closest_approach_z = closest_z; + float current_distance = get_distance(my_loc, obstacle_loc); + obstacle.distance_to_closest_approach = current_distance - closest_xy; + Vector2f net_velocity_ne = Vector2f(my_vel[0] - obstacle_vel[0], my_vel[1] - obstacle_vel[1]); + obstacle.time_to_closest_approach = 0.0f; + if (!is_zero(obstacle.distance_to_closest_approach) && + ! is_zero(net_velocity_ne.length())) { + obstacle.time_to_closest_approach = obstacle.distance_to_closest_approach / net_velocity_ne.length(); + } +} + +MAV_COLLISION_THREAT_LEVEL AP_Avoidance::current_threat_level() const { + if (_obstacles == nullptr) { + return MAV_COLLISION_THREAT_LEVEL_NONE; + } + if (_current_most_serious_threat == -1) { + return MAV_COLLISION_THREAT_LEVEL_NONE; + } + return _obstacles[_current_most_serious_threat].threat_level; +} + +void AP_Avoidance::handle_threat_gcs_notify(AP_Avoidance::Obstacle *threat) +{ + if (threat == nullptr) { + return; + } + + uint32_t now = AP_HAL::millis(); + if (threat->threat_level == MAV_COLLISION_THREAT_LEVEL_NONE) { + // only send cleared messages for a few seconds: + if (_gcs_cleared_messages_first_sent == 0) { + _gcs_cleared_messages_first_sent = now; + } + if (now - _gcs_cleared_messages_first_sent > _gcs_cleared_messages_duration * 1000) { + return; + } + } else { + _gcs_cleared_messages_first_sent = 0; + } + if (now - threat->last_gcs_report_time > _gcs_notify_interval * 1000) { + GCS_MAVLINK::send_collision_all(*threat, mav_avoidance_action()); + threat->last_gcs_report_time = now; + } + +} + +bool AP_Avoidance::obstacle_is_more_serious_threat(const AP_Avoidance::Obstacle &obstacle) const +{ + if (_current_most_serious_threat == -1) { + // any threat is more of a threat than no threat + return true; + } + const AP_Avoidance::Obstacle ¤t = _obstacles[_current_most_serious_threat]; + if (obstacle.threat_level > current.threat_level) { + // threat_level is updated by update_threat_level + return true; + } + if (obstacle.threat_level == current.threat_level && + obstacle.time_to_closest_approach < current.time_to_closest_approach) { + return true; + } + return false; +} + +void AP_Avoidance::check_for_threats() +{ + Location my_loc; + if (!_ahrs.get_position(my_loc)) { + // if we don't know our own location we can't determine any threat level + return; + } + + Vector3f my_vel; + if (!_ahrs.get_velocity_NED(my_vel)) { + // assuming our own velocity to be zero here may cause us to + // fly into something. Better not to attempt to avoid in this + // case. + return; + } + + // we always check all obstacles to see if they are threats since it + // is most likely our own position and/or velocity have changed + // determine the current most-serious-threat + _current_most_serious_threat = -1; + for (uint8_t i=0; i<_obstacle_count; i++) { + + AP_Avoidance::Obstacle &obstacle = _obstacles[i]; + const uint32_t obstacle_age = AP_HAL::millis() - obstacle.timestamp_ms; + debug("i=%d src_id=%d timestamp=%u age=%d", i, obstacle.src_id, obstacle.timestamp_ms, obstacle_age); + + update_threat_level(my_loc, my_vel, obstacle); + debug(" threat-level=%d", obstacle.threat_level); + + // ignore any really old data: + if (obstacle_age > MAX_OBSTACLE_AGE_MS) { + // shrink list if this is the last entry: + if (i == _obstacle_count-1) { + _obstacle_count -= 1; + } + continue; + } + + if (obstacle_is_more_serious_threat(obstacle)) { + _current_most_serious_threat = i; + } + } + if (_current_most_serious_threat != -1) { + debug("Current most serious threat: %d level=%d", _current_most_serious_threat, _obstacles[_current_most_serious_threat].threat_level); + } +} + + +AP_Avoidance::Obstacle *AP_Avoidance::most_serious_threat() +{ + if (_current_most_serious_threat < 0) { + // we *really_ should not have been called! + return nullptr; + } + return &_obstacles[_current_most_serious_threat]; +} + + +void AP_Avoidance::update() +{ + if (!check_startup()) { + return; + } + + if (_adsb.enabled()) { + get_adsb_samples(); + } + + check_for_threats(); + + // notify GCS of most serious thread + handle_threat_gcs_notify(most_serious_threat()); + + // avoid object (if necessary) + handle_avoidance_local(most_serious_threat()); +} + +void AP_Avoidance::handle_avoidance_local(AP_Avoidance::Obstacle *threat) +{ + MAV_COLLISION_THREAT_LEVEL new_threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + MAV_COLLISION_ACTION action = MAV_COLLISION_ACTION_NONE; + + if (threat != nullptr) { + new_threat_level = threat->threat_level; + if (new_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) { + action = (MAV_COLLISION_ACTION)_fail_action.get(); + } + } + + uint32_t now = AP_HAL::millis(); + + if (new_threat_level != _threat_level) { + // transition to higher states immediately, recovery to lower states more slowly + if (((now - _last_state_change_ms) > AP_AVOIDANCE_STATE_RECOVERY_TIME_MS) || (new_threat_level > _threat_level)) { + // handle recovery from high threat level + if (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) { + handle_recovery(_fail_recovery); + _latest_action = MAV_COLLISION_ACTION_NONE; + } + + // update state + _last_state_change_ms = now; + _threat_level = new_threat_level; + } + } + + // handle ongoing threat by calling vehicle specific handler + if ((threat != nullptr) && (_threat_level == MAV_COLLISION_THREAT_LEVEL_HIGH) && (action > MAV_COLLISION_ACTION_REPORT)) { + _latest_action = handle_avoidance(threat, action); + } +} + + +void AP_Avoidance::handle_msg(const mavlink_message_t &msg) +{ + if (!check_startup()) { + // avoidance is not active / allocated + return; + } + + if (msg.msgid != MAVLINK_MSG_ID_GLOBAL_POSITION_INT) { + // we only take position from GLOBAL_POSITION_INT + return; + } + + if (msg.sysid == mavlink_system.sysid) { + // we do not obstruct ourselves.... + return; + } + + // inform AP_Avoidance we have a new player + mavlink_global_position_int_t packet; + mavlink_msg_global_position_int_decode(&msg, &packet); + Location loc; + loc.lat = packet.lat; + loc.lng = packet.lon; + loc.alt = packet.alt / 10; // mm -> cm + loc.flags.relative_alt = false; + Vector3f vel = Vector3f(packet.vx/100.0f, // cm to m + packet.vy/100.0f, + packet.vz/100.0f); + add_obstacle(AP_HAL::millis(), + MAV_COLLISION_SRC_ADSB, + msg.sysid, + loc, + vel); +} + +// wp_speeds in cm/s +bool AP_Avoidance::get_destination_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &newdest_neu, const float wp_speed_xy, const float wp_speed_z, const uint8_t _minimum_avoid_height) +{ + if (obstacle == nullptr) { + // why where we called?! + return false; + } + + Location my_abs_pos; + if (! _ahrs.get_position(my_abs_pos)) { + // we should not get to here! If we don't know our position + // we can't know if there are any threats, for starters! + return false; + } + + Vector3f my_pos_ned; + if (! _ahrs.get_relative_position_NED(my_pos_ned)) { + // we should not get to here! If we don't know our position + // we know if there are any threats, for starters! + return false; + } + + // if their velocity is moving around close to zero then flying + // perpendicular to that velocity may mean we do weird things. + // Instead, we will fly directly away from them: + if (obstacle->_velocity.length() < _low_velocity_threshold) { + const Vector2f delta_pos_xy = location_diff(obstacle->_location, my_abs_pos); + const float delta_pos_z = my_abs_pos.alt - obstacle->_location.alt; + Vector3f delta_pos_xyz = Vector3f(delta_pos_xy[0],delta_pos_xy[1],delta_pos_z); + // avoid divide by zero + if (delta_pos_xyz.is_zero()) { + return false; + } + delta_pos_xyz.normalize(); + newdest_neu[0] = my_pos_ned[0]*100 + delta_pos_xyz[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[1] = my_pos_ned[1]*100 + delta_pos_xyz[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[2] = -my_pos_ned[2]*100 + delta_pos_xyz[2] * wp_speed_z * AP_AVOIDANCE_ESCAPE_TIME_SEC; + if(newdest_neu[2] < _minimum_avoid_height*100) { + newdest_neu[0] = my_pos_ned[0]*100 + delta_pos_xy[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[1] = my_pos_ned[1]*100 + delta_pos_xy[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[2] = -my_pos_ned[2]*100; + } + return true; + } + + { + Vector3f perp_xyz = perpendicular_xyz(obstacle->_location, obstacle->_velocity, my_abs_pos); + perp_xyz.normalize(); + newdest_neu[0] = my_pos_ned[0]*100 + perp_xyz[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[1] = my_pos_ned[1]*100 + perp_xyz[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[2] = -my_pos_ned[2]*100 + perp_xyz[2] * wp_speed_z * AP_AVOIDANCE_ESCAPE_TIME_SEC; + } + + if (newdest_neu[2] < _minimum_avoid_height*100) { + // too close to the ground to do 3D avoidance + // GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "AVOID: PERPENDICULAR: 2D"); + Vector2f perp_xy = perpendicular_xy(obstacle->_location, obstacle->_velocity, my_abs_pos); + perp_xy.normalize(); + newdest_neu[0] = my_pos_ned[0]*100 + perp_xy[0] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[1] = my_pos_ned[1]*100 + perp_xy[1] * wp_speed_xy * AP_AVOIDANCE_ESCAPE_TIME_SEC; + newdest_neu[2] = -my_pos_ned[2]*100; + } + + return true; +} + +// get unit vector away from the nearest obstacle +bool AP_Avoidance::get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu) +{ + if (obstacle == nullptr) { + // why where we called?! + return false; + } + + Location my_abs_pos; + if (!_ahrs.get_position(my_abs_pos)) { + // we should not get to here! If we don't know our position + // we can't know if there are any threats, for starters! + return false; + } + + // if their velocity is moving around close to zero then flying + // perpendicular to that velocity may mean we do weird things. + // Instead, we will fly directly away from them + if (obstacle->_velocity.length() < _low_velocity_threshold) { + const Vector2f delta_pos_xy = location_diff(obstacle->_location, my_abs_pos); + const float delta_pos_z = my_abs_pos.alt - obstacle->_location.alt; + Vector3f delta_pos_xyz = Vector3f(delta_pos_xy.x, delta_pos_xy.y, delta_pos_z); + // avoid div by zero + if (delta_pos_xyz.is_zero()) { + return false; + } + delta_pos_xyz.normalize(); + vec_neu = delta_pos_xyz; + return true; + } else { + vec_neu = perpendicular_xyz(obstacle->_location, obstacle->_velocity, my_abs_pos); + // avoid div by zero + if (vec_neu.is_zero()) { + return false; + } + vec_neu.normalize(); + return true; + } +} + +// helper functions to calculate 3D destination to get us away from obstacle +// v1 is NED +Vector3f AP_Avoidance::perpendicular_xyz(const Location &p1, const Vector3f &v1, const Location &p2) +{ + Vector2f delta_p_2d = location_diff(p1, p2); + Vector3f delta_p_xyz = Vector3f(delta_p_2d[0],delta_p_2d[1],(p2.alt-p1.alt)/100.0f); //check this line + Vector3f v1_xyz = Vector3f(v1[0], v1[1], -v1[2]); + Vector3f ret = Vector3f::perpendicular(delta_p_xyz, v1_xyz); + return ret; +} + +// helper functions to calculate horizontal destination to get us away from obstacle +// v1 is NED +Vector2f AP_Avoidance::perpendicular_xy(const Location &p1, const Vector3f &v1, const Location &p2) +{ + Vector2f delta_p = location_diff(p1, p2); + Vector2f delta_p_n = Vector2f(delta_p[0],delta_p[1]); + Vector2f v1n(v1[0],v1[1]); + Vector2f ret_xy = Vector2f::perpendicular(delta_p_n, v1n); + return ret_xy; +} diff --git a/libraries/AP_Avoidance/AP_Avoidance.h b/libraries/AP_Avoidance/AP_Avoidance.h new file mode 100644 index 0000000000..c75819917a --- /dev/null +++ b/libraries/AP_Avoidance/AP_Avoidance.h @@ -0,0 +1,213 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#pragma once + +/* + This program 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 program 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 . + */ +/* + Situational awareness for ArduPilot + + - record a series of moving points in space which should be avoided + - produce messages for GCS if a collision risk is detected + + Peter Barker, May 2016 + + based on AP_ADSB, Tom Pittenger, November 2015 +*/ + +#include +#include + +// F_RCVRY possible parameter values +#define AP_AVOIDANCE_RECOVERY_NONE 0 +#define AP_AVOIDANCE_RECOVERY_RETURN_TO_PREVIOUS_FLIGHTMODE 1 + +#define AP_AVOIDANCE_STATE_RECOVERY_TIME_MS 2000 // we will not downgrade state any faster than this (2 seconds) + +#define AP_AVOIDANCE_ESCAPE_TIME_SEC 2 // vehicle runs from thread for 2 seconds + +class AP_Avoidance { + +public: + + // obstacle class to hold latest information for a known obstacles + class Obstacle { + public: + MAV_COLLISION_SRC src; + uint32_t src_id; + uint32_t timestamp_ms; + + Location _location; + Vector3f _velocity; + + // fields relating to this being a threat. These would be the reason to have a separate list of threats: + MAV_COLLISION_THREAT_LEVEL threat_level; + float closest_approach_xy; // metres + float closest_approach_z; // metres + float time_to_closest_approach; // seconds, 3D approach + float distance_to_closest_approach; // metres, 3D + uint32_t last_gcs_report_time; // millis + }; + + // constructor + AP_Avoidance(AP_AHRS &ahrs, class AP_ADSB &adsb); + + // add obstacle to the list of known obstacles + void add_obstacle(uint32_t obstacle_timestamp_ms, + const MAV_COLLISION_SRC src, + uint32_t src_id, + const Location &loc, + const Vector3f &vel_ned); + + void add_obstacle(uint32_t obstacle_timestamp_ms, + const MAV_COLLISION_SRC src, + uint32_t src_id, + const Location &loc, + float cog, + float hspeed, + float vspeed); + + // update should be called at 10hz or higher + void update(); + + // enable or disable avoidance + void enable() { _enabled = true; }; + void disable() { _enabled = false; }; + + // current overall threat level + MAV_COLLISION_THREAT_LEVEL current_threat_level() const; + + // add obstacles into the Avoidance system from MAVLink messages + void handle_msg(const mavlink_message_t &msg); + + // for holding parameters + static const struct AP_Param::GroupInfo var_info[]; + +protected: + + // top level avoidance handler. This calls the vehicle specific handle_avoidance with requested action + void handle_avoidance_local(AP_Avoidance::Obstacle *threat); + + // avoid the most significant threat. child classes must override this method + // function returns the action that it is actually taking + virtual MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) = 0; + + // recover after all threats have cleared. child classes must override this method + // recovery_action is from F_RCVRY parameter + virtual void handle_recovery(uint8_t recovery_action) = 0; + + uint32_t _last_state_change_ms = 0; + MAV_COLLISION_THREAT_LEVEL _threat_level = MAV_COLLISION_THREAT_LEVEL_NONE; + + // gcs notification + // specifies how long we should continue sending messages about a threat after it has cleared + static const uint8_t _gcs_cleared_messages_duration = 5; // seconds + uint32_t _gcs_cleared_messages_first_sent; + + void handle_threat_gcs_notify(AP_Avoidance::Obstacle *threat); + + AP_Avoidance::Obstacle *most_serious_threat(); + + // returns an entry from the MAV_COLLISION_ACTION representative + // of what the current avoidance handler is up to. + MAV_COLLISION_ACTION mav_avoidance_action() { return _latest_action; } + + // get target destination that best gets vehicle away from the nearest obstacle + bool get_destination_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &newdest_neu, const float wp_speed_xy, const float wp_speed_z, const uint8_t _minimum_avoid_height); + + // get unit vector away from the nearest obstacle + bool get_vector_perpendicular(const AP_Avoidance::Obstacle *obstacle, Vector3f &vec_neu); + + // helper functions to calculate destination to get us away from obstacle + // Note: v1 is NED + static Vector3f perpendicular_xyz(const Location &p1, const Vector3f &v1, const Location &p2); + static Vector2f perpendicular_xy(const Location &p1, const Vector3f &v1, const Location &p2); + + // reference to AHRS, so we can ask for our position, heading and speed + const AP_AHRS &_ahrs; + +private: + + // constants + const uint32_t MAX_OBSTACLE_AGE_MS = 5000; // obstacles that have not been heard from for 5 seconds are removed from the list + const static uint8_t _gcs_notify_interval = 1; // seconds + + // speed below which we will fly directly away from a threat + // rather than perpendicular to its velocity: + const uint8_t _low_velocity_threshold = 1; // meters/second + + // check to see if we are initialised (and possibly do initialisation) + bool check_startup(); + + // initialize _obstacle_list + void init(); + + // free _obstacle_list + void deinit(); + + // get unique id for adsb + uint32_t src_id_for_adsb_vehicle(AP_ADSB::adsb_vehicle_t vehicle) const; + + void check_for_threats(); + void update_threat_level(const Location &my_loc, + const Vector3f &my_vel, + AP_Avoidance::Obstacle &obstacle); + + // calls into the AP_ADSB library to retrieve vehicle data + void get_adsb_samples(); + + // returns true if the obstacle should be considered more of a + // threat than the current most serious threat + bool obstacle_is_more_serious_threat(const AP_Avoidance::Obstacle &obstacle) const; + + // internal variables + AP_Avoidance::Obstacle *_obstacles; + uint8_t _obstacles_allocated; + uint8_t _obstacle_count; + int8_t _current_most_serious_threat; + MAV_COLLISION_ACTION _latest_action = MAV_COLLISION_ACTION_NONE; + + // external references + class AP_ADSB &_adsb; + + // parameters + AP_Int8 _enabled; + AP_Int8 _obstacles_max; + + AP_Int8 _fail_action; + AP_Int8 _fail_recovery; + AP_Int8 _fail_time_horizon; + AP_Int16 _fail_distance_xy; + AP_Int16 _fail_distance_z; + + AP_Int8 _warn_action; + AP_Int8 _warn_time_horizon; + AP_Float _warn_distance_xy; + AP_Float _warn_distance_z; +}; + +float closest_distance_between_radial_and_point(const Vector2f &w, + const Vector2f &p); +float closest_approach_xy(const Location &my_loc, + const Vector3f &my_vel, + const Location &obstacle_loc, + const Vector3f &obstacle_vel, + uint8_t time_horizon); + +float closest_approach_z(const Location &my_loc, + const Vector3f &my_vel, + const Location &obstacle_loc, + const Vector3f &obstacle_vel, + uint8_t time_horizon);