public void setGPS(maths.Vector3 pos)
{
_pos = pos;
return;
}
private bool GoToCheckpoint()
{
bool checkpointsDone = false;
if (_actualCheckpoint <= _targetCheckpoints.Count - 1)
{
_targetCheckpoints[_actualCheckpoint].VisualPoint.IsVisible = true;
maths.Vector3 errorVector = _targetCheckpoints[_actualCheckpoint].Point - base.Drone.GPS;
//Altitude target
base.TargetAltitude = _targetCheckpoints[_actualCheckpoint].Point.Y;
//Yaw target
float distance = errorVector.Length();
maths.Vector3 direction = errorVector;
direction.Normalize();
if (distance > 0.5f)
{
float errorYaw = (float)Math.Atan2(direction.X, -direction.Z) * 180 / (float)Math.PI;
base.TargetYaw = errorYaw;
}
//Pitch target
maths.Vector3 cosPitchAxis = new maths.Vector3(base.Drone.Pose.M13, base.Drone.Pose.M23, base.Drone.Pose.M33);
float errorPitch = -maths.Vector3.Dot(errorVector, cosPitchAxis);
if (Math.Abs(errorPitch) > DistanceTargetPitch)
{
errorPitch = DistanceTargetPitch * Math.Sign(errorPitch);
}
base.TargetPitch = errorPitch * MaxTargetPitch / (float)DistanceTargetPitch;
//Roll target
maths.Vector3 sinRollAxis = new maths.Vector3(base.Drone.Pose.M11, base.Drone.Pose.M21, base.Drone.Pose.M31);
float errorRoll = -maths.Vector3.Dot(errorVector, sinRollAxis);
if (Math.Abs(errorRoll) > DistanceTargetRoll)
{
errorRoll = DistanceTargetRoll * Math.Sign(errorRoll);
}
base.TargetRoll = errorRoll * MaxTargetRoll / (float)DistanceTargetRoll;
if (distance < _targetCheckpoints[_actualCheckpoint].DistanceError)
{
_targetCheckpoints[_actualCheckpoint].VisualPoint.IsVisible = false;
if (_actualCheckpoint == _targetCheckpoints.Count - 1)
{
checkpointsDone = true;
}
_actualCheckpoint++;
}
}
return checkpointsDone;
}
private void BindJoint(Object sender, EventArgs<IJoint> args)
{
_joint = (IMotorizedHingeJoint)args.Item;
_axis = ((MotorizedHingeJointDesc)_joint.Descriptor).Axis;
TargetRPM = 0;
}