<commit_before>#include <cstring>
#include <iostream>
#include <stdexcept>
#include <string>
using namespace std;
#include <arpa/inet.h>
#include <errno.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netdb.h>
#include <unistd.h>
#include <fcntl.h>
#include "socket.h"

// stdint.h is missing solaris 9-10 =[
#ifdef __sun
#	include <inttypes.h>
#else
#	include <stdint.h>
#endif

void Socket::IgnoreSIGPIPE()
{
	::signal(SIGPIPE, SIG_IGN);
}

const int BUF_LENGTH = 1024;

Socket::Socket(const string& host, const uint16_t port) : fd(-1), ref(0)
{
	Socket::IgnoreSIGPIPE();
	sockaddr_in addr;
	hostent *haddr = 0;

	haddr = ::gethostbyname(host.c_str());
	if (haddr == 0)
	{
		throw runtime_error(string("Socket error (resolve): ") + strerror(errno));
	}

	fd = ::socket(AF_INET, SOCK_STREAM, 0);
	if (fd == -1)
	{
		throw runtime_error(string("Socket error (sock): ") + strerror(errno));
	}
    int flags = fcntl(fd, F_GETFL, 0);
    fcntl(fd, F_SETFL, flags | O_NONBLOCK);// Set the socket to async so that we can have a shorter timeout

	memset((char*)&addr, 0, sizeof(addr));
	memcpy(&addr.sin_addr.s_addr, haddr->h_addr, haddr->h_length);
	addr.sin_family = AF_INET;
	addr.sin_port = htons(port);

	if (::connect(fd, (sockaddr*)&addr, sizeof(addr)) == -1 && errno != EINPROGRESS)
	{
		::close(fd);
		throw runtime_error(string("Socket error (conn): ") + strerror(errno));
	}

	// Use a timeout of 2 seconds
	fd_set fdset;
	FD_ZERO(&fdset);
	FD_SET(fd, &fdset);
	timeval timeout;
	memset((char*)&timeout, 0, sizeof(timeout));
	timeout.tv_sec = 2;

	if (::select(fd + 1, 0, &fdset, 0, &timeout) == -1)
	{
		bool bad = true;
		if (errno == EINPROGRESS)
		{
			int errc;
			socklen_t len = sizeof(errc);

			getsockopt(fd, SOL_SOCKET, SO_ERROR, &errc, &len);

			if (errc == 0)
			{
				bad = false;
			}
		}
		if (bad || errno == EINPROGRESS)
		{
			::close(fd);
			throw runtime_error(string("Socket error (conn): ") + strerror(errno));
		}

	}
    fcntl(fd, F_SETFL, flags);// unset the async socket

	remote = host;
	ref = new int(1);
}

Socket::Socket(int fdes, string Remote) : fd(fdes), ref(0), remote(Remote)
{
//	cerr << "socket new : " << fd << endl;
	ref = new int(1);
}

Socket::Socket(const Socket& rhs) : fd(rhs.fd), ref(rhs.ref), remote(rhs.remote)
{
//	cerr << "socket copy: " << fd << endl;
	++(*ref);
}

Socket& Socket::operator=(const Socket& rhs)
{
	Socket::IgnoreSIGPIPE();
	if (fd != -1)
	{
		--(*ref);
		if (*ref <= 1)
		{
			delete ref;
			::close(fd);
		}
	}
//	cerr << "socket equa: " << fd << endl;
	fd = rhs.fd;
	ref = rhs.ref;
	remote = rhs.remote;
	++(*ref);
	return *this;
}

Socket::~Socket()
{
	--(*ref);
//	cerr << "socket rem : " << fd << " " << *ref << endl;
	if (*ref == 0)
	{
		delete ref;
//		cerr << "socket disp: " << fd << " " << *ref << endl;
		::close(fd);
	}
}

void Socket::close(SocketEnd type)
{
	if ((type & Read) == Read)
	{
		::shutdown(fd, SHUT_RD);
	}
	if ((type & Write) == Write)
	{
		::shutdown(fd, SHUT_WR);
	}
}

string Socket::remoteHost() const
{
	return remote;
}

string Socket::read(long size)
{
	int recvChars, read = inBuffer.length();
	char buffer[BUF_LENGTH];
	string output;

	if (size == -1 || size > (long)inBuffer.length())
	{
		do
		{
			recvChars = ::recv(fd, buffer, BUF_LENGTH, 0);
			if (recvChars > 0)
			{
				read += recvChars;
				inBuffer.append(buffer, recvChars);
			}
		}
		while (recvChars >= 0 && read < size);
	}

	if (!inBuffer.empty() && recvChars == -1)
	{
		throw runtime_error(string("Socket error (read): ") + strerror(errno));
	}

	if (size > read)
	{
		output = inBuffer.substr(0, size);
		inBuffer.erase(0, size);
	}
	else
	{
		output = inBuffer;
		inBuffer.clear();
	}

	return output;
}

void Socket::write(const string& buffer)
{
	if (::send(fd, buffer.data(), buffer.length(), 0) == -1)
	{
		throw runtime_error(string("Socket error (write): ") + strerror(errno));
	}
}

ServerSocket::ServerSocket(const short port)
{
	Socket::IgnoreSIGPIPE();
	sockaddr_in addr;

	fd = ::socket(AF_INET, SOCK_STREAM, 0);
	if (fd == -1)
		throw runtime_error(string("Socket error (sock): ") + strerror(errno));

	memset((char*)&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr.s_addr = htonl(INADDR_ANY);
	addr.sin_port = htons(port);

	if (::bind(fd, (sockaddr*)&addr, sizeof(addr)) == -1)
		throw runtime_error(string("Socket error (bind): ") + strerror(errno));

	if (::listen(fd, 5) == -1)
		throw runtime_error(string("Socket error (listen): ") + strerror(errno));
}

ServerSocket::~ServerSocket()
{
	close(fd);
}

Socket ServerSocket::accept()
{
	Socket::IgnoreSIGPIPE();
	sockaddr_in in;
	socklen_t len = sizeof(in);
	int newSock = ::accept(fd, (sockaddr*)&in, &len);
	if (newSock == -1)
		throw runtime_error(string("Socket error (accept): ") + strerror(errno));

	return Socket(newSock, inet_ntoa(in.sin_addr));
}

void ServerSocket::serveForever(void (*callback)(Socket sock))
{
	while (true)
	{
		callback(accept());
	}
}
<commit_msg>socket.cpp: remove ServerSocket.serveForever()<commit_after>#include <cstring>
#include <iostream>
#include <stdexcept>
#include <string>
using namespace std;
#include <arpa/inet.h>
#include <errno.h>
#include <signal.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/select.h>
#include <netdb.h>
#include <unistd.h>
#include <fcntl.h>
#include "socket.h"

// stdint.h is missing solaris 9-10 =[
#ifdef __sun
#	include <inttypes.h>
#else
#	include <stdint.h>
#endif

void Socket::IgnoreSIGPIPE()
{
	::signal(SIGPIPE, SIG_IGN);
}

const int BUF_LENGTH = 1024;

Socket::Socket(const string& host, const uint16_t port) : fd(-1), ref(0)
{
	Socket::IgnoreSIGPIPE();
	sockaddr_in addr;
	hostent *haddr = 0;

	haddr = ::gethostbyname(host.c_str());
	if (haddr == 0)
	{
		throw runtime_error(string("Socket error (resolve): ") + strerror(errno));
	}

	fd = ::socket(AF_INET, SOCK_STREAM, 0);
	if (fd == -1)
	{
		throw runtime_error(string("Socket error (sock): ") + strerror(errno));
	}
    int flags = fcntl(fd, F_GETFL, 0);
    fcntl(fd, F_SETFL, flags | O_NONBLOCK);// Set the socket to async so that we can have a shorter timeout

	memset((char*)&addr, 0, sizeof(addr));
	memcpy(&addr.sin_addr.s_addr, haddr->h_addr, haddr->h_length);
	addr.sin_family = AF_INET;
	addr.sin_port = htons(port);

	if (::connect(fd, (sockaddr*)&addr, sizeof(addr)) == -1 && errno != EINPROGRESS)
	{
		::close(fd);
		throw runtime_error(string("Socket error (conn): ") + strerror(errno));
	}

	// Use a timeout of 2 seconds
	fd_set fdset;
	FD_ZERO(&fdset);
	FD_SET(fd, &fdset);
	timeval timeout;
	memset((char*)&timeout, 0, sizeof(timeout));
	timeout.tv_sec = 2;

	if (::select(fd + 1, 0, &fdset, 0, &timeout) == -1)
	{
		bool bad = true;
		if (errno == EINPROGRESS)
		{
			int errc;
			socklen_t len = sizeof(errc);

			getsockopt(fd, SOL_SOCKET, SO_ERROR, &errc, &len);

			if (errc == 0)
			{
				bad = false;
			}
		}
		if (bad || errno == EINPROGRESS)
		{
			::close(fd);
			throw runtime_error(string("Socket error (conn): ") + strerror(errno));
		}

	}
    fcntl(fd, F_SETFL, flags);// unset the async socket

	remote = host;
	ref = new int(1);
}

Socket::Socket(int fdes, string Remote) : fd(fdes), ref(0), remote(Remote)
{
//	cerr << "socket new : " << fd << endl;
	ref = new int(1);
}

Socket::Socket(const Socket& rhs) : fd(rhs.fd), ref(rhs.ref), remote(rhs.remote)
{
//	cerr << "socket copy: " << fd << endl;
	++(*ref);
}

Socket& Socket::operator=(const Socket& rhs)
{
	Socket::IgnoreSIGPIPE();
	if (fd != -1)
	{
		--(*ref);
		if (*ref <= 1)
		{
			delete ref;
			::close(fd);
		}
	}
//	cerr << "socket equa: " << fd << endl;
	fd = rhs.fd;
	ref = rhs.ref;
	remote = rhs.remote;
	++(*ref);
	return *this;
}

Socket::~Socket()
{
	--(*ref);
//	cerr << "socket rem : " << fd << " " << *ref << endl;
	if (*ref == 0)
	{
		delete ref;
//		cerr << "socket disp: " << fd << " " << *ref << endl;
		::close(fd);
	}
}

void Socket::close(SocketEnd type)
{
	if ((type & Read) == Read)
	{
		::shutdown(fd, SHUT_RD);
	}
	if ((type & Write) == Write)
	{
		::shutdown(fd, SHUT_WR);
	}
}

string Socket::remoteHost() const
{
	return remote;
}

string Socket::read(long size)
{
	int recvChars, read = inBuffer.length();
	char buffer[BUF_LENGTH];
	string output;

	if (size == -1 || size > (long)inBuffer.length())
	{
		do
		{
			recvChars = ::recv(fd, buffer, BUF_LENGTH, 0);
			if (recvChars > 0)
			{
				read += recvChars;
				inBuffer.append(buffer, recvChars);
			}
		}
		while (recvChars >= 0 && read < size);
	}

	if (!inBuffer.empty() && recvChars == -1)
	{
		throw runtime_error(string("Socket error (read): ") + strerror(errno));
	}

	if (size > read)
	{
		output = inBuffer.substr(0, size);
		inBuffer.erase(0, size);
	}
	else
	{
		output = inBuffer;
		inBuffer.clear();
	}

	return output;
}

void Socket::write(const string& buffer)
{
	if (::send(fd, buffer.data(), buffer.length(), 0) == -1)
	{
		throw runtime_error(string("Socket error (write): ") + strerror(errno));
	}
}

ServerSocket::ServerSocket(const short port)
{
	Socket::IgnoreSIGPIPE();
	sockaddr_in addr;

	fd = ::socket(AF_INET, SOCK_STREAM, 0);
	if (fd == -1)
		throw runtime_error(string("Socket error (sock): ") + strerror(errno));

	memset((char*)&addr, 0, sizeof(addr));
	addr.sin_family = AF_INET;
	addr.sin_addr.s_addr = htonl(INADDR_ANY);
	addr.sin_port = htons(port);

	if (::bind(fd, (sockaddr*)&addr, sizeof(addr)) == -1)
		throw runtime_error(string("Socket error (bind): ") + strerror(errno));

	if (::listen(fd, 5) == -1)
		throw runtime_error(string("Socket error (listen): ") + strerror(errno));
}

ServerSocket::~ServerSocket()
{
	close(fd);
}

Socket ServerSocket::accept()
{
	Socket::IgnoreSIGPIPE();
	sockaddr_in in;
	socklen_t len = sizeof(in);
	int newSock = ::accept(fd, (sockaddr*)&in, &len);
	if (newSock == -1)
		throw runtime_error(string("Socket error (accept): ") + strerror(errno));

	return Socket(newSock, inet_ntoa(in.sin_addr));
}

<|endoftext|>


<commit_before>/* The libMesh Finite Element Library. */
/* Copyright (C) 2003  Benjamin S. Kirk */

/* This library is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU Lesser General Public */
/* License as published by the Free Software Foundation; either */
/* version 2.1 of the License, or (at your option) any later version. */

/* This library is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU */
/* Lesser General Public License for more details. */

/* You should have received a copy of the GNU Lesser General Public */
/* License along with this library; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

 //
Introduction Example 1 - Creation of a Mesh Object

 //
 // This is the first example program.  It simply demonstrates
 // how to create a mesh object.  A mesh is read from file,
 // information is printed to the screen, and the mesh is then
 // written.

 // C++ include files that we need
#include <iostream>
// Functions to initialize the library.
#include "libmesh/libmesh.h"
// Basic include files needed for the mesh functionality.
#include "libmesh/mesh.h"

// Bring in everything from the libMesh namespace
using namespace libMesh;

int main (int argc, char** argv)
{
  // Initialize the library.  This is necessary because the library
  // may depend on a number of other libraries (i.e. MPI and PETSc)
  // that require initialization before use.  When the LibMeshInit
  // object goes out of scope, other libraries and resources are
  // finalized.
  LibMeshInit init (argc, argv);

  // Check for proper usage. The program is designed to be run
  // as follows:
  // ./ex1 -d DIM input_mesh_name [-o output_mesh_name]
  // where [output_mesh_name] is an optional parameter giving
  // a filename to write the mesh into.
  if (argc < 4)
    {
      if (libMesh::processor_id() == 0)
        std::cerr << "Usage: " << argv[0] << " -d 2 in.mesh [-o out.mesh]"
                  << std::endl;

      // This handy function will print the file name, line number,
      // and then abort.  Currently the library does not use C++
      // exception handling.
      libmesh_error();
    }

  // Get the dimensionality of the mesh from argv[2]
  const unsigned int dim = std::atoi(argv[2]);

  // Skip higher-dimensional examples on a lower-dimensional libMesh build
  libmesh_example_assert(dim <= LIBMESH_DIM, "2D/3D support");

  // Create a mesh, with dimension to be overridden later, on the
  // default MPI communicator.
  Mesh mesh(init.comm());

  // We may need XDR support compiled in to read binary .xdr files
  std::string input_filename = argv[3];
  libmesh_example_assert(LIBMESH_HAVE_XDR ||
                         input_filename.rfind(".xdr") >=
                         input_filename.size(), "XDR support");

  // Read the input mesh.
  mesh.read (argv[3]);

  // Print information about the mesh to the screen.
  mesh.print_info();

  // Write the output mesh if the user specified an
  // output file name.
  if (argc >= 6 && std::string("-o") == argv[4])
    {
      // We may need XDR support compiled in to read binary .xdr files
      std::string output_filename = argv[5];
      libmesh_example_assert(LIBMESH_HAVE_XDR ||
                             output_filename.rfind(".xdr") >=
                             output_filename.size(), "XDR support");

      mesh.write (argv[5]);
    }

  // All done.  libMesh objects are destroyed here.  Because the
  // LibMeshInit object was created first, its destruction occurs
  // last, and it's destructor finalizes any external libraries and
  // checks for leaked memory.
  return 0;
}
<commit_msg>Fix for --disable-xdr<commit_after>/* The libMesh Finite Element Library. */
/* Copyright (C) 2003  Benjamin S. Kirk */

/* This library is free software; you can redistribute it and/or */
/* modify it under the terms of the GNU Lesser General Public */
/* License as published by the Free Software Foundation; either */
/* version 2.1 of the License, or (at your option) any later version. */

/* This library is distributed in the hope that it will be useful, */
/* but WITHOUT ANY WARRANTY; without even the implied warranty of */
/* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU */
/* Lesser General Public License for more details. */

/* You should have received a copy of the GNU Lesser General Public */
/* License along with this library; if not, write to the Free Software */
/* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA */

 //
Introduction Example 1 - Creation of a Mesh Object

 //
 // This is the first example program.  It simply demonstrates
 // how to create a mesh object.  A mesh is read from file,
 // information is printed to the screen, and the mesh is then
 // written.

 // C++ include files that we need
#include <iostream>
// Functions to initialize the library.
#include "libmesh/libmesh.h"
// Basic include files needed for the mesh functionality.
#include "libmesh/mesh.h"

// Bring in everything from the libMesh namespace
using namespace libMesh;

int main (int argc, char** argv)
{
  // Initialize the library.  This is necessary because the library
  // may depend on a number of other libraries (i.e. MPI and PETSc)
  // that require initialization before use.  When the LibMeshInit
  // object goes out of scope, other libraries and resources are
  // finalized.
  LibMeshInit init (argc, argv);

  // Check for proper usage. The program is designed to be run
  // as follows:
  // ./ex1 -d DIM input_mesh_name [-o output_mesh_name]
  // where [output_mesh_name] is an optional parameter giving
  // a filename to write the mesh into.
  if (argc < 4)
    {
      if (libMesh::processor_id() == 0)
        std::cerr << "Usage: " << argv[0] << " -d 2 in.mesh [-o out.mesh]"
                  << std::endl;

      // This handy function will print the file name, line number,
      // and then abort.  Currently the library does not use C++
      // exception handling.
      libmesh_error();
    }

  // Get the dimensionality of the mesh from argv[2]
  const unsigned int dim = std::atoi(argv[2]);

  // Skip higher-dimensional examples on a lower-dimensional libMesh build
  libmesh_example_assert(dim <= LIBMESH_DIM, "2D/3D support");

  // Create a mesh, with dimension to be overridden later, on the
  // default MPI communicator.
  Mesh mesh(init.comm());

  // We may need XDR support compiled in to read binary .xdr files
  std::string input_filename = argv[3];
#ifndef LIBMESH_HAVE_XDR
  libmesh_example_assert(input_filename.rfind(".xdr") >=
                         input_filename.size(), "XDR support");
#endif

  // Read the input mesh.
  mesh.read (argv[3]);

  // Print information about the mesh to the screen.
  mesh.print_info();

  // Write the output mesh if the user specified an
  // output file name.
  if (argc >= 6 && std::string("-o") == argv[4])
    {
      // We may need XDR support compiled in to read binary .xdr files
      std::string output_filename = argv[5];
#ifndef LIBMESH_HAVE_XDR
      libmesh_example_assert(output_filename.rfind(".xdr") >=
                             output_filename.size(), "XDR support");
#endif

      mesh.write (argv[5]);
    }

  // All done.  libMesh objects are destroyed here.  Because the
  // LibMeshInit object was created first, its destruction occurs
  // last, and it's destructor finalizes any external libraries and
  // checks for leaked memory.
  return 0;
}
<|endoftext|>


<commit_before>/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

=========================================================================*/

#include <irtkImage.h>

#include <irtkImageFunction.h>

#include <irtkHistogram.h>

#include <irtkTransformation.h>

// Default filenames
char *source_name = NULL, *target_name = NULL, *mask_name = NULL;
char *trans_name  = NULL, *output_name = NULL;

#define DEFAULT_BINS 255

// Default number of bins
int nbins_x = 0, nbins_y = 0;

void usage()
{
  cerr << "Usage: evaluation [target] [source] <options>\n" << endl;
  cerr << "where <options> is one or more of the following: \n" << endl;
  cerr << "<-dofin file>      Input transformation" << endl;
  cerr << "<-nbins_x no>      Number of bins for target (Default smaller of dynamic range or 255)" << endl;
  cerr << "<-nbins_y no>      Number of bins for source (Default as above)" << endl;
  cerr << "<-Rx1 pixel>       Region of interest" << endl;
  cerr << "<-Ry1 pixel>       Region of interest" << endl;
  cerr << "<-Rz1 pixel>       Region of interest" << endl;
  cerr << "<-Rx2 pixel>       Region of interest" << endl;
  cerr << "<-Ry2 pixel>       Region of interest" << endl;
  cerr << "<-Rz2 pixel>       Region of interest" << endl;
  cerr << "<-Tp  value>       Padding value in target" << endl;
  cerr << "<-mask file>       Binary mask to define ROI" << endl;
  cerr << "<-linear>          Linear interpolation" << endl;
  cerr << "<-bspline>         B-spline interpolation" << endl;
  cerr << "<-cspline>         Cubic spline interpolation" << endl;
  cerr << "<-sinc>            Sinc interpolation" << endl;
  cerr << "<-output>          Output similarity values to file" << endl;
  exit(1);
}

int main(int argc, char **argv)
{
  irtkTransformation *transformation = NULL;
  irtkInterpolateImageFunction *interpolator = NULL;
  irtkGreyPixel target_min, source_min, target_max, source_max;
  int ok, i, x, y, z, i1, j1, k1, i2, j2, k2;
  double x1, y1, z1, x2, y2, z2, Tp, widthx, widthy, val;

  // Check command line
  if (argc < 3) {
    usage();
  }

  // Parse source and target images
  target_name = argv[1];
  argc--;
  argv++;
  source_name = argv[1];
  argc--;
  argv++;

  // Read target and source image
  irtkGreyImage target(target_name);
  irtkGreyImage source(source_name);

  // Default padding
  Tp = -1.0 * FLT_MAX;

  // Fix ROI
  i1 = 0;
  j1 = 0;
  k1 = 0;
  i2 = target.GetX();
  j2 = target.GetY();
  k2 = target.GetZ();

  // Fix no. of bins;
  nbins_x = 0;
  nbins_y = 0;

  while (argc > 1) {
    ok = false;
    if ((ok == false) && (strcmp(argv[1], "-dofin") == 0)) {
      argc--;
      argv++;
      trans_name = argv[1];
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-nbins_x") == 0)) {
      argc--;
      argv++;
      nbins_x = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-nbins_y") == 0)) {
      argc--;
      argv++;
      nbins_y = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Tp") == 0)) {
      argc--;
      argv++;
      Tp = atof(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rx1") == 0)) {
      argc--;
      argv++;
      i1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rx2") == 0)) {
      argc--;
      argv++;
      i2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Ry1") == 0)) {
      argc--;
      argv++;
      j1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Ry2") == 0)) {
      argc--;
      argv++;
      j2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rz1") == 0)) {
      argc--;
      argv++;
      k1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rz2") == 0)) {
      argc--;
      argv++;
      k2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-linear") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
        interpolator = new irtkLinearInterpolateImageFunction2D;
      } else {
        interpolator = new irtkLinearInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-bspline") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
      	interpolator = new irtkBSplineInterpolateImageFunction2D;
      } else {
      	interpolator = new irtkBSplineInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-cspline") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
      	interpolator = new irtkCSplineInterpolateImageFunction2D;
      } else {
      	interpolator = new irtkCSplineInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-sinc") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
      	interpolator = new irtkSincInterpolateImageFunction2D;
      } else {
      	interpolator = new irtkSincInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-mask") == 0)) {
      argc--;
      argv++;
      mask_name = argv[1];
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-output") == 0)) {
        argc--;
        argv++;
        output_name = argv[1];
        argc--;
        argv++;
        ok = true;
    }
    if (ok == false) {
      cerr << "Can not parse argument " << argv[1] << endl;
      usage();
    }
  }

  irtkGreyImage mask;

  // Set up a mask,
  if (mask_name == NULL) {
    mask.Read(target_name);

    irtkGreyPixel *ptr2mask = mask.GetPointerToVoxels();
    irtkGreyPixel *ptr2tgt  = target.GetPointerToVoxels();

    for (i = 0; i < target.GetNumberOfVoxels(); i++) {
      if (*ptr2tgt > Tp)
        *ptr2mask = 1;
      else
        *ptr2mask = 0;

      ++ptr2tgt;
      ++ptr2mask;
    }
  } else {
    mask.Read(mask_name);
    if ((mask.GetX() != target.GetX()) ||
        (mask.GetY() != target.GetY()) ||
        (mask.GetZ() != target.GetZ())) {
      cerr << "evaluation2: Target and mask dimensions mismatch. Exiting." << endl;
      exit(1);
    }
  }

  // If there is an region of interest, use it
  if ((i1 != 0) || (i2 != target.GetX()) ||
      (j1 != 0) || (j2 != target.GetY()) ||
      (k1 != 0) || (k2 != target.GetZ())) {
    target = target.GetRegion(i1, j1, k1, i2, j2, k2);
    source = source.GetRegion(i1, j1, k1, i2, j2, k2);
    mask   = mask.GetRegion(i1, j1, k1, i2, j2, k2);
  }

  // Set min and max of histogram
  target.GetMinMax(&target_min, &target_max);
  source.GetMinMax(&source_min, &source_max);

  // Calculate number of bins to use
  if (nbins_x == 0) {
    nbins_x = (int) round(target_max - target_min) + 1;
    if (nbins_x > DEFAULT_BINS)
      nbins_x = DEFAULT_BINS;
  }

  if (nbins_y == 0) {
    nbins_y = (int) round(source_max - source_min) + 1;
    if (nbins_y > DEFAULT_BINS)
      nbins_y = DEFAULT_BINS;
  }

  // Create default interpolator if necessary
  if (interpolator == NULL) {
  	if (source.GetZ() == 1){
  		interpolator = new irtkNearestNeighborInterpolateImageFunction2D;
  	} else {
  		interpolator = new irtkNearestNeighborInterpolateImageFunction;
  	}
  }
  interpolator->SetInput(&source);
  interpolator->Initialize();

  // Calculate the source image domain in which we can interpolate
  interpolator->Inside(x1, y1, z1, x2, y2, z2);

  // Create histogram
  irtkHistogram_2D<int> histogram(nbins_x, nbins_y);
  widthx = (target_max - target_min) / (nbins_x - 1.0);
  widthy = (source_max - source_min) / (nbins_y - 1.0);

  histogram.PutMin(target_min - 0.5*widthx, source_min - 0.5*widthy);
  histogram.PutMax(target_max + 0.5*widthx, source_max + 0.5*widthy);

  if (trans_name == 0) {
    transformation = new irtkRigidTransformation;
  } else {
    transformation = irtkTransformation::New(trans_name);
  }

  target_min = FLT_MAX;
  source_min = FLT_MAX;
  target_max = -1.0 * FLT_MAX;
  source_max = -1.0 * FLT_MAX;

  // Fill histogram
  for (z = 0; z < target.GetZ(); z++) {
    for (y = 0; y < target.GetY(); y++) {
      for (x = 0; x < target.GetX(); x++) {

        if (mask(x, y, z) > 0) {
          val = target(x, y, z);

          if (val > target_max)
            target_max = val;
          if (val < target_min)
            target_min = val;

          irtkPoint p(x, y, z);
          // Transform point into world coordinates
          target.ImageToWorld(p);
          // Transform point
          transformation->Transform(p);
          // Transform point into image coordinates
          source.WorldToImage(p);

        	// A bad thing might happen for the 2D case.
        	if ((source.GetZ() == 1) &&
		    (p._z > 0.5 || p._z < -0.5)){
		  cerr << "Transformed point outside plane of 2D source image." << endl;
		  exit(1);
        	}

        	// 2D and in plane but out of FoV.
        	if ((source.GetZ() == 1) &&
		    (p._x <= x1 || p._x >= x2 ||
		     p._y <= y1 || p._y >= y2))
		  continue;

        	// 3D and out of FoV.
        	if ((source.GetZ() > 1) &&
		    (p._x <= x1 || p._x >= x2 ||
		     p._y <= y1 || p._y >= y2 ||
		     p._z <= z1 || p._z >= z2))
		  continue;

		// Should be able to interpolate if we've got this far.

        	val = interpolator->EvaluateInside(p._x, p._y, p._z);

        	histogram.AddSample(target(x, y, z), val);
        	if (val >  source_max)
        		source_max = val;
        	if (val < source_min)
        		source_min = val;

        }
      }
    }
  }

  cout << "SSD: "    << histogram.SumsOfSquaredDifferences() / (double)histogram.NumberOfSamples() << endl;
  cout << "CC: "     << histogram.CrossCorrelation() << endl;
  cout << "NMI: "    << histogram.NormalizedMutualInformation() << endl;
  if (nbins_x == nbins_y) {
      cout << "Label consistency: " << histogram.LabelConsistency() << endl;
      cout << "Kappa statistic: " << histogram.Kappa() << endl;
    }

  if(output_name){
      cerr << "Writing Results of SSD CC and NMI to " << output_name << endl;
      ofstream fout(output_name,ios::app);
      fout << histogram.SumsOfSquaredDifferences() / (double)histogram.NumberOfSamples() <<endl;
      fout << histogram.CrossCorrelation() <<endl;
      fout << histogram.NormalizedMutualInformation() <<endl;
	  if (nbins_x == nbins_y) {
		  fout << histogram.LabelConsistency() << endl;
	  }
      fout.close();
  }
}
<commit_msg>padding for source image<commit_after>/*=========================================================================

  Library   : Image Registration Toolkit (IRTK)
  Module    : $Id$
  Copyright : Imperial College, Department of Computing
              Visual Information Processing (VIP), 2008 onwards
  Date      : $Date$
  Version   : $Revision$
  Changes   : $Author$

=========================================================================*/

#include <irtkImage.h>

#include <irtkImageFunction.h>

#include <irtkHistogram.h>

#include <irtkTransformation.h>

// Default filenames
char *source_name = NULL, *target_name = NULL, *mask_name = NULL;
char *trans_name  = NULL, *output_name = NULL;

#define DEFAULT_BINS 255

// Default number of bins
int nbins_x = 0, nbins_y = 0;

void usage()
{
  cerr << "Usage: evaluation [target] [source] <options>\n" << endl;
  cerr << "where <options> is one or more of the following: \n" << endl;
  cerr << "<-dofin file>      Input transformation" << endl;
  cerr << "<-nbins_x no>      Number of bins for target (Default smaller of dynamic range or 255)" << endl;
  cerr << "<-nbins_y no>      Number of bins for source (Default as above)" << endl;
  cerr << "<-Rx1 pixel>       Region of interest" << endl;
  cerr << "<-Ry1 pixel>       Region of interest" << endl;
  cerr << "<-Rz1 pixel>       Region of interest" << endl;
  cerr << "<-Rx2 pixel>       Region of interest" << endl;
  cerr << "<-Ry2 pixel>       Region of interest" << endl;
  cerr << "<-Rz2 pixel>       Region of interest" << endl;
  cerr << "<-Tp  value>       Padding value in target" << endl;
  cerr << "<-mask file>       Binary mask to define ROI" << endl;
  cerr << "<-linear>          Linear interpolation" << endl;
  cerr << "<-bspline>         B-spline interpolation" << endl;
  cerr << "<-cspline>         Cubic spline interpolation" << endl;
  cerr << "<-sinc>            Sinc interpolation" << endl;
  cerr << "<-output>          Output similarity values to file" << endl;
  exit(1);
}

int main(int argc, char **argv)
{
  irtkTransformation *transformation = NULL;
  irtkInterpolateImageFunction *interpolator = NULL;
  irtkGreyPixel target_min, source_min, target_max, source_max;
  int ok, i, x, y, z, i1, j1, k1, i2, j2, k2;
  double x1, y1, z1, x2, y2, z2, Tp, widthx, widthy, val;

  // Check command line
  if (argc < 3) {
    usage();
  }

  // Parse source and target images
  target_name = argv[1];
  argc--;
  argv++;
  source_name = argv[1];
  argc--;
  argv++;

  // Read target and source image
  irtkGreyImage target(target_name);
  irtkGreyImage source(source_name);

  // Default padding
  Tp = -1.0 * FLT_MAX;

  // Fix ROI
  i1 = 0;
  j1 = 0;
  k1 = 0;
  i2 = target.GetX();
  j2 = target.GetY();
  k2 = target.GetZ();

  // Fix no. of bins;
  nbins_x = 0;
  nbins_y = 0;

  while (argc > 1) {
    ok = false;
    if ((ok == false) && (strcmp(argv[1], "-dofin") == 0)) {
      argc--;
      argv++;
      trans_name = argv[1];
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-nbins_x") == 0)) {
      argc--;
      argv++;
      nbins_x = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-nbins_y") == 0)) {
      argc--;
      argv++;
      nbins_y = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Tp") == 0)) {
      argc--;
      argv++;
      Tp = atof(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rx1") == 0)) {
      argc--;
      argv++;
      i1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rx2") == 0)) {
      argc--;
      argv++;
      i2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Ry1") == 0)) {
      argc--;
      argv++;
      j1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Ry2") == 0)) {
      argc--;
      argv++;
      j2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rz1") == 0)) {
      argc--;
      argv++;
      k1 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-Rz2") == 0)) {
      argc--;
      argv++;
      k2 = atoi(argv[1]);
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-linear") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
        interpolator = new irtkLinearInterpolateImageFunction2D;
      } else {
        interpolator = new irtkLinearInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-bspline") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
      	interpolator = new irtkBSplineInterpolateImageFunction2D;
      } else {
      	interpolator = new irtkBSplineInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-cspline") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
      	interpolator = new irtkCSplineInterpolateImageFunction2D;
      } else {
      	interpolator = new irtkCSplineInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-sinc") == 0)) {
      argc--;
      argv++;
      if (source.GetZ() == 1){
      	interpolator = new irtkSincInterpolateImageFunction2D;
      } else {
      	interpolator = new irtkSincInterpolateImageFunction;
      }
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-mask") == 0)) {
      argc--;
      argv++;
      mask_name = argv[1];
      argc--;
      argv++;
      ok = true;
    }
    if ((ok == false) && (strcmp(argv[1], "-output") == 0)) {
        argc--;
        argv++;
        output_name = argv[1];
        argc--;
        argv++;
        ok = true;
    }
    if (ok == false) {
      cerr << "Can not parse argument " << argv[1] << endl;
      usage();
    }
  }

  irtkGreyImage mask;

  // Set up a mask,
  if (mask_name == NULL) {
    mask.Read(target_name);

    irtkGreyPixel *ptr2mask = mask.GetPointerToVoxels();
    irtkGreyPixel *ptr2tgt  = target.GetPointerToVoxels();

    for (i = 0; i < target.GetNumberOfVoxels(); i++) {
      if (*ptr2tgt > Tp)
        *ptr2mask = 1;
      else
        *ptr2mask = 0;

      ++ptr2tgt;
      ++ptr2mask;
    }
  } else {
    mask.Read(mask_name);
    if ((mask.GetX() != target.GetX()) ||
        (mask.GetY() != target.GetY()) ||
        (mask.GetZ() != target.GetZ())) {
      cerr << "evaluation2: Target and mask dimensions mismatch. Exiting." << endl;
      exit(1);
    }
  }

  // If there is an region of interest, use it
  if ((i1 != 0) || (i2 != target.GetX()) ||
      (j1 != 0) || (j2 != target.GetY()) ||
      (k1 != 0) || (k2 != target.GetZ())) {
    target = target.GetRegion(i1, j1, k1, i2, j2, k2);
    source = source.GetRegion(i1, j1, k1, i2, j2, k2);
    mask   = mask.GetRegion(i1, j1, k1, i2, j2, k2);
  }

  // Set min and max of histogram
  target.GetMinMax(&target_min, &target_max);
  source.GetMinMax(&source_min, &source_max);

  // Calculate number of bins to use
  if (nbins_x == 0) {
    nbins_x = (int) round(target_max - target_min) + 1;
    if (nbins_x > DEFAULT_BINS)
      nbins_x = DEFAULT_BINS;
  }

  if (nbins_y == 0) {
    nbins_y = (int) round(source_max - source_min) + 1;
    if (nbins_y > DEFAULT_BINS)
      nbins_y = DEFAULT_BINS;
  }

  // Create default interpolator if necessary
  if (interpolator == NULL) {
  	if (source.GetZ() == 1){
  		interpolator = new irtkNearestNeighborInterpolateImageFunction2D;
  	} else {
  		interpolator = new irtkNearestNeighborInterpolateImageFunction;
  	}
  }
  interpolator->SetInput(&source);
  interpolator->Initialize();

  // Calculate the source image domain in which we can interpolate
  interpolator->Inside(x1, y1, z1, x2, y2, z2);

  // Create histogram
  irtkHistogram_2D<int> histogram(nbins_x, nbins_y);
  widthx = (target_max - target_min) / (nbins_x - 1.0);
  widthy = (source_max - source_min) / (nbins_y - 1.0);

  histogram.PutMin(target_min - 0.5*widthx, source_min - 0.5*widthy);
  histogram.PutMax(target_max + 0.5*widthx, source_max + 0.5*widthy);

  if (trans_name == 0) {
    transformation = new irtkRigidTransformation;
  } else {
    transformation = irtkTransformation::New(trans_name);
  }

  target_min = FLT_MAX;
  source_min = FLT_MAX;
  target_max = -1.0 * FLT_MAX;
  source_max = -1.0 * FLT_MAX;

  // Fill histogram
  for (z = 0; z < target.GetZ(); z++) {
    for (y = 0; y < target.GetY(); y++) {
      for (x = 0; x < target.GetX(); x++) {

        if (mask(x, y, z) > 0) {
          val = target(x, y, z);

          if (val > target_max)
            target_max = val;
          if (val < target_min)
            target_min = val;

          irtkPoint p(x, y, z);
          // Transform point into world coordinates
          target.ImageToWorld(p);
          // Transform point
          transformation->Transform(p);
          // Transform point into image coordinates
          source.WorldToImage(p);

        	// A bad thing might happen for the 2D case.
        	if ((source.GetZ() == 1) &&
		    (p._z > 0.5 || p._z < -0.5)){
		  cerr << "Transformed point outside plane of 2D source image." << endl;
		  exit(1);
        	}

        	// 2D and in plane but out of FoV.
        	if ((source.GetZ() == 1) &&
		    (p._x <= x1 || p._x >= x2 ||
		     p._y <= y1 || p._y >= y2))
		  continue;

        	// 3D and out of FoV.
        	if ((source.GetZ() > 1) &&
		    (p._x <= x1 || p._x >= x2 ||
		     p._y <= y1 || p._y >= y2 ||
		     p._z <= z1 || p._z >= z2))
		  continue;

		// Should be able to interpolate if we've got this far.

        	val = interpolator->EvaluateInside(p._x, p._y, p._z);

			if(val > Tp){
        		histogram.AddSample(target(x, y, z), val);
			}

        	if (val >  source_max)
        		source_max = val;
        	if (val < source_min)
        		source_min = val;

        }
      }
    }
  }

  cout << "SSD: "    << histogram.SumsOfSquaredDifferences() / (double)histogram.NumberOfSamples() << endl;
  cout << "CC: "     << histogram.CrossCorrelation() << endl;
  cout << "NMI: "    << histogram.NormalizedMutualInformation() << endl;
  if (nbins_x == nbins_y) {
      cout << "Label consistency: " << histogram.LabelConsistency() << endl;
      cout << "Kappa statistic: " << histogram.Kappa() << endl;
    }

  if(output_name){
      cerr << "Writing Results of SSD CC and NMI to " << output_name << endl;
      ofstream fout(output_name,ios::app);
      fout << histogram.SumsOfSquaredDifferences() / (double)histogram.NumberOfSamples() <<endl;
      fout << histogram.CrossCorrelation() <<endl;
      fout << histogram.NormalizedMutualInformation() <<endl;
	  if (nbins_x == nbins_y) {
		  fout << histogram.LabelConsistency() << endl;
	  }
      fout.close();
  }
}
<|endoftext|>


<commit_before>/***************************************************
  This is a library for the HDC1000 Humidity & Temp Sensor

  Designed specifically to work with the HDC1008 sensor from Adafruit
  ----> https://www.adafruit.com/products/2635

  These sensors use I2C to communicate, 2 pins are required to
  interface
  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.
  BSD license, all text above must be included in any redistribution

  Modified for Photon  needs application.h for types  RMB
 ****************************************************/
#include "application.h"
#include "Adafruit_HDC1000/Adafruit_HDC1000.h"

Adafruit_HDC1000::Adafruit_HDC1000() {
}

boolean Adafruit_HDC1000::begin(uint8_t addr) {
  _i2caddr = addr;

  Wire.begin();

  reset();
  if (read16(HDC1000_MANUFID) != 0x5449) return false;
  if (read16(HDC1000_DEVICEID) != 0x1000) return false;
  return true;
}

void Adafruit_HDC1000::reset(void) {
  // reset,combined temp/humidity measurement,and select 14 bit temp & humidity resolution
  uint16_t config = HDC1000_CONFIG_RST | HDC1000_CONFIG_MODE | HDC1000_CONFIG_TRES_14 | HDC1000_CONFIG_HRES_14;

  Wire.beginTransmission(_i2caddr);
  Wire.write(HDC1000_CONFIG);   // set pointer register to configuration register   RMB
  Wire.write(config>>8);        // now write out 2 bytes MSB first    RMB
  Wire.write(config&0xFF);
  Wire.endTransmission();
  delay(15);
}

float Adafruit_HDC1000::readTemperature(void) {

  float temp = (read32(HDC1000_TEMP, 20) >> 16);
  temp /= 65536;
  temp *= 165;
  temp -= 40;

  return temp;
}

float Adafruit_HDC1000::readHumidity(void) {
  // reads both temp and humidity, masks out temp in highest 16 bits
  // originally used hum but humidity declared in private section of class
  float humidity = (read32(HDC1000_TEMP, 20) & 0xFFFF);

  humidity /= 65536;
  humidity *= 100;

  return hum;
}

// Add ability to test battery voltage, useful in remote monitoring, TRUE if <2.8V
// Thanks to KFricke for micropython-hdc1008 on GitHub, usually called after Temp/Humid reading  RMB

boolean Adafruit_HDC1000::batteryLOW(void)  {

  uint16_t battLOW = (read16(HDC1000_CONFIG_BATT, 20));

  battLOW &= HDC1000_CONFIG_BATT;   // mask off other bits, bit 11 will be 1 if voltage < 2.8V

  if (battLOW> 0) return true;
  return  false;
}

/*********************************************************************/

uint16_t Adafruit_HDC1000::read16(uint8_t a, uint8_t d) {
  Wire.beginTransmission(_i2caddr);
  Wire.write(a);
  Wire.endTransmission();
  delay(d);
  Wire.requestFrom((uint8_t)_i2caddr, (uint8_t)2);
  uint16_t r = Wire.read();
  r <<= 8;
  r |= Wire.read();
  //Serial.println(r, HEX);
  return r;
}

uint32_t Adafruit_HDC1000::read32(uint8_t a, uint8_t d) {
  Wire.beginTransmission(_i2caddr);
  Wire.write(a);
  Wire.endTransmission();
  // delay was hardcoded as 50, should be d   (delay)
  delay(d);
  Wire.requestFrom((uint8_t)_i2caddr, (uint8_t)4);
  uint32_t r = Wire.read();
  // assembles temp into highest 16 bits, humidity into lowest 16 bits
  r <<= 8;
  r |= Wire.read();
  r <<= 8;
  r |= Wire.read();
  r <<= 8;
  r |= Wire.read();
  //Serial.println(r, HEX);
  return r;
}
<commit_msg>add ReadTempHumidity() function and getter func<commit_after>/***************************************************
  This is a library for the HDC1000 Humidity & Temp Sensor

  Designed specifically to work with the HDC1008 sensor from Adafruit
  ----> https://www.adafruit.com/products/2635

  These sensors use I2C to communicate, 2 pins are required to
  interface
  Adafruit invests time and resources providing this open source code,
  please support Adafruit and open-source hardware by purchasing
  products from Adafruit!

  Written by Limor Fried/Ladyada for Adafruit Industries.
  BSD license, all text above must be included in any redistribution

  Modified for Photon  needs application.h for types  RMB
 ****************************************************/
#include "application.h"
#include "Adafruit_HDC1000/Adafruit_HDC1000.h"

Adafruit_HDC1000::Adafruit_HDC1000() {
}

boolean Adafruit_HDC1000::begin(uint8_t addr) {
  _i2caddr = addr;

  Wire.begin();

  reset();
  if (read16(HDC1000_MANUFID) != 0x5449) return false;
  if (read16(HDC1000_DEVICEID) != 0x1000) return false;
  return true;
}

void Adafruit_HDC1000::reset(void) {
  // reset,combined temp/humidity measurement,and select 14 bit temp & humidity resolution
  uint16_t config = HDC1000_CONFIG_RST | HDC1000_CONFIG_MODE | HDC1000_CONFIG_TRES_14 | HDC1000_CONFIG_HRES_14;

  Wire.beginTransmission(_i2caddr);
  Wire.write(HDC1000_CONFIG);   // set pointer register to configuration register   RMB
  Wire.write(config>>8);        // now write out 2 bytes MSB first    RMB
  Wire.write(config&0xFF);
  Wire.endTransmission();
  delay(15);
}

float Adafruit_HDC1000::readTemperature(void) {

  float temp = (read32(HDC1000_TEMP, 20) >> 16);
  temp /= 65536;
  temp *= 165;
  temp -= 40;

  return temp;
}

float Adafruit_HDC1000::readHumidity(void) {
  // reads both temp and humidity but masks out temp in highest 16 bits
  // originally used hum but humidity declared in private section of class
  float humidity = (read32(HDC1000_TEMP, 20) & 0xFFFF);

  humidity /= 65536;
  humidity *= 100;

  return humidity;
}

void Adafruit_HDC1000::ReadTempHumidity(void) {
  // HDC1008 setup to measure both temperature and humidity in one conversion
  // this is a different way to access data in ONE read
  // this sets internal private variables that can be accessed by Get() functions

  uint32_t rt,rh ;  // working variables

  rt = read32(HDC1000_TEMP, 20);    // get temp and humidity reading together
  rh = rt;                          // save a copy for humidity processing

  float temp = (rt >> 16);        // convert to temp first
  temp /= 65536;
  temp *= 165;
  temp -= 40;

  float humidity = (rh & 0xFFFF);   // now convert to humidity
  humidity /= 65536;
  humidity *= 100;
}

float Adafruit_HDC1000::GetTemperature(void) {
  // getter function to access private temp variable

  return temp ;
}

float Adafruit_HDC1000::GetHumidity(void) {
  // getter function to access private humidity variable

  return humidity ;
}

// Add ability to test battery voltage, useful in remote monitoring, TRUE if <2.8V
// usually called after Temp/Humid reading  RMB
// Thanks to KFricke for micropython-hdc1008 example on GitHub
boolean Adafruit_HDC1000::batteryLOW(void)  {

  uint16_t battLOW = (read16(HDC1000_CONFIG_BATT, 20));

  battLOW &= HDC1000_CONFIG_BATT;   // mask off other bits, bit 11 will be 1 if voltage < 2.8V

  if (battLOW> 0) return true;
  return  false;
}

/*********************************************************************/

uint16_t Adafruit_HDC1000::read16(uint8_t a, uint8_t d) {
  Wire.beginTransmission(_i2caddr);
  Wire.write(a);
  Wire.endTransmission();
  delay(d);
  Wire.requestFrom((uint8_t)_i2caddr, (uint8_t)2);
  uint16_t r = Wire.read();
  r <<= 8;
  r |= Wire.read();
  //Serial.println(r, HEX);
  return r;
}

uint32_t Adafruit_HDC1000::read32(uint8_t a, uint8_t d) {
  Wire.beginTransmission(_i2caddr);
  Wire.write(a);
  Wire.endTransmission();
  // delay was hardcoded as 50, should use d   RMB
  delay(d);
  Wire.requestFrom((uint8_t)_i2caddr, (uint8_t)4);
  uint32_t r = Wire.read();
  // assembles temp into highest 16 bits, humidity into lowest 16 bits
  r <<= 8;
  r |= Wire.read();
  r <<= 8;
  r |= Wire.read();
  r <<= 8;
  r |= Wire.read();
  //Serial.println(r, HEX);
  return r;
}
<|endoftext|>


<commit_before>#ifndef ecmdSharedUtils_h
#define ecmdSharedUtils_h
// Copyright **********************************************************
//
// File ecmdSharedUtils.H
//
// IBM Confidential
// OCO Source Materials
// 9400 Licensed Internal Code
// (C) COPYRIGHT IBM CORP. 1996
//
// The source code for this program is not published or otherwise
// divested of its trade secrets, irrespective of what has been
// deposited with the U.S. Copyright Office.
//
// End Copyright ******************************************************

/* $Header$ */

/**
 * @file ecmdSharedUtils.H
 * @brief Useful functions for use throughout the ecmd C API and Plugin
 *
 */

//--------------------------------------------------------------------
// Includes
//--------------------------------------------------------------------
#include <string>
#include <vector>
#include <inttypes.h>

#include <ecmdDefines.H>
#include <ecmdDataBuffer.H>
#include <ecmdStructs.H>

//--------------------------------------------------------------------
// ENUM Definitions
//--------------------------------------------------------------------
/**
 @brief Used by ecmdSetTargetDepth
*/
typedef enum {
  ECMD_DEPTH_CAGE = 0,
  ECMD_DEPTH_NODE,
  ECMD_DEPTH_SLOT,
  ECMD_DEPTH_CHIP,
  ECMD_DEPTH_CHIPUNIT,
  ECMD_DEPTH_THREAD,
  ECMD_DEPTH_CORE  // Re-added on 9/27/07 by JTA.  We need both CORE and CHIPUNIT for ecmdSetTargetDepth
} ecmdTargetDepth_t;

//--------------------------------------------------------------------
//  Forward References
//--------------------------------------------------------------------
/* The following two functions are overridden in ecmdClientPerlapi.H to remove io_argc */
#ifndef ECMD_PERLAPI

/** @name Command Line Parsing Functions */
//@{

/**
 * @brief Iterates over argv, looking for given option string, removes it if found
 * @retval 1 if option found, 0 otherwise
 * @param io_argc Pointer to number of elements in io_argv array
 * @param io_argv Array of strings passed in from command line
 * @param i_option Option to look for
 * @see ecmdParseOptionWithArgs

 */
bool ecmdParseOption (int * io_argc, char ** io_argv[], const char * i_option);

/**
 * @brief Iterates over argv, looking for given option string, removes it if found
 * @retval Value of option arg if found, NULL otherwise
 * @param io_argc Pointer to number of elements in io_argv array
 * @param io_argv Array of strings passed in from command line
 * @param i_option Option to look for
 * @see ecmdParseOptionWithArgs

 */
char * ecmdParseOptionWithArgs(int * io_argc, char ** io_argv[], const char * i_option);

//@}
#endif

/**
 * @brief Breaks the string line into tokens based on all chars in seperators
 * @param line String to tokenize
 * @param seperators String of characters to use as seperators
 * @param tokens Vector of strings that contain all the tokens
*/
void ecmdParseTokens(std::string line, const char* seperators, std::vector<std::string> & tokens);

/** @name Gen Functions */
//@{

/**
 * @brief Turns the data in the buffer into ebcdic text
 * @param i_data Data to convert
 * @param start Bit to start at
 * @param bitLen Number of bits
*/
std::string ecmdGenEbcdic(ecmdDataBuffer &i_data, int start, int bitLen);

/**
 * @brief Default function for converting hex strings to unsigned int arrays
 * @retval First element of the parsed data, or 0xFFFFFFFF if error
 * @param o_numPtr The array that stores the data parsed from the input string
 * @param i_hexChars input string of hex data- alignment stuff handled by Left and Right functions
 * @param startPos
 * @see ecmdGenB32FromHexRight
 * @see ecmdGenB32FromHexLeft
 */
uint32_t ecmdGenB32FromHex(uint32_t * o_numPtr, const char * i_hexChars, int startPos);

/**
 * @brief Convert a string of left-aligned Hex chars into a left-aligned unsigned int array
 * @retval The first element of the parsed string data, or 0xFFFFFFFF if error
 * @param o_numPtr The array that stores the data parsed from the input string
 * @param i_hexChars A string of hex characters
 * @see ecmdGenB32FromHexRight
 * @see ecmdGenB32FromHex
 */
uint32_t ecmdGenB32FromHexLeft(uint32_t * o_numPtr, const char * i_hexChars);

/**
 * @brief Convert a string of right-aligned Hex chars into a left-aligned unsigned int array
 * @retval The first element of the parsed string data, or 0xFFFFFFFF if error
 * @param o_numPtr The array that stores the data parsed from the input string
 * @param i_hexChars A string of hex characters
 * @param i_expectBits The number of defined bits in the o_numPtr array returned
 * @see ecmdGenB32FromHex
 * @see ecmdGenB32FromHexLeft
 */
uint32_t ecmdGenB32FromHexRight(uint32_t * o_numPtr, const char * i_hexChars, int i_expectBits = 0);

//@}

/**
 * @brief Converts strings to unsigned int values. The input format is 0xABCDEF.
 * @param i_str String in hexadecimal notation
 * @date Tue Sep 21 13:22:33 2004
 * @retval uint32_t value of converted input string
 */
uint32_t ecmdHexToUInt32(const char* i_str);

/**
 * @brief Calculates a 32bit hash value for a given string.
 *
 * LICENSE:
 * By Bob Jenkins, 1996.  bob_jenkins@burtleburtle.net.  You may use this
 * code any way you wish, private, educational, or commercial.  It's free.
 * See http://burtleburtle.net/bob/hash/doobs.html
 *
 * @param i_str String to convert to hash
 * @param i_c Start value for hash.
 * @retval Hash value
 */
uint32_t ecmdHashString32(const char *i_str, uint32_t i_c);

/**
 *  @brief Sets State Fields of Chip Target based on depth
 *  @param io_target an ecmdChipTarget struct representing a specific eCmd target
 *  @param i_depth an ecmdTargetDepth_t enum representing depth to be set valid
 *  @retval ECMD_SUCCESS if setting successful
 *  @retval ECMD_INVALID_ARGS if unsuccessful in finding a matching depth
 *  @post State Fields of Chip Target are set to either ECMD_TARGET_FIELD VALID or ECMD_TARGET_FIELD_UNUSED

 TARGET DEPTH  : Input To This Function
 TARGET STATES : Gets Set By This Function

 */
uint32_t ecmdSetTargetDepth(ecmdChipTarget & io_target, ecmdTargetDepth_t i_depth);

/**
 *  @brief Sets Fields of ecmdMemoryEntry_t from the DCard data in the file
 *  @param i_filename file to read the d-card data from
 *  @param o_data list to be updated with the d-card data
 *  @retval ECMD_SUCCESS if setting successful
 *  @retval ECMD_INVALID_ARGS if unsuccessful in finding a matching depth
 */
uint32_t ecmdReadDcard(const char *i_filename, std::list<ecmdMemoryEntry> &o_data);

#endif /* ecmdSharedUtils_h */

// Change Log *********************************************************
//
//  Flag Reason   Vers Date     Coder  Description
//  ---- -------- ---- -------- -----  -------------------------------
//                              cengel Initial Creation
//                              kloss  added hash functions: hashString32, hexToUInt32
//                              baiocchi Added ecmdSetTargetDepth()
// End Change Log *****************************************************
<commit_msg>Fixed core/chipunit order since we have to re-release anyways<commit_after>#ifndef ecmdSharedUtils_h
#define ecmdSharedUtils_h
// Copyright **********************************************************
//
// File ecmdSharedUtils.H
//
// IBM Confidential
// OCO Source Materials
// 9400 Licensed Internal Code
// (C) COPYRIGHT IBM CORP. 1996
//
// The source code for this program is not published or otherwise
// divested of its trade secrets, irrespective of what has been
// deposited with the U.S. Copyright Office.
//
// End Copyright ******************************************************

/* $Header$ */

/**
 * @file ecmdSharedUtils.H
 * @brief Useful functions for use throughout the ecmd C API and Plugin
 *
 */

//--------------------------------------------------------------------
// Includes
//--------------------------------------------------------------------
#include <string>
#include <vector>
#include <inttypes.h>

#include <ecmdDefines.H>
#include <ecmdDataBuffer.H>
#include <ecmdStructs.H>

//--------------------------------------------------------------------
// ENUM Definitions
//--------------------------------------------------------------------
/**
 @brief Used by ecmdSetTargetDepth
*/
typedef enum {
  ECMD_DEPTH_CAGE = 0,
  ECMD_DEPTH_NODE,
  ECMD_DEPTH_SLOT,
  ECMD_DEPTH_CHIP,
  ECMD_DEPTH_CORE,  // Re-added on 9/27/07 by JTA.  We need both CORE and CHIPUNIT for ecmdSetTargetDepth
  ECMD_DEPTH_CHIPUNIT,
  ECMD_DEPTH_THREAD
} ecmdTargetDepth_t;

//--------------------------------------------------------------------
//  Forward References
//--------------------------------------------------------------------
/* The following two functions are overridden in ecmdClientPerlapi.H to remove io_argc */
#ifndef ECMD_PERLAPI

/** @name Command Line Parsing Functions */
//@{

/**
 * @brief Iterates over argv, looking for given option string, removes it if found
 * @retval 1 if option found, 0 otherwise
 * @param io_argc Pointer to number of elements in io_argv array
 * @param io_argv Array of strings passed in from command line
 * @param i_option Option to look for
 * @see ecmdParseOptionWithArgs

 */
bool ecmdParseOption (int * io_argc, char ** io_argv[], const char * i_option);

/**
 * @brief Iterates over argv, looking for given option string, removes it if found
 * @retval Value of option arg if found, NULL otherwise
 * @param io_argc Pointer to number of elements in io_argv array
 * @param io_argv Array of strings passed in from command line
 * @param i_option Option to look for
 * @see ecmdParseOptionWithArgs

 */
char * ecmdParseOptionWithArgs(int * io_argc, char ** io_argv[], const char * i_option);

//@}
#endif

/**
 * @brief Breaks the string line into tokens based on all chars in seperators
 * @param line String to tokenize
 * @param seperators String of characters to use as seperators
 * @param tokens Vector of strings that contain all the tokens
*/
void ecmdParseTokens(std::string line, const char* seperators, std::vector<std::string> & tokens);

/** @name Gen Functions */
//@{

/**
 * @brief Turns the data in the buffer into ebcdic text
 * @param i_data Data to convert
 * @param start Bit to start at
 * @param bitLen Number of bits
*/
std::string ecmdGenEbcdic(ecmdDataBuffer &i_data, int start, int bitLen);

/**
 * @brief Default function for converting hex strings to unsigned int arrays
 * @retval First element of the parsed data, or 0xFFFFFFFF if error
 * @param o_numPtr The array that stores the data parsed from the input string
 * @param i_hexChars input string of hex data- alignment stuff handled by Left and Right functions
 * @param startPos
 * @see ecmdGenB32FromHexRight
 * @see ecmdGenB32FromHexLeft
 */
uint32_t ecmdGenB32FromHex(uint32_t * o_numPtr, const char * i_hexChars, int startPos);

/**
 * @brief Convert a string of left-aligned Hex chars into a left-aligned unsigned int array
 * @retval The first element of the parsed string data, or 0xFFFFFFFF if error
 * @param o_numPtr The array that stores the data parsed from the input string
 * @param i_hexChars A string of hex characters
 * @see ecmdGenB32FromHexRight
 * @see ecmdGenB32FromHex
 */
uint32_t ecmdGenB32FromHexLeft(uint32_t * o_numPtr, const char * i_hexChars);

/**
 * @brief Convert a string of right-aligned Hex chars into a left-aligned unsigned int array
 * @retval The first element of the parsed string data, or 0xFFFFFFFF if error
 * @param o_numPtr The array that stores the data parsed from the input string
 * @param i_hexChars A string of hex characters
 * @param i_expectBits The number of defined bits in the o_numPtr array returned
 * @see ecmdGenB32FromHex
 * @see ecmdGenB32FromHexLeft
 */
uint32_t ecmdGenB32FromHexRight(uint32_t * o_numPtr, const char * i_hexChars, int i_expectBits = 0);

//@}

/**
 * @brief Converts strings to unsigned int values. The input format is 0xABCDEF.
 * @param i_str String in hexadecimal notation
 * @date Tue Sep 21 13:22:33 2004
 * @retval uint32_t value of converted input string
 */
uint32_t ecmdHexToUInt32(const char* i_str);

/**
 * @brief Calculates a 32bit hash value for a given string.
 *
 * LICENSE:
 * By Bob Jenkins, 1996.  bob_jenkins@burtleburtle.net.  You may use this
 * code any way you wish, private, educational, or commercial.  It's free.
 * See http://burtleburtle.net/bob/hash/doobs.html
 *
 * @param i_str String to convert to hash
 * @param i_c Start value for hash.
 * @retval Hash value
 */
uint32_t ecmdHashString32(const char *i_str, uint32_t i_c);

/**
 *  @brief Sets State Fields of Chip Target based on depth
 *  @param io_target an ecmdChipTarget struct representing a specific eCmd target
 *  @param i_depth an ecmdTargetDepth_t enum representing depth to be set valid
 *  @retval ECMD_SUCCESS if setting successful
 *  @retval ECMD_INVALID_ARGS if unsuccessful in finding a matching depth
 *  @post State Fields of Chip Target are set to either ECMD_TARGET_FIELD VALID or ECMD_TARGET_FIELD_UNUSED

 TARGET DEPTH  : Input To This Function
 TARGET STATES : Gets Set By This Function

 */
uint32_t ecmdSetTargetDepth(ecmdChipTarget & io_target, ecmdTargetDepth_t i_depth);

/**
 *  @brief Sets Fields of ecmdMemoryEntry_t from the DCard data in the file
 *  @param i_filename file to read the d-card data from
 *  @param o_data list to be updated with the d-card data
 *  @retval ECMD_SUCCESS if setting successful
 *  @retval ECMD_INVALID_ARGS if unsuccessful in finding a matching depth
 */
uint32_t ecmdReadDcard(const char *i_filename, std::list<ecmdMemoryEntry> &o_data);

#endif /* ecmdSharedUtils_h */

// Change Log *********************************************************
//
//  Flag Reason   Vers Date     Coder  Description
//  ---- -------- ---- -------- -----  -------------------------------
//                              cengel Initial Creation
//                              kloss  added hash functions: hashString32, hexToUInt32
//                              baiocchi Added ecmdSetTargetDepth()
// End Change Log *****************************************************
<|endoftext|>


<commit_before>#include <cmath>
#include <numeric>
#include "control_loop.h"
#include "SystemState.h"

namespace hardware {

WORKING_AREA(ControlLoop::waControlThread, 4096);
Thread * ControlLoop::tp_control_ = 0;

ControlLoop::ControlLoop()
  : startup_{false}, front_wheel_encoder_{STM32_TIM4, 800}
{
  front_wheel_encoder_.set_count(0);
  STM32_TIM5->CNT = 0;
}

// Caller: shell thread
msg_t ControlLoop::start(const char * filename)
{
  tp_control_ = chThdCreateStatic(waControlThread,
                                  sizeof(waControlThread),
                                  chThdGetPriority() + 3,
                                  ControlLoop::thread_function,
                                  const_cast<char *>(filename));
  if (!tp_control_)
    return 1;

  return 0;
}

// Caller: shell thread
msg_t ControlLoop::stop()
{
  chThdTerminate(tp_control_);
  msg_t m = chThdWait(tp_control_);
  tp_control_ = 0;
  return m;
}

// Caller: shell thread
void ControlLoop::shell_command(BaseSequentialStream *chp, int argc, char *argv[])
{
  if (argc > 1 || argc < 0) {
    chprintf(chp, "Invalid usage.\r\n");
    return;
  }

  msg_t m;
  if (tp_control_) { // control thread is running
    m = stop();
    if (argc == 0) {
      chprintf(chp, "Data collection and control terminated with %d errors.\r\n", m);
      return;
    }
  } else {           // control thread is not running
    if (argc == 0)
      m = start("samples.dat");
    else
      m = start(argv[0]);

    if (m == 0) {
      chprintf(chp, "Data collection and control initiated.\r\n");
      return;
    }
  }
  chprintf(chp, "Errors starting threads with error:  %d.\r\n", m);
}

// Caller: initially called by OS when thread is created.
// This is the highest priority thread
msg_t ControlLoop::thread_function(void * arg)
{
  chRegSetThreadName("control");
  ControlLoop loop;

  return loop.exec(static_cast<const char *>(const_cast<const void *>(arg)));
}

void ControlLoop::set_gyro_lean(Sample& s)
{
  s.has_gyro_lean = true;
  static std::array<float, 100> lean_array {{}};
  static int lean_i = 0;
  static uint32_t system_time_prev = 0;

  // first pass, use static lean value
  s.gyro_lean.startup = startup_;
  if (startup_) {
    float ax = s.mpu6050.accelerometer_x;
    float ay = s.mpu6050.accelerometer_y;
    float az = s.mpu6050.accelerometer_z;
    float accel_mag = std::sqrt(ax*ax + ay*ay + az*az);
    float lean_static = std::asin(ax / accel_mag);

    lean_array[lean_i] = lean_static;
    lean_i = (lean_i + 1)  % lean_array.size();

    // after lean_array.size() samples, set the average value
    if (lean_i == 0) {
      float lean_avg = (std::accumulate(lean_array.begin(),
                                        lean_array.end(), 0.0f) /
                        lean_array.size());
      lean_array[0] = lean_avg;
      s.gyro_lean.angle = lean_avg;
      startup_ = false;
    } else {
      s.gyro_lean.angle = lean_static;
    }
    system_time_prev = s.system_time;
    return;
  }

  // after setting average static lean, use gyro lean
  float gyro_lean;
  float dt = ((s.system_time - system_time_prev) *
              constants::system_timer_seconds_per_count);
  gyro_lean = lean_array[0] + s.mpu6050.gyroscope_y * dt;

  lean_array[0] = gyro_lean;
  s.gyro_lean.angle = gyro_lean;
  system_time_prev = s.system_time;
  return;
}

// Caller: Control thread
msg_t ControlLoop::exec(const char * file_name)
{
  logging::SampleBuffer sample_buffer(file_name);
  Sample s;
  memset(&s, 0, sizeof(s));

  systime_t time = chTimeNow();     // Initial time
  systime_t sleep_time;
  for (uint32_t i = 0; !chThdShouldTerminate(); ++i) {
    time += MS2ST(constants::loop_period_ms);       // Next deadline

    // Begin pre control data collection
    s.system_time = STM32_TIM5->CNT;
    s.loop_count = i;
    imu_.acquire_data(s);
    s.encoder.front_wheel = front_wheel_encoder_.get_angle();
    s.system_state |= systemstate::CollectionEnabled;
    set_gyro_lean(s);
    // End pre control data collection

    // Begin control
    rear_motor_controller_.update(s); // must be called prior to fork update since
    fork_motor_controller_.update(s); // rear wheel rate is needed for gain scheduling
    // End control

    // Put the sample in to the buffer
    bool encode_failure = !sample_buffer.insert(s);

    // Illuminate the lean and steer LED's based on latest sample
    illuminate_lean_steer(s);

    // Clear the sample for the next iteration
    // The first time through the loop, computation_time will be logged as zero,
    // subsequent times will be accurate but delayed by one sample period. This
    // is done to ensure that encoding computation is part of timing
    // measurement.
    uint32_t ti = s.system_time;
    memset(&s, 0, sizeof(s));
    s.computation_time = STM32_TIM5->CNT - ti;
    // Similarly, encode failures will be delayed by one sample.
    if (encode_failure)
      s.system_state |= systemstate::SampleBufferEncodeError;

    // set hardware button set
    if (hw_button_enabled())
      s.system_state |= systemstate::HWButton;

    // Go to sleep until next interval
    chSysLock();
    sleep_time = time - chTimeNow();
    if (static_cast<int32_t>(sleep_time) > 0)
      chThdSleepS(sleep_time);
    chSysUnlock();
  } // for

  return sample_buffer.flush_and_close();
}

}
<commit_msg>Fix startup_ initial value.<commit_after>#include <cmath>
#include <numeric>
#include "control_loop.h"
#include "SystemState.h"

namespace hardware {

WORKING_AREA(ControlLoop::waControlThread, 4096);
Thread * ControlLoop::tp_control_ = 0;

ControlLoop::ControlLoop()
  : startup_{true}, front_wheel_encoder_{STM32_TIM4, 800}
{
  front_wheel_encoder_.set_count(0);
  STM32_TIM5->CNT = 0;
}

// Caller: shell thread
msg_t ControlLoop::start(const char * filename)
{
  tp_control_ = chThdCreateStatic(waControlThread,
                                  sizeof(waControlThread),
                                  chThdGetPriority() + 3,
                                  ControlLoop::thread_function,
                                  const_cast<char *>(filename));
  if (!tp_control_)
    return 1;

  return 0;
}

// Caller: shell thread
msg_t ControlLoop::stop()
{
  chThdTerminate(tp_control_);
  msg_t m = chThdWait(tp_control_);
  tp_control_ = 0;
  return m;
}

// Caller: shell thread
void ControlLoop::shell_command(BaseSequentialStream *chp, int argc, char *argv[])
{
  if (argc > 1 || argc < 0) {
    chprintf(chp, "Invalid usage.\r\n");
    return;
  }

  msg_t m;
  if (tp_control_) { // control thread is running
    m = stop();
    if (argc == 0) {
      chprintf(chp, "Data collection and control terminated with %d errors.\r\n", m);
      return;
    }
  } else {           // control thread is not running
    if (argc == 0)
      m = start("samples.dat");
    else
      m = start(argv[0]);

    if (m == 0) {
      chprintf(chp, "Data collection and control initiated.\r\n");
      return;
    }
  }
  chprintf(chp, "Errors starting threads with error:  %d.\r\n", m);
}

// Caller: initially called by OS when thread is created.
// This is the highest priority thread
msg_t ControlLoop::thread_function(void * arg)
{
  chRegSetThreadName("control");
  ControlLoop loop;

  return loop.exec(static_cast<const char *>(const_cast<const void *>(arg)));
}

void ControlLoop::set_gyro_lean(Sample& s)
{
  s.has_gyro_lean = true;
  static std::array<float, 100> lean_array {{}};
  static int lean_i = 0;
  static uint32_t system_time_prev = 0;

  // first pass, use static lean value
  s.gyro_lean.startup = startup_;
  if (startup_) {
    float ax = s.mpu6050.accelerometer_x;
    float ay = s.mpu6050.accelerometer_y;
    float az = s.mpu6050.accelerometer_z;
    float accel_mag = std::sqrt(ax*ax + ay*ay + az*az);
    float lean_static = std::asin(ax / accel_mag);

    lean_array[lean_i] = lean_static;
    lean_i = (lean_i + 1)  % lean_array.size();

    // after lean_array.size() samples, set the average value
    if (lean_i == 0) {
      float lean_avg = (std::accumulate(lean_array.begin(),
                                        lean_array.end(), 0.0f) /
                        lean_array.size());
      lean_array[0] = lean_avg;
      s.gyro_lean.angle = lean_avg;
      startup_ = false;
    } else {
      s.gyro_lean.angle = lean_static;
    }
    system_time_prev = s.system_time;
    return;
  }

  // after setting average static lean, use gyro lean
  float gyro_lean;
  float dt = ((s.system_time - system_time_prev) *
              constants::system_timer_seconds_per_count);
  gyro_lean = lean_array[0] + s.mpu6050.gyroscope_y * dt;

  lean_array[0] = gyro_lean;
  s.gyro_lean.angle = gyro_lean;
  system_time_prev = s.system_time;
  return;
}

// Caller: Control thread
msg_t ControlLoop::exec(const char * file_name)
{
  logging::SampleBuffer sample_buffer(file_name);
  Sample s;
  memset(&s, 0, sizeof(s));

  systime_t time = chTimeNow();     // Initial time
  systime_t sleep_time;
  for (uint32_t i = 0; !chThdShouldTerminate(); ++i) {
    time += MS2ST(constants::loop_period_ms);       // Next deadline

    // Begin pre control data collection
    s.system_time = STM32_TIM5->CNT;
    s.loop_count = i;
    imu_.acquire_data(s);
    s.encoder.front_wheel = front_wheel_encoder_.get_angle();
    s.system_state |= systemstate::CollectionEnabled;
    set_gyro_lean(s);
    // End pre control data collection

    // Begin control
    rear_motor_controller_.update(s); // must be called prior to fork update since
    fork_motor_controller_.update(s); // rear wheel rate is needed for gain scheduling
    // End control

    // Put the sample in to the buffer
    bool encode_failure = !sample_buffer.insert(s);

    // Illuminate the lean and steer LED's based on latest sample
    illuminate_lean_steer(s);

    // Clear the sample for the next iteration
    // The first time through the loop, computation_time will be logged as zero,
    // subsequent times will be accurate but delayed by one sample period. This
    // is done to ensure that encoding computation is part of timing
    // measurement.
    uint32_t ti = s.system_time;
    memset(&s, 0, sizeof(s));
    s.computation_time = STM32_TIM5->CNT - ti;
    // Similarly, encode failures will be delayed by one sample.
    if (encode_failure)
      s.system_state |= systemstate::SampleBufferEncodeError;

    // set hardware button set
    if (hw_button_enabled())
      s.system_state |= systemstate::HWButton;

    // Go to sleep until next interval
    chSysLock();
    sleep_time = time - chTimeNow();
    if (static_cast<int32_t>(sleep_time) > 0)
      chThdSleepS(sleep_time);
    chSysUnlock();
  } // for

  return sample_buffer.flush_and_close();
}

}
<|endoftext|>


<commit_before>/*
 *  ______                   __  __              __
 * /\  _  \           __    /\ \/\ \            /\ \__
 * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
 *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
 *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
 *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
 *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
 * @copyright Copyright 2017 Avidbots Corp.
 * @name	 world.cpp
 * @brief	 Loads world file
 * @author Joseph Duchesne
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Avidbots Corp.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *      copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Avidbots Corp. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#include <Box2D/Box2D.h>
#include <flatland_server/debug_visualization.h>
#include <flatland_server/exceptions.h>
#include <flatland_server/world.h>
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <boost/filesystem.hpp>
#include <map>
#include <string>

namespace flatland_server {

World::World() : gravity_(0, 0) { physics_world_ = new b2World(gravity_); }

World::~World() {
  for (int i = 0; i < layers_.size(); i++) {
    delete layers_[i];
  }

  delete physics_world_;
}

World *World::MakeWorld(const std::string &yaml_path) {
  // parse the world YAML file
  YAML::Node yaml;

  try {
    yaml = YAML::LoadFile(yaml_path);
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + yaml_path, e);
  }

  if (yaml["properties"] && yaml["properties"].IsMap()) {
    // TODO (Chunshang): parse properties
  } else {
    throw YAMLException("Missing/invalid world param \"properties\"");
  }

  World *w = new World();

  try {
    w->LoadLayers(yaml_path);
    w->LoadModels(yaml_path);
  } catch (const YAML::Exception &e) {
    throw YAMLException(e);
  }

  return w;
}

void World::LoadLayers(const std::string &yaml_path) {
  boost::filesystem::path path(yaml_path);

  YAML::Node yaml;

  try {
    yaml = YAML::LoadFile(path.string());
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + path.string(), e);
  }

  if (!yaml["layers"] || !yaml["layers"].IsSequence()) {
    throw YAMLException("Missing/invalid world param \"layers\"");
  }

  // loop through each layer and parse the data
  for (int i = 0; i < yaml["layers"].size(); i++) {
    Layer *layer;

    if (cfr_.IsLayersFull()) {
      throw YAMLException("Max number of layers reached, max is " +
                          cfr_.MAX_LAYERS);
    }

    layer = Layer::MakeLayer(physics_world_, &cfr_, path.parent_path(),
                             yaml["layers"][i]);

    layers_.push_back(layer);
    ROS_INFO_NAMED("Layer", "Layer %s loaded", layer->name_.c_str());
  }
}

void World::LoadModels(const std::string &yaml_path) {

  boost::filesystem::path path(yaml_path);
  YAML::Node yaml;

  try {
    yaml = YAML::LoadFile(path.string());
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + path.string(), e);
  }

  // models is optional
  if (yaml["models"] && !yaml["models"].IsSequence()) {
    throw YAMLException("Invalid world param \"layers\", must be a sequence");
  } else if (yaml["models"]) {
    for (int i = 0; i < yaml["models"].size(); i++) {
      boost::filesystem::path model_path(yaml["models"][i].as<std::string>());

      if (model_path.string().front() != '/') {
        model_path = path.parent_path() / model_path;
      }

      LoadModel(model_path);
    }
  }

}

void World::LoadModel(const boost::filesystem::path &model_yaml_path) {
  YAML::Node n;

  try {
    n = YAML::LoadFile(model_yaml_path.string());
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + model_yaml_path.string(), e);
  }

  Model *model = Model::MakeModel(physics_world_, &cfr_, model_yaml_path, n);

  models_.push_back(model);
}

void World::DebugVisualize() {
  for (auto &layer : layers_) {
    DebugVisualization::get().Visualize(
        std::string("layer_") + layer->name_, layer->body_->physics_body_,
        layer->body_->color_[0], layer->body_->color_[1],
        layer->body_->color_[2], layer->body_->color_[3]);
  }
}

};  // namespace flatland_server
<commit_msg>clang format<commit_after>/*
 *  ______                   __  __              __
 * /\  _  \           __    /\ \/\ \            /\ \__
 * \ \ \L\ \  __  __ /\_\   \_\ \ \ \____    ___\ \ ,_\   ____
 *  \ \  __ \/\ \/\ \\/\ \  /'_` \ \ '__`\  / __`\ \ \/  /',__\
 *   \ \ \/\ \ \ \_/ |\ \ \/\ \L\ \ \ \L\ \/\ \L\ \ \ \_/\__, `\
 *    \ \_\ \_\ \___/  \ \_\ \___,_\ \_,__/\ \____/\ \__\/\____/
 *     \/_/\/_/\/__/    \/_/\/__,_ /\/___/  \/___/  \/__/\/___/
 * @copyright Copyright 2017 Avidbots Corp.
 * @name	 world.cpp
 * @brief	 Loads world file
 * @author Joseph Duchesne
 *
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2017, Avidbots Corp.
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *      copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of the Avidbots Corp. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 */

#include <Box2D/Box2D.h>
#include <flatland_server/debug_visualization.h>
#include <flatland_server/exceptions.h>
#include <flatland_server/world.h>
#include <ros/ros.h>
#include <yaml-cpp/yaml.h>
#include <boost/filesystem.hpp>
#include <map>
#include <string>

namespace flatland_server {

World::World() : gravity_(0, 0) { physics_world_ = new b2World(gravity_); }

World::~World() {
  for (int i = 0; i < layers_.size(); i++) {
    delete layers_[i];
  }

  delete physics_world_;
}

World *World::MakeWorld(const std::string &yaml_path) {
  // parse the world YAML file
  YAML::Node yaml;

  try {
    yaml = YAML::LoadFile(yaml_path);
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + yaml_path, e);
  }

  if (yaml["properties"] && yaml["properties"].IsMap()) {
    // TODO (Chunshang): parse properties
  } else {
    throw YAMLException("Missing/invalid world param \"properties\"");
  }

  World *w = new World();

  try {
    w->LoadLayers(yaml_path);
    w->LoadModels(yaml_path);
  } catch (const YAML::Exception &e) {
    throw YAMLException(e);
  }

  return w;
}

void World::LoadLayers(const std::string &yaml_path) {
  boost::filesystem::path path(yaml_path);

  YAML::Node yaml;

  try {
    yaml = YAML::LoadFile(path.string());
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + path.string(), e);
  }

  if (!yaml["layers"] || !yaml["layers"].IsSequence()) {
    throw YAMLException("Missing/invalid world param \"layers\"");
  }

  // loop through each layer and parse the data
  for (int i = 0; i < yaml["layers"].size(); i++) {
    Layer *layer;

    if (cfr_.IsLayersFull()) {
      throw YAMLException("Max number of layers reached, max is " +
                          cfr_.MAX_LAYERS);
    }

    layer = Layer::MakeLayer(physics_world_, &cfr_, path.parent_path(),
                             yaml["layers"][i]);

    layers_.push_back(layer);
    ROS_INFO_NAMED("Layer", "Layer %s loaded", layer->name_.c_str());
  }
}

void World::LoadModels(const std::string &yaml_path) {
  boost::filesystem::path path(yaml_path);
  YAML::Node yaml;

  try {
    yaml = YAML::LoadFile(path.string());
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + path.string(), e);
  }

  // models is optional
  if (yaml["models"] && !yaml["models"].IsSequence()) {
    throw YAMLException("Invalid world param \"layers\", must be a sequence");
  } else if (yaml["models"]) {
    for (int i = 0; i < yaml["models"].size(); i++) {
      boost::filesystem::path model_path(yaml["models"][i].as<std::string>());

      if (model_path.string().front() != '/') {
        model_path = path.parent_path() / model_path;
      }

      LoadModel(model_path);
    }
  }
}

void World::LoadModel(const boost::filesystem::path &model_yaml_path) {
  YAML::Node n;

  try {
    n = YAML::LoadFile(model_yaml_path.string());
  } catch (const YAML::Exception &e) {
    throw YAMLException("Error loading " + model_yaml_path.string(), e);
  }

  Model *model = Model::MakeModel(physics_world_, &cfr_, model_yaml_path, n);

  models_.push_back(model);
}

void World::DebugVisualize() {
  for (auto &layer : layers_) {
    DebugVisualization::get().Visualize(
        std::string("layer_") + layer->name_, layer->body_->physics_body_,
        layer->body_->color_[0], layer->body_->color_[1],
        layer->body_->color_[2], layer->body_->color_[3]);
  }
}

};  // namespace flatland_server
<|endoftext|>


<commit_before>/*
 * The Apache Software License, Version 1.1
 *
 * Copyright (c) 1999-2002 The Apache Software Foundation.  All rights
 * reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xerces" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache\@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation, and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com .  For more information
 * on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 */

/*
 * $Id$
 */

// ---------------------------------------------------------------------------
//  Includes
// ---------------------------------------------------------------------------
#include <xercesc/sax/SAXParseException.hpp>
#include "DOMTreeErrorReporter.hpp"
#include <iostream.h>
#include <stdlib.h>
#include <string.h>

void DOMTreeErrorReporter::warning(const SAXParseException&)
{
    //
    // Ignore all warnings.
    //
}

void DOMTreeErrorReporter::error(const SAXParseException& toCatch)
{
    fSawErrors = true;
    cerr << "Error at file \"" << StrX(toCatch.getSystemId())
		 << "\", line " << toCatch.getLineNumber()
		 << ", column " << toCatch.getColumnNumber()
         << "\n   Message: " << StrX(toCatch.getMessage()) << endl;
}

void DOMTreeErrorReporter::fatalError(const SAXParseException& toCatch)
{
    fSawErrors = true;
    cerr << "Fatal Error at file \"" << StrX(toCatch.getSystemId())
		 << "\", line " << toCatch.getLineNumber()
		 << ", column " << toCatch.getColumnNumber()
         << "\n   Message: " << StrX(toCatch.getMessage()) << endl;
}

void DOMTreeErrorReporter::resetErrors()
{
    // No-op in this case
}

<commit_msg>DOMPrint minor update: reset fSawErrors in DOMTreeErrorReporter::resetErrors<commit_after>/*
 * The Apache Software License, Version 1.1
 *
 * Copyright (c) 1999-2002 The Apache Software Foundation.  All rights
 * reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in
 *    the documentation and/or other materials provided with the
 *    distribution.
 *
 * 3. The end-user documentation included with the redistribution,
 *    if any, must include the following acknowledgment:
 *       "This product includes software developed by the
 *        Apache Software Foundation (http://www.apache.org/)."
 *    Alternately, this acknowledgment may appear in the software itself,
 *    if and wherever such third-party acknowledgments normally appear.
 *
 * 4. The names "Xerces" and "Apache Software Foundation" must
 *    not be used to endorse or promote products derived from this
 *    software without prior written permission. For written
 *    permission, please contact apache\@apache.org.
 *
 * 5. Products derived from this software may not be called "Apache",
 *    nor may "Apache" appear in their name, without prior written
 *    permission of the Apache Software Foundation.
 *
 * THIS SOFTWARE IS PROVIDED ``AS IS'' AND ANY EXPRESSED OR IMPLIED
 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES
 * OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED.  IN NO EVENT SHALL THE APACHE SOFTWARE FOUNDATION OR
 * ITS CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
 * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
 * OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 * ====================================================================
 *
 * This software consists of voluntary contributions made by many
 * individuals on behalf of the Apache Software Foundation, and was
 * originally based on software copyright (c) 1999, International
 * Business Machines, Inc., http://www.ibm.com .  For more information
 * on the Apache Software Foundation, please see
 * <http://www.apache.org/>.
 */

/*
 * $Id$
 */

// ---------------------------------------------------------------------------
//  Includes
// ---------------------------------------------------------------------------
#include <xercesc/sax/SAXParseException.hpp>
#include "DOMTreeErrorReporter.hpp"
#include <iostream.h>
#include <stdlib.h>
#include <string.h>

void DOMTreeErrorReporter::warning(const SAXParseException&)
{
    //
    // Ignore all warnings.
    //
}

void DOMTreeErrorReporter::error(const SAXParseException& toCatch)
{
    fSawErrors = true;
    cerr << "Error at file \"" << StrX(toCatch.getSystemId())
		 << "\", line " << toCatch.getLineNumber()
		 << ", column " << toCatch.getColumnNumber()
         << "\n   Message: " << StrX(toCatch.getMessage()) << endl;
}

void DOMTreeErrorReporter::fatalError(const SAXParseException& toCatch)
{
    fSawErrors = true;
    cerr << "Fatal Error at file \"" << StrX(toCatch.getSystemId())
		 << "\", line " << toCatch.getLineNumber()
		 << ", column " << toCatch.getColumnNumber()
         << "\n   Message: " << StrX(toCatch.getMessage()) << endl;
}

void DOMTreeErrorReporter::resetErrors()
{
    fSawErrors = false;
}

<|endoftext|>