public Status ApproachInit(
string ship_name,
string dock_tag = null,
string control_node_tag = null,
double distance = 50d)
{
approach_dock_tag_ = dock_tag;
approach_control_node_tag_ = control_node_tag;
approach_distance_max_ = distance;
IEnumerable <Vessel> vessels = SpaceCenter.Vessels.Where(v => v.Name == ship_name);
if (vessels.Count() == 0)
{
return(Status.FAIL);
}
approach_vessel_ = vessels.First();
SpaceCenter.TargetVessel = approach_vessel_;
approach_tar_pos_stream_ = Conn.AddStream(() => approach_vessel_.Position(OrbitBodyFrame));
approach_tar_vel_stream_ = Conn.AddStream(() => approach_vessel_.Velocity(OrbitBodyFrame));
approach_tar_dir_stream_ = null;
Vector3d tar_pos = new Vector3d(approach_tar_pos_stream_.Get());
Vector3d tar_vel = new Vector3d(approach_tar_vel_stream_.Get());
double tar_distance = (tar_pos - State.Vessel.Position).Length();
double tar_rel_vel = (tar_vel - State.Vessel.Velocity).Length();
if (tar_distance < 1000d && tar_rel_vel < Math.Max(10d, Math.Sqrt(tar_distance)))
{
approach_stage_ = ApproachStage.DOCKING;
}
else
{
approach_stage_ = ApproachStage.ENGINE;
}
if (string.IsNullOrEmpty(dock_tag))
{
approach_target_stage_ = ApproachTargetStage.PORT;
}
else
{
approach_target_stage_ = ApproachTargetStage.SHIP;
}
return(Status.SUCCESS);
}
public Status Approach(out double distance)
{
if (ApproachGetDstInfo(
out Vector3d dst_pos, out Vector3d dst_vel,
out Vector3d dst_acc, out Vector3d dst_dir) == Status.FAIL)
{
distance = -1d;
return(Status.FAIL);
}
ApproachGetTarInfo(
dst_pos, dst_dir,
out Vector3d tar_vec, out Vector3d tar_vel_on_dir, out Vector3d tar_vec_lateral, out Vector3d tar_vec_dir,
out double tar_distance_lateral, out double tar_distance_facing, out double tar_distance);
distance = tar_distance;
if (ApproachFindDockingPort(tar_distance) == Status.FAIL)
{
return(Status.FAIL);
}
Vector3d rel_vel_vec = State.Vessel.Velocity - dst_vel;
double rel_vel = rel_vel_vec.Length();
switch (approach_stage_)
{
case ApproachStage.ENGINE:
{
ApproachCalculateThrust(
dst_vel, dst_acc, tar_vec_dir, tar_distance, rel_vel_vec,
out Vector3d tar_acc_vec, out Vector3d tar_dir, out double route_vel, out double tar_vel_err_mag,
out double tar_thrust_acc, out Vector3d tar_thrust_acc_vec, out double throttle);
if (route_vel < 50d && tar_vel_err_mag < 50d)
{
Command.SetTargetDirection(-rel_vel_vec.Norm());
approach_stage_ = ApproachStage.MIX;
}
else
{
Command.SetTargetDirection(tar_dir);
}
Command.SetThrottle(throttle);
}
break;
case ApproachStage.MIX:
{
ApproachCalculateThrust(
dst_vel, dst_acc, tar_vec_dir, tar_distance, rel_vel_vec,
out Vector3d tar_acc_vec, out Vector3d tar_dir, out double route_vel, out double tar_vel_err_mag,
out double tar_thrust_acc, out Vector3d tar_thrust_acc_vec, out double throttle);
if (tar_acc_vec.Length() * State.Vessel.Mass <= 0.2d * Math.Min(Math.Min(State.Vessel.AvailableRCSForce.Item1.X, State.Vessel.AvailableRCSForce.Item1.Y), State.Vessel.AvailableRCSForce.Item1.Z) &&
rel_vel < 1d)
{
throttle = 0d;
approach_stage_ = string.IsNullOrEmpty(approach_dock_tag_) ?
ApproachStage.PCS : ApproachStage.DOCKING;
if (!string.IsNullOrEmpty(approach_control_node_tag_))
{
try
{
ActiveVessel.Parts.Controlling =
ActiveVessel.Parts.WithTag(approach_control_node_tag_).First();
Thread.Sleep(500);
State.Reset();
}
catch (Exception)
{ }
}
}
Command.SetThrottle(throttle);
RcsSetByForce((tar_acc_vec - tar_thrust_acc_vec) * State.Vessel.Mass);
}
break;
case ApproachStage.PCS:
{
ApproachCalculatePcs(
dst_dir, tar_vec_lateral,
tar_distance_lateral, tar_distance_facing,
rel_vel_vec, out Vector3d tar_acc_vec);
Command.SetThrottle(0d);
RcsSetByForce(tar_acc_vec * State.Vessel.Mass);
}
break;
case ApproachStage.DOCKING:
{
ApproachCalculatePcs(
dst_dir, tar_vec_lateral,
tar_distance_lateral, tar_distance_facing,
rel_vel_vec, out Vector3d tar_acc_vec);
Command.SetTargetDirection(-dst_dir);
Command.SetThrottle(0d);
RcsSetByForce(tar_acc_vec * State.Vessel.Mass);
}
break;
}
//Conn.Drawing().Clear();
//Conn.Drawing().AddDirection(
// SpaceCenter.TransformDirection(tar_vec.ToTuple(), OrbitBodyFrame, ActiveVessel.ReferenceFrame),
// ActiveVessel.ReferenceFrame);
//Conn.Drawing().AddDirection(
// SpaceCenter.TransformDirection(Command.DirectionVector.ToTuple(), OrbitBodyFrame, ActiveVessel.ReferenceFrame),
// ActiveVessel.ReferenceFrame);
//Conn.Drawing().AddDirection(
// SpaceCenter.TransformDirection(rel_vel_vec.Norm().ToTuple(), OrbitBodyFrame, ActiveVessel.ReferenceFrame),
// ActiveVessel.ReferenceFrame,
// 50f);
//Console.WriteLine("{0}\t{1:0.00}\t{2:0.0}",
// approach_stage_,
// tar_distance_lateral,
// tar_distance);
return(Status.UNFINISHED);
}
