/// 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; // if waypoint controller is active and copter has reached the previous waypoint use current pos target as the origin if ((AP_HAL::millis() - _wp_last_update) < 1000) { origin = _pos_control.get_pos_target(); }else{ // otherwise calculate origin from the current position and velocity _pos_control.get_stopping_point_xy(origin); _pos_control.get_stopping_point_z(origin); } // convert origin to alt-above-terrain if (terrain_alt) { float terr_offset; if (!get_terrain_offset(terr_offset)) { return false; } origin.z -= terr_offset; } // set origin and destination return set_spline_origin_and_destination(origin, destination, terrain_alt, stopped_at_start, seg_end_type, next_destination); }
/// set_spline_destination waypoint using position vector (distance from home in cm) /// 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) { Vector3f origin; // if waypoint controller is active and copter has reached the previous waypoint use it for the origin if( _flags.reached_destination && ((hal.scheduler->millis() - _wp_last_update) < 1000) ) { origin = _destination; }else{ // otherwise calculate origin from the current position and velocity _pos_control.get_stopping_point_xy(origin); _pos_control.get_stopping_point_z(origin); } // set origin and destination set_spline_origin_and_destination(origin, destination, stopped_at_start, seg_end_type, next_destination); }