public double getError (PoseHandle pose)
{
double pi2 = 2.0 * Math.PI;
double error = getCommand () - pose.getYaw () + pi2;
error -= Math.Floor ( error / pi2 ) * pi2;
return error - pi2;
}
public double getError (PoseHandle pose)
{
return getCommand () - pose.get ().position.z;
}
public StateHandle (QuadrotorInterface qInterface, Pose pose, Twist twist)
{
poseHandle = new PoseHandle ( qInterface, pose );
twistHandle = new TwistHandle ( qInterface, twist );
}
public void getError (PoseHandle pose, ref double x, ref double y)
{
getCommand ( ref x, ref y );
x -= pose.get ().position.x;
y -= pose.get ().position.y;
}
void registerPose (Pose pose)
{
pose_ = new PoseHandle ( this, pose );
}
