Allowed sharing of common C++ code between RoboRIO and Simulation.

Change-Id: I8bf2bda9df389c13ae0567a62dbf0ca931ceb6f8

wpilibc/wpilibC++Devices/src/Utility.cpp

@@ -0,0 +1,270 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
+/* Open Source Software - may be modified and shared by FRC teams. The code   */
+/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
+/*----------------------------------------------------------------------------*/
+
+#include "Utility.h"
+
+//#include "NetworkCommunication/FRCComm.h"
+#include "HAL/HAL.hpp"
+#include "Task.h"
+#include <iostream>
+#include <sstream>
+#include <cstdio>
+#include <cstdlib>
+#include <cstring>
+#include <execinfo.h>
+#include <cxxabi.h>
+#include "nivision.h"
+
+static bool suspendOnAssertEnabled = false;
+
+/**
+ * Enable suspend on asssert.
+ * If enabled, the user task will be suspended whenever an assert fails. This
+ * will allow the user to attach to the task with the debugger and examine variables
+ * around the failure.
+ */
+void wpi_suspendOnAssertEnabled(bool enabled)
+{
+	suspendOnAssertEnabled = enabled;
+}
+
+/**
+ * Assert implementation.
+ * This allows breakpoints to be set on an assert.
+ * The users don't call this, but instead use the wpi_assert macros in Utility.h.
+ */
+bool wpi_assert_impl(bool conditionValue,
+                     const char *conditionText,
+                     const char *message,
+                     const char *fileName,
+                     uint32_t lineNumber,
+                     const char *funcName)
+{
+	if(!conditionValue)
+	{
+		std::stringstream errorStream;
+
+		errorStream << "Assertion \"" << conditionText << "\" ";
+		errorStream << "on line " << lineNumber << " ";
+		errorStream << "of "<< basename(fileName) << " ";
+
+		if(message)
+		{
+			errorStream << "failed: " << message << std::endl;
+		}
+		else
+		{
+			errorStream << "failed." << std::endl;
+		}
+
+		errorStream << GetStackTrace(2);
+
+		std::string error = errorStream.str();
+
+		// Print the error and send it to the DriverStation
+		std::cout << error << std::endl;
+		HALSetErrorData(error.c_str(), error.size(), 100);
+
+		if (suspendOnAssertEnabled) suspendTask(0);
+	}
+
+	return conditionValue;
+}
+
+/**
+ * Common error routines for wpi_assertEqual_impl and wpi_assertNotEqual_impl
+ * This should not be called directly; it should only be used by wpi_assertEqual_impl
+ * and wpi_assertNotEqual_impl.
+ */
+void wpi_assertEqual_common_impl(const char *valueA,
+                                 const char *valueB,
+                                 const char *equalityType,
+                                 const char *message,
+                                 const char *fileName,
+                                 uint32_t lineNumber,
+                                 const char *funcName)
+{
+	std::stringstream errorStream;
+
+	errorStream << "Assertion \"" << valueA << " " << equalityType << " " << valueB << "\" ";
+	errorStream << "on line " << lineNumber << " ";
+	errorStream << "of "<< basename(fileName) << " ";
+
+	if(message)
+	{
+		errorStream << "failed: " << message << std::endl;
+	}
+	else
+	{
+		errorStream << "failed." << std::endl;
+	}
+
+	errorStream << GetStackTrace(3);
+
+	std::string error = errorStream.str();
+
+	// Print the error and send it to the DriverStation
+	std::cout << error << std::endl;
+	HALSetErrorData(error.c_str(), error.size(), 100);
+
+	if (suspendOnAssertEnabled) suspendTask(0);
+}
+
+/**
+ * Assert equal implementation.
+ * This determines whether the two given integers are equal. If not,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertEqual macros in Utility.h.
+ */
+bool wpi_assertEqual_impl(int valueA,
+                          int valueB,
+                          const char *valueAString,
+                          const char *valueBString,
+                          const char *message,
+                          const char *fileName,
+                          uint32_t lineNumber,
+                          const char *funcName)
+{
+	if(!(valueA == valueB))
+	{
+		wpi_assertEqual_common_impl(valueAString, valueBString,
+			"==", message, fileName, lineNumber, funcName);
+	}
+	return valueA == valueB;
+}
+
+/**
+ * Assert not equal implementation.
+ * This determines whether the two given integers are equal. If so,
+ * the value of each is printed along with an optional message string.
+ * The users don't call this, but instead use the wpi_assertNotEqual macros in Utility.h.
+ */
+bool wpi_assertNotEqual_impl(int valueA,
+                             int valueB,
+                             const char *valueAString,
+                             const char *valueBString,
+                             const char *message,
+                             const char *fileName,
+                             uint32_t lineNumber,
+                             const char *funcName)
+{
+	if(!(valueA != valueB))
+	{
+		wpi_assertEqual_common_impl(valueAString, valueBString,
+			"!=", message, fileName, lineNumber, funcName);
+	}
+	return valueA != valueB;
+}
+
+
+/**
+ * Return the FPGA Version number.
+ * For now, expect this to be competition year.
+ * @return FPGA Version number.
+ */
+uint16_t GetFPGAVersion()
+{
+	int32_t status = 0;
+	uint16_t version = getFPGAVersion(&status);
+	wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+	return version;
+}
+
+/**
+ * Return the FPGA Revision number.
+ * The format of the revision is 3 numbers.
+ * The 12 most significant bits are the Major Revision.
+ * the next 8 bits are the Minor Revision.
+ * The 12 least significant bits are the Build Number.
+ * @return FPGA Revision number.
+ */
+uint32_t GetFPGARevision()
+{
+	int32_t status = 0;
+	uint32_t revision = getFPGARevision(&status);
+	wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+	return revision;
+}
+
+/**
+ * Read the microsecond-resolution timer on the FPGA.
+ *
+ * @return The current time in microseconds according to the FPGA (since FPGA reset).
+ */
+uint32_t GetFPGATime()
+{
+	int32_t status = 0;
+	uint32_t time = getFPGATime(&status);
+	wpi_setGlobalErrorWithContext(status, getHALErrorMessage(status));
+	return time;
+}
+
+/**
+ * Get the state of the "USER" button on the RoboRIO
+ * @return true if the button is currently pressed down
+ */
+bool GetUserButton()
+{
+	int32_t status = 0;
+
+	bool value = getFPGAButton(&status);
+	wpi_setGlobalError(status);
+
+	return value;
+}
+
+/**
+ * Demangle a C++ symbol, used for printing stack traces.
+ */
+static std::string demangle(char const *mangledSymbol)
+{
+	char buffer[256];
+	size_t length;
+	int status;
+
+	if(sscanf(mangledSymbol, "%*[^(]%*[^_]%255[^)+]", buffer))
+	{
+		char *symbol = abi::__cxa_demangle(buffer, NULL, &length, &status);
+
+		if(status == 0)
+		{
+			return symbol;
+		}
+		else
+		{
+			// If the symbol couldn't be demangled, it's probably a C function,
+			// so just return it as-is.
+			return buffer;
+		}
+	}
+
+	// If everything else failed, just return the mangled symbol
+	return mangledSymbol;
+}
+
+/**
+ * Get a stack trace, ignoring the first "offset" symbols.
+ */
+std::string GetStackTrace(uint32_t offset)
+{
+	void *stackTrace[128];
+	int stackSize = backtrace(stackTrace, 128);
+	char **mangledSymbols = backtrace_symbols(stackTrace, stackSize);
+	std::stringstream trace;
+
+	for(int i = offset; i < stackSize; i++)
+	{
+		// Only print recursive functions once in a row.
+		if(i == 0 ||stackTrace[i] != stackTrace[i - 1])
+		{
+			trace << "\tat " << demangle(mangledSymbols[i]) << std::endl;
+		}
+	}
+
+	free(mangledSymbols);
+
+	return trace.str();
+}