diff --git a/libraries/AC_WPNav/AC_WPNav.cpp b/libraries/AC_WPNav/AC_WPNav.cpp index 59465c7a71..960d80c574 100644 --- a/libraries/AC_WPNav/AC_WPNav.cpp +++ b/libraries/AC_WPNav/AC_WPNav.cpp @@ -822,11 +822,43 @@ void AC_WPNav::calculate_wp_leash_length() /// spline methods /// -/// set_spline_destination waypoint using position vector (distance from home in cm) +/// set_spline_destination waypoint using location class +/// returns false if conversion from location to vector from ekf origin cannot be calculated /// stopped_at_start should be set to true if vehicle is stopped at the origin /// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type /// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE -void AC_WPNav::set_spline_destination(const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination) +bool AC_WPNav::set_spline_destination(const Location_Class& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination) +{ + // convert destination location to vector + Vector3f dest_neu; + bool dest_terr_alt; + if (!get_vector_NEU(destination, dest_neu, dest_terr_alt)) { + return false; + } + + // make altitude frames consistent + if (!next_destination.change_alt_frame(destination.get_alt_frame())) { + return false; + } + + // convert next destination to vector + Vector3f next_dest_neu; + bool next_dest_terr_alt; + if (!get_vector_NEU(next_destination, next_dest_neu, next_dest_terr_alt)) { + return false; + } + + // set target as vector from EKF origin + return set_spline_destination(dest_neu, dest_terr_alt, stopped_at_start, seg_end_type, next_dest_neu); +} + +/// set_spline_destination waypoint using position vector (distance from home in cm) +/// returns false if conversion from location to vector from ekf origin cannot be calculated +/// terrain_alt should be true if destination.z is a desired altitudes above terrain (false if its desired altitudes above ekf origin) +/// stopped_at_start should be set to true if vehicle is stopped at the origin +/// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type +/// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE +bool AC_WPNav::set_spline_destination(const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination) { Vector3f origin; @@ -839,13 +871,23 @@ void AC_WPNav::set_spline_destination(const Vector3f& destination, bool stopped_ _pos_control.get_stopping_point_z(origin); } + // convert origin to alt-above-terrain + if (terrain_alt) { + float origin_terr_offset; + if (!get_terrain_offset(origin, origin_terr_offset)) { + return false; + } + origin.z -= origin_terr_offset; + } + // set origin and destination - set_spline_origin_and_destination(origin, destination, stopped_at_start, seg_end_type, next_destination); + return set_spline_origin_and_destination(origin, destination, terrain_alt, stopped_at_start, seg_end_type, next_destination); } /// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) +/// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if desired altitudes above ekf origin) /// seg_type should be calculated by calling function based on the mission -void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination) +bool AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination) { // mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint bool prev_segment_exists = (_flags.reached_destination && ((AP_HAL::millis() - _wp_last_update) < 1000)); @@ -933,26 +975,38 @@ void AC_WPNav::set_spline_origin_and_destination(const Vector3f& origin, const V // store origin and destination locations _origin = origin; _destination = destination; + _terrain_alt = terrain_alt; // calculate slow down distance calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms); + // get origin's alt-above-terrain + float origin_terr_offset = 0.0f; + if (terrain_alt) { + if (!get_terrain_offset(origin, origin_terr_offset)) { + return false; + } + } + // initialise intermediate point to the origin - _pos_control.set_pos_target(origin); + _pos_control.set_pos_target(origin + Vector3f(0,0,origin_terr_offset)); _flags.reached_destination = false; _flags.segment_type = SEGMENT_SPLINE; _flags.new_wp_destination = true; // flag new waypoint so we can freeze the pos controller's feed forward and smooth the transition + + return true; } /// update_spline - update spline controller -void AC_WPNav::update_spline() +bool AC_WPNav::update_spline() { // exit immediately if this is not a spline segment if (_flags.segment_type != SEGMENT_SPLINE) { - return; + return false; } float dt = _pos_control.time_since_last_xy_update(); + bool ret = true; // run at poscontrol update rate if (dt >= _pos_control.get_dt_xy()) { @@ -962,7 +1016,10 @@ void AC_WPNav::update_spline() } // advance the target if necessary - advance_spline_target_along_track(dt); + if (!advance_spline_target_along_track(dt)) { + // To-Do: handle failure to advance along track (due to missing terrain data) + ret = false; + } // freeze feedforwards during known discontinuities // TODO: why always consider Z axis discontinuous? @@ -977,6 +1034,7 @@ void AC_WPNav::update_spline() _wp_last_update = AP_HAL::millis(); } + return ret; } /// update_spline_solution - recalculates hermite_spline_solution grid @@ -990,7 +1048,7 @@ void AC_WPNav::update_spline_solution(const Vector3f& origin, const Vector3f& de } /// advance_spline_target_along_track - move target location along track from origin to destination -void AC_WPNav::advance_spline_target_along_track(float dt) +bool AC_WPNav::advance_spline_target_along_track(float dt) { if (!_flags.reached_destination) { Vector3f target_pos, target_vel; @@ -1003,7 +1061,16 @@ void AC_WPNav::advance_spline_target_along_track(float dt) // get current location Vector3f curr_pos = _inav.get_position(); + + // get terrain altitude offset for origin and current position (i.e. change in terrain altitude from a position vs ekf origin) + float curr_terr_offset = 0.0f; + if (_terrain_alt && !get_terrain_offset(curr_pos, curr_terr_offset)) { + return false; + } + + // calculate position error Vector3f track_error = curr_pos - target_pos; + track_error.z -= curr_terr_offset; // calculate the horizontal error float track_error_xy = pythagorous2(track_error.x, track_error.y); @@ -1051,6 +1118,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) } // update target position + target_pos.z += curr_terr_offset; _pos_control.set_pos_target(target_pos); // update the yaw @@ -1065,6 +1133,7 @@ void AC_WPNav::advance_spline_target_along_track(float dt) _flags.reached_destination = true; } } + return true; } // calc_spline_pos_vel_accel - calculates target position, velocity and acceleration for the given "spline_time" diff --git a/libraries/AC_WPNav/AC_WPNav.h b/libraries/AC_WPNav/AC_WPNav.h index 65a4703f2c..d7173ed9b4 100644 --- a/libraries/AC_WPNav/AC_WPNav.h +++ b/libraries/AC_WPNav/AC_WPNav.h @@ -196,23 +196,35 @@ public: // get_yaw - returns target yaw in centi-degrees (used for wp and spline navigation) float get_yaw() const { return _yaw; } - /// set_spline_destination waypoint using position vector (distance from home in cm) + /// set_spline_destination waypoint using location class + /// returns false if conversion from location to vector from ekf origin cannot be calculated + /// terrain_alt should be true if destination.z is a desired altitude above terrain (false if its desired altitude above ekf origin) /// stopped_at_start should be set to true if vehicle is stopped at the origin /// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type /// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE - void set_spline_destination(const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination); + bool set_spline_destination(const Location_Class& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Location_Class next_destination); + + /// set_spline_destination waypoint using position vector (distance from home in cm) + /// returns false if conversion from location to vector from ekf origin cannot be calculated + /// terrain_alt should be true if destination.z is a desired altitudes above terrain (false if its desired altitudes above ekf origin) + /// stopped_at_start should be set to true if vehicle is stopped at the origin + /// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type + /// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE + /// next_destination.z must be in the same "frame" as destination.z (i.e. if destination is a alt-above-terrain, next_destination should be too) + bool set_spline_destination(const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination); /// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm) + /// terrain_alt should be true if origin.z and destination.z are desired altitudes above terrain (false if desired altitudes above ekf origin) /// stopped_at_start should be set to true if vehicle is stopped at the origin /// seg_end_type should be set to stopped, straight or spline depending upon the next segment's type /// next_destination should be set to the next segment's destination if the seg_end_type is SEGMENT_END_STRAIGHT or SEGMENT_END_SPLINE - void set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination); + bool set_spline_origin_and_destination(const Vector3f& origin, const Vector3f& destination, bool terrain_alt, bool stopped_at_start, spline_segment_end_type seg_end_type, const Vector3f& next_destination); /// reached_spline_destination - true when we have come within RADIUS cm of the waypoint bool reached_spline_destination() const { return _flags.reached_destination; } /// update_spline - update spline controller - void update_spline(); + bool update_spline(); /// /// shared methods @@ -274,7 +286,8 @@ protected: void update_spline_solution(const Vector3f& origin, const Vector3f& dest, const Vector3f& origin_vel, const Vector3f& dest_vel); /// advance_spline_target_along_track - move target location along track from origin to destination - void advance_spline_target_along_track(float dt); + /// returns false if it is unable to advance (most likely because of missing terrain data) + bool advance_spline_target_along_track(float dt); /// calc_spline_pos_vel - update position and velocity from given spline time /// relies on update_spline_solution being called since the previous