/// 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);
}
