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