diff --git a/libraries/AR_WPNav/AR_WPNav.cpp b/libraries/AR_WPNav/AR_WPNav.cpp index 368baa5371..e6f21d5fd3 100644 --- a/libraries/AR_WPNav/AR_WPNav.cpp +++ b/libraries/AR_WPNav/AR_WPNav.cpp @@ -145,65 +145,13 @@ void AR_WPNav::update(float dt) } _last_update_ms = AP_HAL::millis(); - // run path planning around obstacles - bool stop_vehicle = false; - - // true if OA has been recently active; - bool _oa_was_active = _oa_active; - - AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton(); - if (oa != nullptr) { - AP_OAPathPlanner::OAPathPlannerUsed path_planner_used; - const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin, _destination, _oa_origin, _oa_destination, path_planner_used); - switch (oa_retstate) { - case AP_OAPathPlanner::OA_NOT_REQUIRED: - _oa_active = false; - break; - case AP_OAPathPlanner::OA_PROCESSING: - case AP_OAPathPlanner::OA_ERROR: - // during processing or in case of error, slow vehicle to a stop - stop_vehicle = true; - _oa_active = false; - break; - case AP_OAPathPlanner::OA_SUCCESS: - _oa_active = true; - break; - } - } - if (!_oa_active) { - _oa_origin = _origin; - _oa_destination = _destination; - } - update_distance_and_bearing_to_destination(); - // if object avoidance is active check if vehicle should pivot towards destination - if (_oa_active) { - _pivot.check_activation(_oa_wp_bearing_cd * 0.01); - } - - // check if vehicle is in recovering state after switching off OA - if (!_oa_active && _oa_was_active) { - if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET) { - //reset wp origin to vehicles current location - _origin = current_loc; - } - } - // advance target along path unless vehicle is pivoting if (!_pivot.active()) { advance_wp_target_along_track(current_loc, dt); } - // handle stopping vehicle if avoidance has failed - if (stop_vehicle) { - // decelerate to speed to zero and set turn rate to zero - _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); - _desired_lat_accel = 0.0f; - _desired_turn_rate_rads = 0.0f; - return; - } - // update_steering_and_speed update_steering_and_speed(current_loc, dt); } @@ -221,8 +169,8 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location _scurve_prev_leg = _scurve_this_leg; // initialise some variables - _oa_origin = _origin = _destination; - _oa_destination = _destination = destination; + _origin = _destination; + _destination = destination; _orig_and_dest_valid = true; _reached_destination = false; @@ -232,7 +180,7 @@ bool AR_WPNav::set_desired_location(const struct Location& destination, Location // or journey to previous waypoint was interrupted or navigation has just started if (!_fast_waypoint) { _pivot.deactivate(); - _pivot.check_activation(_oa_wp_bearing_cd * 0.01); + _pivot.check_activation(oa_wp_bearing_cd() * 0.01); } // convert origin and destination to offset from EKF origin @@ -434,23 +382,10 @@ void AR_WPNav::update_distance_and_bearing_to_destination() if (!_orig_and_dest_valid || !AP::ahrs().get_location(current_loc)) { _distance_to_destination = 0.0f; _wp_bearing_cd = 0.0f; - - // update OA adjusted values - _oa_distance_to_destination = 0.0f; - _oa_wp_bearing_cd = 0.0f; return; } _distance_to_destination = current_loc.get_distance(_destination); _wp_bearing_cd = current_loc.get_bearing_to(_destination); - - // update OA adjusted values - if (_oa_active) { - _oa_distance_to_destination = current_loc.get_distance(_oa_destination); - _oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination); - } else { - _oa_distance_to_destination = _distance_to_destination; - _oa_wp_bearing_cd = _wp_bearing_cd; - } } // calculate steering and speed to drive along line from origin to destination waypoint @@ -459,7 +394,7 @@ void AR_WPNav::update_steering_and_speed(const Location ¤t_loc, float dt) // handle pivot turns if (_pivot.active()) { _cross_track_error = calc_crosstrack_error(current_loc); - _desired_heading_cd = _reversed ? wrap_360_cd(_oa_wp_bearing_cd + 18000) : _oa_wp_bearing_cd;; + _desired_heading_cd = _reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd(); _desired_turn_rate_rads = _pivot.get_turn_rate_rads(_desired_heading_cd * 0.01, dt); _desired_lat_accel = 0.0f; @@ -495,26 +430,30 @@ void AR_WPNav::apply_speed_min(float &desired_speed) const desired_speed = MAX(desired_speed, _speed_min); } -// calculate the crosstrack error (does not rely on L1 controller) +// calculate the crosstrack error float AR_WPNav::calc_crosstrack_error(const Location& current_loc) const { if (!_orig_and_dest_valid) { return 0.0f; } - // calculate the NE position of destination relative to origin - Vector2f dest_from_origin = _oa_origin.get_distance_NE(_oa_destination); + // get object avoidance adjusted origin and destination + const Location &orig = get_oa_origin(); + const Location &dest = get_oa_destination(); - // return distance to origin if length of track is very small + // calculate the NE position of destination relative to origin + Vector2f dest_from_origin = orig.get_distance_NE(dest); + + // return distance to destination if length of track is very small if (dest_from_origin.length() < 1.0e-6f) { - return current_loc.get_distance_NE(_oa_destination).length(); + return current_loc.get_distance_NE(dest).length(); } // convert to a vector indicating direction only dest_from_origin.normalize(); // calculate the NE position of the vehicle relative to origin - const Vector2f veh_from_origin = _oa_origin.get_distance_NE(current_loc); + const Vector2f veh_from_origin = orig.get_distance_NE(current_loc); // calculate distance to target track, for reporting return fabsf(veh_from_origin % dest_from_origin); diff --git a/libraries/AR_WPNav/AR_WPNav.h b/libraries/AR_WPNav/AR_WPNav.h index c06408da1c..91e4eeef3b 100644 --- a/libraries/AR_WPNav/AR_WPNav.h +++ b/libraries/AR_WPNav/AR_WPNav.h @@ -7,8 +7,6 @@ #include #include "AR_PivotTurn.h" -const float AR_WPNAV_HEADING_UNKNOWN = 99999.0f; // used to indicate to set_desired_location method that next leg's heading is unknown - class AR_WPNav { public: @@ -23,7 +21,7 @@ public: void init(float speed_max, float accel_max, float lat_accel_max, float jerk_max); // update navigation - void update(float dt); + virtual void update(float dt); // return desired speed float get_desired_speed() const { return _desired_speed; } @@ -47,7 +45,7 @@ public: // set desired location and (optionally) next_destination // next_destination should be provided if known to allow smooth cornering - bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; + virtual bool set_desired_location(const Location &destination, Location next_destination = Location()) WARN_IF_UNUSED; // set desired location to a reasonable stopping point, return true on success bool set_desired_location_to_stopping_location() WARN_IF_UNUSED; @@ -57,7 +55,7 @@ public: bool set_desired_location_NED(const Vector3f &destination, const Vector3f &next_destination) WARN_IF_UNUSED; // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck - bool reached_destination() const { return _reached_destination; } + virtual bool reached_destination() const { return _reached_destination; } // return distance (in meters) to destination float get_distance_to_destination() const { return _distance_to_destination; } @@ -66,18 +64,21 @@ public: bool is_destination_valid() const { return _orig_and_dest_valid; } // get current destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) - const Location &get_destination() { return _destination; } - - // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) - const Location &get_oa_destination() { return _oa_destination; } + const Location &get_destination() const { return _destination; } // return heading (in centi-degrees) and cross track error (in meters) for reporting to ground station (NAV_CONTROLLER_OUTPUT message) float wp_bearing_cd() const { return _wp_bearing_cd; } float nav_bearing_cd() const { return _desired_heading_cd; } float crosstrack_error() const { return _cross_track_error; } + // get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + virtual const Location &get_oa_origin() const { return _origin; } + + // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + virtual const Location &get_oa_destination() const { return get_destination(); } + // return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) - float oa_wp_bearing_cd() const { return _oa_wp_bearing_cd; } + virtual float oa_wp_bearing_cd() const { return wp_bearing_cd(); } // settor to allow vehicle code to provide turn related param values to this library (should be updated regularly) void set_turn_params(float turn_radius, bool pivot_possible); @@ -94,7 +95,7 @@ public: // parameter var table static const struct AP_Param::GroupInfo var_info[]; -private: +protected: // true if update has been called recently bool is_active() const; @@ -155,11 +156,4 @@ private: // variables for reporting float _distance_to_destination; // distance from vehicle to final destination in meters bool _reached_destination; // true once the vehicle has reached the destination - - // object avoidance variables - bool _oa_active; // true if we should use alternative destination to avoid obstacles - Location _oa_origin; // intermediate origin during avoidance - Location _oa_destination; // intermediate destination during avoidance - float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters - float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees }; diff --git a/libraries/AR_WPNav/AR_WPNav_OA.cpp b/libraries/AR_WPNav/AR_WPNav_OA.cpp new file mode 100644 index 0000000000..68d51e3c51 --- /dev/null +++ b/libraries/AR_WPNav/AR_WPNav_OA.cpp @@ -0,0 +1,228 @@ +/* + 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 . + */ + +#include +#include +#include "AR_WPNav_OA.h" +#include + +extern const AP_HAL::HAL& hal; + +// update navigation +void AR_WPNav_OA::update(float dt) +{ + // exit immediately if no current location, origin or destination + Location current_loc; + float speed; + if (!hal.util->get_soft_armed() || !is_destination_valid() || !AP::ahrs().get_location(current_loc) || !_atc.get_forward_speed(speed)) { + _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_lat_accel = 0.0f; + _desired_turn_rate_rads = 0.0f; + _oa_active = false; + return; + } + + // run path planning around obstacles + bool stop_vehicle = false; + + // backup _origin and _destination when not doing oa + if (!_oa_active) { + _origin_oabak = _origin; + _destination_oabak = _destination; + } + + AP_OAPathPlanner *oa = AP_OAPathPlanner::get_singleton(); + if (oa != nullptr) { + Location oa_origin_new, oa_destination_new; + AP_OAPathPlanner::OAPathPlannerUsed path_planner_used; + const AP_OAPathPlanner::OA_RetState oa_retstate = oa->mission_avoidance(current_loc, _origin_oabak, _destination_oabak, oa_origin_new, oa_destination_new, path_planner_used); + switch (oa_retstate) { + + case AP_OAPathPlanner::OA_NOT_REQUIRED: + if (_oa_active) { + // object avoidance has become inactive so reset target to original destination + if (!AR_WPNav::set_desired_location(_destination_oabak)) { + // this should never happen because we should have an EKF origin and the destination must be valid + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; + } + _oa_active = false; + // ToDo: handle "if (oa->get_options() & AP_OAPathPlanner::OA_OPTION_WP_RESET)" + } + break; + + case AP_OAPathPlanner::OA_PROCESSING: + case AP_OAPathPlanner::OA_ERROR: + // during processing or in case of error, slow vehicle to a stop + stop_vehicle = true; + _oa_active = false; + break; + + case AP_OAPathPlanner::OA_SUCCESS: + // handling of returned destination depends upon path planner used + switch (path_planner_used) { + + case AP_OAPathPlanner::OAPathPlannerUsed::None: + case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerVertical: + // this should never happen. this means the path planner has returned success but has returned an invalid planner + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + _oa_active = false; + stop_vehicle = true; + return; + + case AP_OAPathPlanner::OAPathPlannerUsed::Dijkstras: + // Dijkstra's. Action is only needed if path planner has just became active or the target destination's lat or lon has changed + if (!_oa_active || !oa_destination_new.same_latlon_as(_oa_destination)) { + if (AR_WPNav::set_desired_location(oa_destination_new)) { + // if new target set successfully, update oa state and destination + _oa_active = true; + _oa_origin = oa_origin_new; + _oa_destination = oa_destination_new; + } else { + // this should never happen + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; + } + } + break; + + case AP_OAPathPlanner::OAPathPlannerUsed::BendyRulerHorizontal: { + // calculate final destination as an offset from EKF origin in NE + Vector2f dest_NE_ftype; + if (oa_destination_new.get_vector_xy_from_origin_NE(dest_NE_ftype)) { + // initialise position controller if required + if (!_oa_active) { + _pos_control.init(); + _oa_active = true; + } + _oa_origin = oa_origin_new; + _oa_destination = oa_destination_new; + + // check for pivot turn + update_oa_distance_and_bearing_to_destination(); + _pivot.check_activation((_reversed ? wrap_360_cd(oa_wp_bearing_cd() + 18000) : oa_wp_bearing_cd()) * 0.01); + + if (!_pivot.active()) { + // pass the desired position directly to the position controller + dest_NE_ftype *= 0.01; // convert to meters + Vector2p dest_NE(dest_NE_ftype.x, dest_NE_ftype.y); + _pos_control.input_pos_target(dest_NE, dt); + + // run position controllers + _pos_control.update(dt); + } + + update_distance_and_bearing_to_destination(); + + // update_steering_and_speed + update_steering_and_speed(current_loc, dt); + + // return without calling parent AC_WPNav + return; + } else { + // this should never happen because we can only get here if we have an EKF origin + INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control); + stop_vehicle = true; + } + } + break; + + } // switch (path_planner_used) { + } // switch (oa_retstate) { + } // if (oa != nullptr) { + + update_oa_distance_and_bearing_to_destination(); + + // handle stopping vehicle if avoidance has failed + if (stop_vehicle) { + // decelerate to speed to zero and set turn rate to zero + _desired_speed_limited = _atc.get_desired_speed_accel_limited(0.0f, dt); + _desired_lat_accel = 0.0f; + _desired_turn_rate_rads = 0.0f; + return; + } + + // call parent update + AR_WPNav::update(dt); +} + +// set desired location and (optionally) next_destination +// next_destination should be provided if known to allow smooth cornering +bool AR_WPNav_OA::set_desired_location(const struct Location& destination, Location next_destination) +{ + const bool ret = AR_WPNav::set_desired_location(destination, next_destination); + + if (ret) { + // disable object avoidance, it will be re-enabled (if necessary) on next update + _oa_active = false; + } + + return ret; +} + +// true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck +bool AR_WPNav_OA::reached_destination() const +{ + // object avoidance should always be deactivated before reaching final destination + if (_oa_active) { + return false; + } + + return AR_WPNav::reached_destination(); +} + +// get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) +const Location &AR_WPNav_OA::get_oa_origin() const +{ + if (_oa_active) { + return _oa_origin; + } + + return _origin; +} + +// get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) +const Location &AR_WPNav_OA::get_oa_destination() const +{ + if (_oa_active) { + return _oa_destination; + } + + return AR_WPNav::get_oa_destination(); +} + +// return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) +float AR_WPNav_OA::oa_wp_bearing_cd() const +{ + if (_oa_active) { + return _oa_wp_bearing_cd; + } + + return AR_WPNav::oa_wp_bearing_cd(); +} + +// update distance from vehicle's current position to destination +void AR_WPNav_OA::update_oa_distance_and_bearing_to_destination() +{ + // update OA adjusted values + Location current_loc; + if (_oa_active && AP::ahrs().get_location(current_loc)) { + _oa_distance_to_destination = current_loc.get_distance(_oa_destination); + _oa_wp_bearing_cd = current_loc.get_bearing_to(_oa_destination); + } else { + _oa_distance_to_destination = AR_WPNav::get_distance_to_destination(); + _oa_wp_bearing_cd = AR_WPNav::wp_bearing_cd(); + } +} diff --git a/libraries/AR_WPNav/AR_WPNav_OA.h b/libraries/AR_WPNav/AR_WPNav_OA.h new file mode 100644 index 0000000000..195ed3028f --- /dev/null +++ b/libraries/AR_WPNav/AR_WPNav_OA.h @@ -0,0 +1,43 @@ +#pragma once + +#include "AR_WPNav.h" + +class AR_WPNav_OA : public AR_WPNav { +public: + + // re-use parent's constructor + using AR_WPNav::AR_WPNav; + + // update navigation + void update(float dt) override; + + // set desired location and (optionally) next_destination + // next_destination should be provided if known to allow smooth cornering + bool set_desired_location(const Location &destination, Location next_destination = Location()) override WARN_IF_UNUSED; + + // true if vehicle has reached desired location. defaults to true because this is normally used by missions and we do not want the mission to become stuck + bool reached_destination() const override; + + // get object avoidance adjusted origin. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + const Location &get_oa_origin() const override; + + // get object avoidance adjusted destination. Note: this is not guaranteed to be valid (i.e. _orig_and_dest_valid is not checked) + const Location &get_oa_destination() const override; + + // return the heading (in centi-degrees) to the next waypoint accounting for OA, (used by sailboats) + float oa_wp_bearing_cd() const override; + +private: + + // update distance and bearing from vehicle's current position to destination + void update_oa_distance_and_bearing_to_destination(); + + // object avoidance variables + bool _oa_active; // true if we should use alternative destination to avoid obstacles + Location _origin_oabak; // backup of _origin so it can be restored when oa completes + Location _destination_oabak; // backup of _desitnation so it can be restored when oa completes + Location _oa_origin; // intermediate origin during avoidance + Location _oa_destination; // intermediate destination during avoidance + float _oa_distance_to_destination; // OA (object avoidance) distance from vehicle to _oa_destination in meters + float _oa_wp_bearing_cd; // OA adjusted heading to _oa_destination in centi-degrees +};