/// set_spline_destination waypoint using position vector (distance from home in cm)
/// seg_type should be calculated by calling function based on the mission
public void set_spline_destination(Vector3 destination, bool stopped_at_start,
spline_segment_end_type seg_end_type, Vector3 next_destination)
{
// should be origin
Vector3 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
{
// my edit
origin = _origin;
// 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);
}
/// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// seg_type should be calculated by calling function based on the mission
void set_spline_origin_and_destination(Vector3 origin, Vector3 destination, bool stopped_at_start, spline_segment_end_type seg_end_type, Vector3 next_destination)
{
// mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint
bool prev_segment_exists = (_flags.reached_destination);// && ((hal.scheduler->millis() - _wp_last_update) < 1000));
// segment start types
// stop - vehicle is not moving at origin
// straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
// _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
// spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
// calculate spline velocity at origin
if (stopped_at_start || !prev_segment_exists)
{
// if vehicle is stopped at the origin, set origin velocity to 0.1 * distance vector from origin to destination
_spline_origin_vel = (destination - origin) * 0.1f;
_spline_time = 0.0f;
_spline_vel_scaler = 0.0f;
}
else
{
// look at previous segment to determine velocity at origin
if (_flags.segment_type == SegmentType.SEGMENT_STRAIGHT)
{
// previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
// before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
_spline_origin_vel = (_destination - _origin);
_spline_time = 0.0f; // To-Do: this should be set based on how much overrun there was from straight segment?
_spline_vel_scaler = 0.0f; // To-Do: this should be set based on speed at end of prev straight segment?
}
else
{
// previous segment is splined, vehicle will fly through origin
// we can use the previous segment's destination velocity as this segment's origin velocity
// Note: previous segment will leave destination velocity parallel to position difference vector
// from previous segment's origin to this segment's destination)
_spline_origin_vel = _spline_destination_vel;
if (_spline_time > 1.0f && _spline_time < 1.1f)
{ // To-Do: remove hard coded 1.1f
_spline_time -= 1.0f;
}
else
{
_spline_time = 0.0f;
}
_spline_vel_scaler = 0.0f;
}
}
// calculate spline velocity at destination
switch (seg_end_type)
{
case spline_segment_end_type.SEGMENT_END_STOP:
// if vehicle stops at the destination set destination velocity to 0.1 * distance vector from origin to destination
_spline_destination_vel = (destination - origin) * 0.1f;
_flags.fast_waypoint = false;
break;
case spline_segment_end_type.SEGMENT_END_STRAIGHT:
// if next segment is straight, vehicle's final velocity should face along the next segment's position
_spline_destination_vel = (next_destination - destination);
_flags.fast_waypoint = true;
break;
case spline_segment_end_type.SEGMENT_END_SPLINE:
// if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination
_spline_destination_vel = (next_destination - origin);
_flags.fast_waypoint = true;
break;
}
// code below ensures we don't get too much overshoot when the next segment is short
float vel_len = (float)((_spline_origin_vel + _spline_destination_vel).length());
float pos_len = (float)(destination - origin).length() * 4.0f;
if (vel_len > pos_len)
{
// if total start+stop velocity is more than twice position difference
// use a scaled down start and stop velocityscale the start and stop velocities down
float vel_scaling = pos_len / vel_len;
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel * vel_scaling, _spline_destination_vel * vel_scaling);
}
else
{
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
}
// initialise yaw heading to current heading
//_yaw = _ahrs->yaw_sensor;
// store origin and destination locations
_origin = origin;
_destination = destination;
// calculate slow down distance
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
// initialise intermediate point to the origin
//_pos_control.set_pos_target(origin);
_flags.reached_destination = false;
_flags.segment_type = SegmentType.SEGMENT_SPLINE;
}
/// set_spline_origin_and_destination - set origin and destination waypoints using position vectors (distance from home in cm)
/// seg_type should be calculated by calling function based on the mission
void set_spline_origin_and_destination(Vector3 origin, Vector3 destination, bool stopped_at_start,
spline_segment_end_type seg_end_type, Vector3 next_destination)
{
// mission is "active" if wpnav has been called recently and vehicle reached the previous waypoint
bool prev_segment_exists = (_flags.reached_destination);
// && ((hal.scheduler->millis() - _wp_last_update) < 1000));
// segment start types
// stop - vehicle is not moving at origin
// straight-fast - vehicle is moving, previous segment is straight. vehicle will fly straight through the waypoint before beginning it's spline path to the next wp
// _flag.segment_type holds whether prev segment is straight vs spline but we don't know if it has a delay
// spline-fast - vehicle is moving, previous segment is splined, vehicle will fly through waypoint but previous segment should have it flying in the correct direction (i.e. exactly parallel to position difference vector from previous segment's origin to this segment's destination)
// calculate spline velocity at origin
if (stopped_at_start || !prev_segment_exists)
{
// if vehicle is stopped at the origin, set origin velocity to 0.1 * distance vector from origin to destination
_spline_origin_vel = (destination - origin)*0.1f;
_spline_time = 0.0f;
_spline_vel_scaler = 0.0f;
}
else
{
// look at previous segment to determine velocity at origin
if (_flags.segment_type == SegmentType.SEGMENT_STRAIGHT)
{
// previous segment is straight, vehicle is moving so vehicle should fly straight through the origin
// before beginning it's spline path to the next waypoint. Note: we are using the previous segment's origin and destination
_spline_origin_vel = (_destination - _origin);
_spline_time = 0.0f;
// To-Do: this should be set based on how much overrun there was from straight segment?
_spline_vel_scaler = 0.0f;
// To-Do: this should be set based on speed at end of prev straight segment?
}
else
{
// previous segment is splined, vehicle will fly through origin
// we can use the previous segment's destination velocity as this segment's origin velocity
// Note: previous segment will leave destination velocity parallel to position difference vector
// from previous segment's origin to this segment's destination)
_spline_origin_vel = _spline_destination_vel;
if (_spline_time > 1.0f && _spline_time < 1.1f)
{
// To-Do: remove hard coded 1.1f
_spline_time -= 1.0f;
}
else
{
_spline_time = 0.0f;
}
_spline_vel_scaler = 0.0f;
}
}
// calculate spline velocity at destination
switch (seg_end_type)
{
case spline_segment_end_type.SEGMENT_END_STOP:
// if vehicle stops at the destination set destination velocity to 0.1 * distance vector from origin to destination
_spline_destination_vel = (destination - origin)*0.1f;
_flags.fast_waypoint = false;
break;
case spline_segment_end_type.SEGMENT_END_STRAIGHT:
// if next segment is straight, vehicle's final velocity should face along the next segment's position
_spline_destination_vel = (next_destination - destination);
_flags.fast_waypoint = true;
break;
case spline_segment_end_type.SEGMENT_END_SPLINE:
// if next segment is splined, vehicle's final velocity should face parallel to the line from the origin to the next destination
_spline_destination_vel = (next_destination - origin);
_flags.fast_waypoint = true;
break;
}
// code below ensures we don't get too much overshoot when the next segment is short
float vel_len = (float) ((_spline_origin_vel + _spline_destination_vel).length());
float pos_len = (float) (destination - origin).length()*4.0f;
if (vel_len > pos_len)
{
// if total start+stop velocity is more than twice position difference
// use a scaled down start and stop velocityscale the start and stop velocities down
float vel_scaling = pos_len/vel_len;
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel*vel_scaling,
_spline_destination_vel*vel_scaling);
}
else
{
// update spline calculator
update_spline_solution(origin, destination, _spline_origin_vel, _spline_destination_vel);
}
// initialise yaw heading to current heading
//_yaw = _ahrs->yaw_sensor;
// store origin and destination locations
_origin = origin;
_destination = destination;
// calculate slow down distance
calc_slow_down_distance(_wp_speed_cms, _wp_accel_cms);
// initialise intermediate point to the origin
//_pos_control.set_pos_target(origin);
_flags.reached_destination = false;
_flags.segment_type = SegmentType.SEGMENT_SPLINE;
}
/// set_spline_destination waypoint using position vector (distance from home in cm)
/// seg_type should be calculated by calling function based on the mission
public void set_spline_destination(Vector3 destination, bool stopped_at_start,
spline_segment_end_type seg_end_type, Vector3 next_destination)
{
// should be origin
Vector3 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
{
// my edit
origin = _origin;
// 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);
}
