copied everything over from 2012 and removed all of the actual robot code except the drivetrain stuff
git-svn-id: https://robotics.mvla.net/svn/frc971/2013/trunk/src@4078 f308d9b7-e957-4cde-b6ac-9a88185e7312
diff --git a/aos/crio/README.txt b/aos/crio/README.txt
new file mode 100644
index 0000000..182b323
--- /dev/null
+++ b/aos/crio/README.txt
@@ -0,0 +1,5 @@
+see ../README.txt for stuff affecting crio and atom code
+
+[NOTES]
+The assumption that sizeof(pointers) == sizeof(int) == sizeof(UINT32) == sizeof(uint32_t) == 4 is all over the crio code. The vxworks apis use UINT32 to pass user-defined arguments, and just passing pointers through those makes the code a lot simpler.
+
diff --git a/aos/crio/Talon.cpp b/aos/crio/Talon.cpp
new file mode 100644
index 0000000..54ef459
--- /dev/null
+++ b/aos/crio/Talon.cpp
@@ -0,0 +1,21 @@
+#include "aos/crio/Talon.h"
+#include "WPILib/DigitalModule.h"
+
+Talon::Talon(UINT32 channel) : SafePWM(channel) {
+ // 255 = 2.5ms, 0 = 0.5ms (or something close to that)
+ // these numbers were determined empirically with real hardware by Brian
+ // on 11/23/12
+ // got 211->210 as first transition that made a speed difference and
+ // 53->54 on the other side
+ // going 2 to each side to make sure we get the full range
+ SetBounds(213, 137, 132, 127, 50);
+ // 1X = every 5.05ms, 2X and 4x are multiples of that
+ SetPeriodMultiplier(kPeriodMultiplier_1X);
+ SetRaw(m_centerPwm);
+}
+
+void Talon::Set(float speed, UINT8 /*syncGroup*/) { SetSpeed(speed); }
+float Talon::Get() { return GetSpeed(); }
+void Talon::Disable() { SetRaw(kPwmDisabled); }
+
+void Talon::PIDWrite(float output) { Set(output); }
diff --git a/aos/crio/Talon.h b/aos/crio/Talon.h
new file mode 100644
index 0000000..c116a4d
--- /dev/null
+++ b/aos/crio/Talon.h
@@ -0,0 +1,21 @@
+#ifndef AOS_CRIO_TALON_H_
+#define AOS_CRIO_TALON_H_
+
+#include "WPILib/SafePWM.h"
+#include "WPILib/SpeedController.h"
+#include "WPILib/PIDOutput.h"
+
+// Used for controlling a Talon speed controller. Non-standard API and
+// namespace so that the likely WPILib version will be drop-in replaceable.
+class Talon : public SafePWM, public SpeedController, public PIDOutput {
+ public:
+ explicit Talon(UINT32 channel);
+
+ virtual void Set(float value, UINT8 syncGroup=0);
+ virtual float Get();
+ virtual void Disable();
+
+ virtual void PIDWrite(float output);
+};
+
+#endif // AOS_CRIO_TALON_H_
diff --git a/aos/crio/aos_ctdt.h b/aos/crio/aos_ctdt.h
new file mode 100644
index 0000000..9292823
--- /dev/null
+++ b/aos/crio/aos_ctdt.h
@@ -0,0 +1,17 @@
+#ifndef AOS_CTDT_H_
+#define AOS_CTDT_H_
+
+// This function will call any function that starts with aos_init_function_*.
+// It will assume that these functions have the signature
+// 'extern "C" aos_init_function_whatever(void);'
+// The aos_ctdt.c/o files are generated at compile time (like ctdt.c/o).
+#ifdef __cplusplus
+extern "C" {
+#endif
+void aos_call_init_functions();
+#ifdef __cplusplus
+}
+#endif
+
+#endif
+
diff --git a/aos/crio/bin/netconsole.sh b/aos/crio/bin/netconsole.sh
new file mode 100755
index 0000000..8976400
--- /dev/null
+++ b/aos/crio/bin/netconsole.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+socat UDP4-RECV:6666,reuseaddr!!UDP4-SENDTO:robot:6668 READLINE
diff --git a/aos/crio/controls/ControlsManager.cpp b/aos/crio/controls/ControlsManager.cpp
new file mode 100644
index 0000000..a37ec6f
--- /dev/null
+++ b/aos/crio/controls/ControlsManager.cpp
@@ -0,0 +1,53 @@
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "WPILib/Compressor.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/controls/ControlsManager.h"
+#include "aos/common/Configuration.h"
+#include "aos/crio/aos_ctdt.h"
+#include "aos/crio/motor_server/CRIOControlLoopRunner.h"
+#include "aos/crio/motor_server/MotorServer.h"
+
+namespace aos {
+namespace crio {
+
+// Everything gets an explicit Start call here before calling all of the init
+// functions because it means that all static variables will be initialized
+// before anything actually starts running. It also means that everything will
+// be initialized before any of the init functions start trying to register
+// themselves etc.
+void ControlsManager::StartCompetition() {
+ printf("aos::ControlsManager::RobotMain\n");
+ (new Compressor(14, 1))->Start();
+
+ logging::Start();
+ LOG(INFO, "logging started\n");
+
+ GetWatchdog().SetEnabled(false);
+ LOG(INFO, "disabled watchdog\n");
+
+ RegisterControlLoops();
+ LOG(INFO, "registered control loops\n");
+
+ // CRIOControlLoopRunner calls part of MotorServer, so MotorServer has to get
+ // initialized first.
+ MotorServer::Start();
+ LOG(INFO, "MotorServer started\n");
+ CRIOControlLoopRunner::Start();
+ LOG(INFO, "cRIO control loops started\n");
+
+ LOG(INFO, "calling init functions\n");
+ aos_call_init_functions();
+ LOG(INFO, "initialized\n");
+
+ // Wait forever so that this task doesn't end to avoid confusing any brittle
+ // FIRST code that might be hiding somewhere.
+ while (true) {
+ select(0, NULL, NULL, NULL, NULL);
+ }
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/controls/ControlsManager.h b/aos/crio/controls/ControlsManager.h
new file mode 100644
index 0000000..986fe02
--- /dev/null
+++ b/aos/crio/controls/ControlsManager.h
@@ -0,0 +1,21 @@
+#include "WPILib/DriverStation.h"
+#include "WPILib/RobotBase.h"
+
+namespace aos {
+namespace crio {
+
+class ControlsManager : public RobotBase {
+ public:
+ // Gets called when it is time to register all the control loops.
+ virtual void RegisterControlLoops() = 0;
+ virtual void StartCompetition();
+ static inline ControlsManager &GetInstance() {
+ return *static_cast<ControlsManager *>(&RobotBase::getInstance());
+ }
+ inline DriverStation *GetDS() {
+ return m_ds;
+ }
+};
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/controls/JoyStickRead.cpp b/aos/crio/controls/JoyStickRead.cpp
new file mode 100644
index 0000000..1f5e4c7
--- /dev/null
+++ b/aos/crio/controls/JoyStickRead.cpp
@@ -0,0 +1,91 @@
+#include "WPILib/Task.h"
+#include "WPILib/Timer.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/controls/ControlsManager.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/messages/RobotState.q.h"
+
+namespace aos {
+namespace crio {
+
+class JoystickRead {
+ /*virtual void Disabled () {
+ int i = 0;
+ while (IsDisabled()) {
+ printf("Disabled! %d\n", i);
+ Wait(0.1);
+ i++;
+ }
+ printf("Done with disabled. %d\n", i);
+ }
+ virtual void Autonomous () {
+ int j = 0;
+ while (IsAutonomous()) {
+ printf("Autonomous! %d\n", j);
+ Wait(0.1);
+ //if (j > 5) {
+ //i(0);
+ //}
+ j ++;
+ }
+ printf("Done with autonomous. %d\n", j);
+ }
+ virtual void OperatorControl () {
+ int i = 0;
+ while (IsOperatorControl()) {
+ printf("Operator Control! %d\n", i);
+ Wait(0.1);
+ i ++;
+ }
+ printf("Done with operator control. %d\n", i);
+ }*/
+ public:
+ DriverStation *ds;
+ JoystickRead() {}
+ void Run() {
+ SendSocket sock(NetworkPort::kDS,
+ configuration::GetIPAddress(
+ configuration::NetworkDevice::kAtom));
+ FRCCommonControlData data;
+
+ ds = ControlsManager::GetInstance().GetDS();
+
+ while (true) {
+ // I checked, and this is done intelligently in WPILib.
+ ds->WaitForData();
+
+ robot_state.MakeWithBuilder().enabled(ds->IsEnabled())
+ .autonomous(ds->IsAutonomous()).team_id(ds->GetTeamNumber()).Send();
+ LOG(DEBUG, "sending joystick data\n");
+ data.enabled = ds->IsEnabled();
+ data.autonomous = ds->IsAutonomous();
+ data.fmsAttached = ds->IsFMSAttached();
+ SetStick(data.stick0Axes, 1);
+ SetStick(data.stick1Axes, 2);
+ SetStick(data.stick2Axes, 3);
+ SetStick(data.stick3Axes, 4);
+ data.stick0Buttons = ds->GetStickButtons(1);
+ data.stick1Buttons = ds->GetStickButtons(2);
+ data.stick2Buttons = ds->GetStickButtons(3);
+ data.stick3Buttons = ds->GetStickButtons(4);
+ data.teamID = ds->GetTeamNumber();
+ sock.Send(&data, sizeof(data));
+ }
+ }
+ void SetStick(int8_t axes[6], uint32_t stick) {
+ for (int i = 0; i < 6; ++i) {
+ double val = ds->GetStickAxis(stick, i + 1);
+ if (val < 0) {
+ axes[i] = (val * 128.0) + 0.5;
+ } else {
+ axes[i] = (val * 127.0) + 0.5;
+ }
+ }
+ }
+};
+
+} // namespace crio
+} // namespace aos
+
+AOS_RUN_FORK(aos::crio::JoystickRead, "JSR", 100)
diff --git a/aos/crio/crio.gyp b/aos/crio/crio.gyp
new file mode 100644
index 0000000..2d37999
--- /dev/null
+++ b/aos/crio/crio.gyp
@@ -0,0 +1,19 @@
+{
+ 'targets': [
+ {
+ # This test runs on the atom to verify that the cRIO version of the queues
+ # works.
+ 'target_name': 'unsafe_queue_test',
+ 'type': '<(aos_target)',
+ 'sources': [
+ 'queue_test.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ '<(AOS)/build/aos.gyp:libaos',
+ '<(AOS)/common/common.gyp:common',
+ '<(AOS)/common/common.gyp:queue_test_queue',
+ ],
+ },
+ ],
+}
diff --git a/aos/crio/googletest/google_test_server.cc b/aos/crio/googletest/google_test_server.cc
new file mode 100644
index 0000000..46631b5
--- /dev/null
+++ b/aos/crio/googletest/google_test_server.cc
@@ -0,0 +1,45 @@
+#include <stdio.h>
+
+#include "gtest/gtest.h"
+
+extern "C" int run_gtest(char *arg1, char *arg2, char *arg3, char *arg4,
+ char *arg5, char *arg6, char *arg7, char *arg8,
+ char *arg9, char *arg10, char *arg11) {
+ static bool run = false;
+ if (!run) {
+ run = true;
+ } else {
+ printf("error: gtest only supports being run once\n");
+ return -1;
+ }
+
+ char *argv[1 + 11 + 1];
+ // In /tmp in case it wants to write anything relative to "itself".
+ argv[0] = const_cast<char *>("/tmp/aos-crio-googletest-runner");
+ argv[12] = NULL; // the argv passed to main is always NULL-terminated
+ argv[1] = arg1;
+ argv[2] = arg2;
+ argv[3] = arg3;
+ argv[4] = arg4;
+ argv[5] = arg5;
+ argv[6] = arg6;
+ argv[7] = arg7;
+ argv[8] = arg8;
+ argv[9] = arg9;
+ argv[10] = arg10;
+ argv[11] = arg11;
+ int argc = 0;
+ while (argc[argv] != NULL) ++argc;
+
+ testing::GTEST_FLAG(color) = "yes";
+ testing::InitGoogleTest(&argc, argv);
+
+ if (argc > 1) {
+ printf("warning: flags not recognized by gtest passed\n");
+ for (int i = 1; i < argc; ++i) {
+ printf("\t%s\n", argv[i]);
+ }
+ }
+
+ return RUN_ALL_TESTS();
+}
diff --git a/aos/crio/googletest/googletest.gyp b/aos/crio/googletest/googletest.gyp
new file mode 100644
index 0000000..d887ecd
--- /dev/null
+++ b/aos/crio/googletest/googletest.gyp
@@ -0,0 +1,14 @@
+{
+ 'targets': [
+ {
+ 'target_name': 'googletest',
+ 'type': 'static_library',
+ 'sources': [
+ 'google_test_server.cc',
+ ],
+ 'dependencies': [
+ '<(EXTERNALS):gtest',
+ ],
+ },
+ ],
+}
diff --git a/aos/crio/logging/crio_logging.cpp b/aos/crio/logging/crio_logging.cpp
new file mode 100644
index 0000000..8ebafc0
--- /dev/null
+++ b/aos/crio/logging/crio_logging.cpp
@@ -0,0 +1,110 @@
+#include <string.h>
+
+#include "WPILib/Timer.h"
+#include "WPILib/Task.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/Configuration.h"
+#include "aos/common/die.h"
+
+#undef ERROR
+#define DECL_LEVEL(name, value) const log_level name = value;
+DECL_LEVELS
+#undef DECL_LEVEL
+
+//#define fprintf(...)
+
+namespace aos {
+namespace logging {
+namespace {
+
+MSG_Q_ID queue;
+uint8_t sequence = 0;
+
+// This gets run in a low-priority task to take the logs from the queue and send
+// them to the atom over a TCP socket.
+void DoTask() {
+ SendSocket sock;
+ log_crio_message msg;
+ while (true) {
+ const int ret = msgQReceive(queue, reinterpret_cast<char *>(&msg),
+ sizeof(msg), WAIT_FOREVER);
+ if (ret == ERROR) {
+ fprintf(stderr, "logging: warning: receiving a message failed"
+ " with %d (%s)", errno, strerror(errno));
+ continue;
+ }
+ if (ret != sizeof(msg)) {
+ fprintf(stderr, "logging: warning: received a message of size %d "
+ "instead of %zd\n", ret, sizeof(msg));
+ continue;
+ }
+
+ if (sock.LastStatus() != 0) {
+ if (sock.Connect(NetworkPort::kLogs,
+ configuration::GetIPAddress(
+ configuration::NetworkDevice::kAtom),
+ SOCK_STREAM) != 0) {
+ fprintf(stderr, "logging: warning: connecting failed"
+ " because of %d: %s\n", errno, strerror(errno));
+ }
+ }
+ sock.Send(&msg, sizeof(msg));
+ if (sock.LastStatus() != 0) {
+ fprintf(stderr, "logging: warning: sending '%s' failed"
+ " because of %d: %s\n", msg.message, errno, strerror(errno));
+ }
+ }
+}
+
+} // namespace
+
+void Start() {
+ queue = msgQCreate(100, // max messages
+ sizeof(log_crio_message),
+ MSG_Q_PRIORITY);
+ Task *task = new Task("LogSender",
+ (FUNCPTR)(DoTask),
+ 150); // priority
+ task->Start();
+}
+
+int Do(log_level level, const char *format, ...) {
+ log_crio_message msg;
+ msg.time = Timer::GetFPGATimestamp();
+ msg.level = level;
+ msg.sequence = __sync_fetch_and_add(&sequence, 1);
+
+ const char *continued = "...";
+ const size_t size = sizeof(msg.message) - strlen(continued);
+ va_list ap;
+ va_start(ap, format);
+ const int ret = vsnprintf(msg.message, size, format, ap);
+ va_end(ap);
+
+ if (ret < 0) {
+ fprintf(stderr, "logging: error: vsnprintf failed with %d (%s)\n",
+ errno, strerror(errno));
+ return -1;
+ } else if (static_cast<uintmax_t>(ret) >= static_cast<uintmax_t>(size)) {
+ // overwrite the NULL at the end of the existing one and
+ // copy in the one on the end of continued
+ memcpy(&msg.message[size - 1], continued, strlen(continued) + 1);
+ }
+ if (msgQSend(queue, reinterpret_cast<char *>(&msg), sizeof(msg),
+ NO_WAIT, MSG_PRI_NORMAL) == ERROR) {
+ fprintf(stderr, "logging: warning: sending message '%s'"
+ " failed with %d (%s)", msg.message, errno, strerror(errno));
+ return -1;
+ }
+
+ if (level == FATAL) {
+ aos::Die("%s", msg.message);
+ }
+
+ return 0;
+}
+
+} // namespace logging
+} // namespace aos
diff --git a/aos/crio/logging/crio_logging.h b/aos/crio/logging/crio_logging.h
new file mode 100644
index 0000000..c3dbf2a
--- /dev/null
+++ b/aos/crio/logging/crio_logging.h
@@ -0,0 +1,36 @@
+#ifndef AOS_CRIO_CRIO_LOGGING_LOGGING_H_
+#define AOS_CRIO_CRIO_LOGGING_LOGGING_H_
+
+#ifndef AOS_COMMON_LOGGING_LOGGING_H_
+#error This file may only be #included through common/logging/logging.h!!!
+#endif
+
+#undef extern
+#undef const
+#undef ERROR
+
+#include <msgQLib.h>
+#include <stdint.h>
+
+//#define LOG(level, fmt, args...) printf(STRINGIFY(level) ": " fmt, ##args)
+#define LOG(level, fmt, args...) \
+ ::aos::logging::Do(level, \
+ LOG_SOURCENAME ": " STRINGIFY(__LINE__) ": " fmt, \
+ ##args)
+//#define LOG(...)
+
+namespace aos {
+namespace logging {
+
+// Initialize internal variables and start up the task that sends the logs to
+// the atom. Must be called before Do.
+void Start();
+// The function that the LOG macro actually calls. Queues up a message for the
+// task to send. Start must be called before this function is.
+int Do(log_level level, const char *format, ...)
+ __attribute__((format(printf, 2, 3)));
+
+} // namespace logging
+} // namespace aos
+
+#endif // AOS_CRIO_CRIO_LOGGING_LOGGING_H_
diff --git a/aos/crio/messages/DriverStationDisplay.h b/aos/crio/messages/DriverStationDisplay.h
new file mode 100644
index 0000000..59d97b5
--- /dev/null
+++ b/aos/crio/messages/DriverStationDisplay.h
@@ -0,0 +1,51 @@
+#ifndef AOS_CRIO_DRIVER_STATION_DISPLAY_H_
+#define AOS_CRIO_DRIVER_STATION_DISPLAY_H_
+
+#include <stdarg.h>
+
+#include "WPILib/DriverStationLCD.h"
+
+namespace aos {
+
+class DriverStationDisplay {
+ public:
+ static void Send(int line, const char *fmt, ...)
+ __attribute__((format(printf, 2, 3))) {
+ DriverStationLCD::Line ds_line;
+ switch (line) {
+ case 0:
+ ds_line = DriverStationLCD::kMain_Line6;
+ break;
+ case 1:
+ ds_line = DriverStationLCD::kUser_Line1;
+ break;
+ case 2:
+ ds_line = DriverStationLCD::kUser_Line2;
+ break;
+ case 3:
+ ds_line = DriverStationLCD::kUser_Line3;
+ break;
+ case 4:
+ ds_line = DriverStationLCD::kUser_Line4;
+ break;
+ case 5:
+ ds_line = DriverStationLCD::kUser_Line5;
+ break;
+ case 6:
+ ds_line = DriverStationLCD::kUser_Line6;
+ break;
+ default:
+ printf("illegal line number %hhd\n", line);
+ return;
+ }
+ va_list args;
+ va_start(args, fmt);
+ DriverStationLCD::GetInstance()->VPrintfLine(ds_line, fmt, args);
+ va_end(args);
+ DriverStationLCD::GetInstance()->UpdateLCD();
+ }
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.cpp b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
new file mode 100644
index 0000000..9d6cd52
--- /dev/null
+++ b/aos/crio/motor_server/CRIOControlLoopRunner.cpp
@@ -0,0 +1,49 @@
+#include "CRIOControlLoopRunner.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+#include "aos/crio/motor_server/MotorOutput.h"
+
+using ::aos::control_loops::SerializableControlLoop;
+
+namespace aos {
+namespace crio {
+
+bool CRIOControlLoopRunner::started_ = false;
+std::vector<SerializableControlLoop *> CRIOControlLoopRunner::loops_;
+Mutex CRIOControlLoopRunner::loops_lock;
+
+void CRIOControlLoopRunner::Start() {
+ if (started_) {
+ LOG(WARNING, "not going to Start twice!!\n");
+ return;
+ }
+ started_ = true;
+
+ // TODO(aschuh): Hold on to a handle to this...
+ (new WDInterruptNotifier<void>(Notify))->StartPeriodic(0.01);
+}
+
+void CRIOControlLoopRunner::AddControlLoop(SerializableControlLoop *loop) {
+ MutexLocker control_loop_goals_locker(&loops_lock);
+ loops_.push_back(loop);
+ MotorServer::RegisterControlLoopGoal(loop);
+}
+
+void CRIOControlLoopRunner::Notify(void *) {
+ // TODO(aschuh): Too many singletons/static classes!
+ SensorOutputs::UpdateAll();
+ // sensors get read first so it doesn't really matter if this takes a little bit
+ {
+ MutexLocker control_loop_goals_locker(
+ &MotorServer::control_loop_goals_lock);
+ for (auto it = loops_.begin(); it != loops_.end(); ++it) {
+ (*it)->Iterate();
+ }
+ }
+ MotorOutput::RunIterationAll();
+ MotorServer::WriteOutputs();
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/motor_server/CRIOControlLoopRunner.h b/aos/crio/motor_server/CRIOControlLoopRunner.h
new file mode 100644
index 0000000..efed120
--- /dev/null
+++ b/aos/crio/motor_server/CRIOControlLoopRunner.h
@@ -0,0 +1,40 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
+#define AOS_CRIO_MOTOR_SERVER_CRIO_CONTROL_LOOP_RUNNER_H_
+
+#include <vector>
+#include <semLib.h>
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/mutex.h"
+
+namespace aos {
+namespace crio {
+
+// Runs crio-side control loops. Completely static because there is no reason
+// for multiple ones and it gets rid of the problem of passing an instance
+// around.
+class CRIOControlLoopRunner {
+ public:
+ // Spawns a new Task that loops forever.
+ // No other functions should be called before this one returns.
+ static void Start();
+
+ // Adds a control loop to run.
+ // This class takes control of the instance.
+ static void AddControlLoop(control_loops::SerializableControlLoop *loop);
+
+ private:
+ static bool started_;
+
+ static std::vector<control_loops::SerializableControlLoop *> loops_;
+ static Mutex loops_lock;
+
+ // Gets called by a WDInterruptNotifier on 0.01 second intervals.
+ static void Notify(void *);
+};
+
+
+} // namespace crio
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/ControlLoopGoals.h b/aos/crio/motor_server/ControlLoopGoals.h
new file mode 100644
index 0000000..f22c0a2
--- /dev/null
+++ b/aos/crio/motor_server/ControlLoopGoals.h
@@ -0,0 +1,44 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_CONTROL_LOOP_GOAL_H_
+#define AOS_CRIO_MOTOR_SERVER_CONTROL_LOOP_GOAL_H_
+
+#include <vector>
+#include <semLib.h>
+
+namespace aos {
+
+// This class is used to keep track of all the control loop goals. It exists
+// because of several bugs discovered in the std::map implementation.
+class ControlLoopGoals {
+ public:
+ struct Goal {
+ const char *const name;
+ const size_t length;
+ void (*const zero)();
+ void (*const ntoh)(const char *);
+ Goal(const char *name, size_t length, void (*zero)(), void (*ntoh)(const char *)) :
+ name(name), length(length), zero(zero), ntoh(ntoh) {}
+ };
+
+ private:
+ std::vector<Goal *> goals_;
+
+ public:
+ ControlLoopGoals() {}
+ void Add(const char *name, size_t length, void (*zero)(), void (*ntoh)(const char *)) {
+ char *storage = new char[10];
+ memcpy(storage, name, sizeof(storage));
+ goals_.push_back(new Goal(storage, length, zero, ntoh));
+ }
+ const Goal *Get(const char *name) {
+ for (auto it = goals_.begin(); it != goals_.end(); ++it) {
+ if (memcmp((*it)->name, name, sizeof((*it)->name)) == 0) {
+ return *it;
+ }
+ }
+ return NULL;
+ }
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/MotorControllerOutput.cpp b/aos/crio/motor_server/MotorControllerOutput.cpp
new file mode 100644
index 0000000..a0897b4
--- /dev/null
+++ b/aos/crio/motor_server/MotorControllerOutput.cpp
@@ -0,0 +1,103 @@
+#include "aos/crio/motor_server/MotorControllerOutput.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/byteorder.h"
+#include "aos/common/commonmath.h"
+
+namespace aos {
+
+void LinearizedVictor::Set(float speed, UINT8 syncGroup) {
+ speed_ = speed;
+ Victor::Set(Linearize(speed), syncGroup);
+}
+
+float LinearizedVictor::Get() {
+ return speed_;
+}
+
+void LinearizedVictor::Disable() {
+ Victor::Disable();
+ speed_ = 0.0;
+}
+
+double LinearizedVictor::Linearize(double goal_speed) {
+ // These values were derived by putting the robot up on blocks, and driving it
+ // at various speeds. The power required to drive at these speeds was then
+ // recorded and fit with gnuplot.
+ const double deadband_value = 0.082;
+ // If we are outside the range such that the motor is actually moving,
+ // subtract off the constant offset from the deadband. This makes the
+ // function odd and intersect the origin, making the fitting easier to do.
+ if (goal_speed > deadband_value) {
+ goal_speed -= deadband_value;
+ } else if (goal_speed < -deadband_value) {
+ goal_speed += deadband_value;
+ } else {
+ goal_speed = 0.0;
+ }
+ goal_speed = goal_speed / (1.0 - deadband_value);
+
+ double goal_speed2 = goal_speed * goal_speed;
+ double goal_speed3 = goal_speed2 * goal_speed;
+ double goal_speed4 = goal_speed3 * goal_speed;
+ double goal_speed5 = goal_speed4 * goal_speed;
+ double goal_speed6 = goal_speed5 * goal_speed;
+ double goal_speed7 = goal_speed6 * goal_speed;
+
+ // Constants for the 5th order polynomial
+ double victor_fit_e1 = 0.437239;
+ double victor_fit_c1 = -1.56847;
+ double victor_fit_a1 = (- (125.0 * victor_fit_e1 + 125.0
+ * victor_fit_c1 - 116.0) / 125.0);
+ double answer_5th_order = (victor_fit_a1 * goal_speed5
+ + victor_fit_c1 * goal_speed3
+ + victor_fit_e1 * goal_speed);
+
+ // Constants for the 7th order polynomial
+ double victor_fit_c2 = -5.46889;
+ double victor_fit_e2 = 2.24214;
+ double victor_fit_g2 = -0.042375;
+ double victor_fit_a2 = (- (125.0 * (victor_fit_c2 + victor_fit_e2
+ + victor_fit_g2) - 116.0) / 125.0);
+ double answer_7th_order = (victor_fit_a2 * goal_speed7
+ + victor_fit_c2 * goal_speed5
+ + victor_fit_e2 * goal_speed3
+ + victor_fit_g2 * goal_speed);
+
+
+ // Average the 5th and 7th order polynomials, and add a bit of linear power in
+ // as well. The average turns out to nicely fit the data in gnuplot with nice
+ // smooth curves, and the linear power gives it a bit more punch at low speeds
+ // again. Stupid victors.
+ double answer = 0.85 * 0.5 * (answer_7th_order + answer_5th_order)
+ + .15 * goal_speed * (1.0 - deadband_value);
+
+ // Add the deadband power back in to make it so that the motor starts moving
+ // when any power is applied. This is what the fitting assumes.
+ if (answer > 0.001) {
+ answer += deadband_value;
+ } else if (answer < -0.001) {
+ answer -= deadband_value;
+ }
+
+ return Clip(answer, -1.0, 1.0);
+}
+
+bool MotorControllerOutput::ReadValue(ByteBuffer &buff) {
+ const float val = buff.read_float();
+ if (val == (1.0 / 0.0)) {
+ return false;
+ }
+ value = val;
+ return true;
+}
+void MotorControllerOutput::SetValue() {
+ output.Set(value);
+}
+void MotorControllerOutput::NoValue() {
+ // this is NOT a Set(0.0); it's the same as when the robot is disabled
+ output.Disable();
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/MotorControllerOutput.h b/aos/crio/motor_server/MotorControllerOutput.h
new file mode 100644
index 0000000..41c3546
--- /dev/null
+++ b/aos/crio/motor_server/MotorControllerOutput.h
@@ -0,0 +1,68 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_MOTOR_CONTROLLER_OUTPUT_H_
+#define AOS_CRIO_MOTOR_SERVER_MOTOR_CONTROLLER_OUTPUT_H_
+
+#include "aos/crio/motor_server/OutputDevice.h"
+
+#include "aos/crio/Talon.h"
+
+#include "WPILib/SpeedController.h"
+#include "WPILib/Jaguar.h"
+#include "WPILib/CANJaguar.h"
+#include "WPILib/Victor.h"
+
+namespace aos {
+
+// LinearizedVictor is a Victor that transforms the set values to linearize the
+// hardware's response curve.
+class LinearizedVictor : public Victor {
+ public:
+ explicit LinearizedVictor(uint32_t channel) : Victor(channel), speed_(0) {}
+ virtual void Set(float speed, UINT8 syncGroup=0);
+ virtual float Get();
+ virtual void Disable();
+
+ // Returns the linearized motor power to apply to get the motor to go at the
+ // provided goal_speed.
+ static double Linearize(double goal_speed);
+
+ private:
+ // The speed last sent to the Victor.
+ float speed_;
+};
+
+class MotorControllerOutput : public OutputDevice {
+ private:
+ SpeedController &output;
+ protected:
+ double value;
+ MotorControllerOutput(SpeedController *output) : OutputDevice(), output(*output) {
+ value = 0.0;
+ }
+ // TODO(brians) add virtual destructor?
+
+ virtual bool ReadValue(ByteBuffer &buff);
+ virtual void SetValue();
+ virtual void NoValue();
+};
+
+class JaguarOutput : public MotorControllerOutput {
+ public:
+ JaguarOutput(uint32_t port) : MotorControllerOutput(new Jaguar(port)) {}
+};
+class CANJaguarOutput : public MotorControllerOutput {
+ public:
+ CANJaguarOutput(uint32_t port) : MotorControllerOutput(new CANJaguar(port)) {}
+};
+class VictorOutput : public MotorControllerOutput {
+ public:
+ VictorOutput(uint32_t port)
+ : MotorControllerOutput(new LinearizedVictor(port)) {}
+};
+class TalonOutput : public MotorControllerOutput {
+ public:
+ TalonOutput(uint32_t port) : MotorControllerOutput(new Talon(port)) {}
+};
+
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/MotorOutput.cpp b/aos/crio/motor_server/MotorOutput.cpp
new file mode 100644
index 0000000..f2e9925
--- /dev/null
+++ b/aos/crio/motor_server/MotorOutput.cpp
@@ -0,0 +1,22 @@
+#include "MotorOutput.h"
+
+namespace aos {
+
+SEM_ID MotorOutput::lock = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+std::vector<MotorOutput *> MotorOutput::instances;
+
+void MotorOutput::Run() {
+ semTake(lock, WAIT_FOREVER);
+ instances.push_back(this);
+ semGive(lock);
+}
+void MotorOutput::RunIterationAll() {
+ semTake(lock, WAIT_FOREVER);
+ for (auto it = instances.begin(); it != instances.end(); ++it) {
+ (*it)->RunIteration();
+ }
+ semGive(lock);
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/MotorOutput.h b/aos/crio/motor_server/MotorOutput.h
new file mode 100644
index 0000000..af7c803
--- /dev/null
+++ b/aos/crio/motor_server/MotorOutput.h
@@ -0,0 +1,27 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_MOTOR_OUTPUT_H_
+#define AOS_CRIO_MOTOR_SERVER_MOTOR_OUTPUT_H_
+
+#include <vector>
+#include <semLib.h>
+
+namespace aos {
+
+// The place where the outputs from crio control loops get written out to the
+// motors.
+class MotorOutput {
+ public:
+ // Call RunIteration on all instances that have been Run.
+ static void RunIterationAll();
+ void Run();
+ protected:
+ // Write the outputs from crio control loops to wherever they go.
+ virtual void RunIteration() = 0;
+ private:
+ static std::vector<MotorOutput *> instances;
+ static SEM_ID lock;
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/motor_server/MotorServer.cpp b/aos/crio/motor_server/MotorServer.cpp
new file mode 100644
index 0000000..77a0579
--- /dev/null
+++ b/aos/crio/motor_server/MotorServer.cpp
@@ -0,0 +1,227 @@
+#include "aos/crio/motor_server/MotorServer.h"
+
+#include <stdio.h>
+#include <stdlib.h>
+#include "usrLib.h"
+#include "aos/common/inttypes.h"
+
+#include "WPILib/Timer.h"
+#include "WPILib/Task.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/motor_server/MotorControllerOutput.h"
+#include "aos/crio/motor_server/SolenoidOutput.h"
+#include "aos/common/Configuration.h"
+
+namespace aos {
+namespace crio {
+
+ByteBuffer MotorServer::buff(4096);
+DriverStationLCD *MotorServer::ds_lcd(NULL);
+int MotorServer::count(0);
+ReceiveSocket *MotorServer::sock(NULL);
+SEM_ID MotorServer::motorSync = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+OutputDevice *MotorServer::output_devices[256][kMaxOutputDeviceNumber];
+Mutex MotorServer::control_loop_goals_lock;
+::std::map<uint32_t,
+ control_loops::SerializableControlLoop *> MotorServer::loops;
+Task *MotorServer::tcpTask;
+void MotorServer::Start() {
+ sock = new ReceiveSocket(NetworkPort::kMotors);
+
+ memset(output_devices, 0x00, sizeof(output_devices));
+
+ tcpTask = new Task("MRLoop",
+ reinterpret_cast<FUNCPTR>(RunReaderTask),
+ WORK_PRIORITY);
+
+ tcpTask->Start();
+}
+
+void MotorServer::ProcessBuf() {
+ semTake(motorSync, WAIT_FOREVER);
+ bool cont = true;
+ while (true) {
+ if (!cont) {
+ LOG(WARNING, "Malformed Packet\n");
+ goto end;
+ }
+ switch (const int value = buff.read_char()) {
+ case 'g':
+ cont = ProcessControlLoopGoal();
+ break;
+ case 'd':
+ cont = ProcessDSLine();
+ break;
+ case -1:
+ goto end;
+ default:
+ cont = ProcessOutputDevice(value);
+ break;
+ }
+ }
+end:
+ ++count;
+ semGive(motorSync);
+}
+bool MotorServer::ProcessOutputDevice(const int type) {
+ const int id = buff.read_char(); // 1-indexed
+ if (id < 1 || id > static_cast<ssize_t>(kMaxOutputDeviceNumber)) {
+ if (id != -1) {
+ LOG(ERROR, "illegal OutputDevice id %d\n", id);
+ }
+ return false;
+ }
+
+ if (output_devices[type][id - 1] == NULL) {
+ switch (type) {
+ case 'v':
+ output_devices[type][id - 1] = new VictorOutput(id);
+ break;
+ case 'j':
+ output_devices[type][id - 1] = new JaguarOutput(id);
+ break;
+ case 'c':
+ output_devices[type][id - 1] = new CANJaguarOutput(id);
+ break;
+ case 't':
+ output_devices[type][id - 1] = new TalonOutput(id);
+ break;
+ case 's':
+ output_devices[type][id - 1] = new SolenoidOutput(id);
+ break;
+ default:
+ LOG(ERROR, "unrecognized OutputDevice type %d\n", type);
+ return false;
+ }
+ }
+ return output_devices[type][id - 1]->ReadValue(buff);
+}
+
+bool MotorServer::ProcessDSLine() {
+ int line = buff.read_char();
+ if (line == -1) {
+ return false;
+ }
+ // TODO(brians): Subfunction
+ DriverStationLCD::Line ds_line;
+ switch (line) {
+ case 0:
+ ds_line = DriverStationLCD::kMain_Line6;
+ break;
+ case 1:
+ ds_line = DriverStationLCD::kUser_Line1;
+ break;
+ case 2:
+ ds_line = DriverStationLCD::kUser_Line2;
+ break;
+ case 3:
+ ds_line = DriverStationLCD::kUser_Line3;
+ break;
+ case 4:
+ ds_line = DriverStationLCD::kUser_Line4;
+ break;
+ case 5:
+ ds_line = DriverStationLCD::kUser_Line5;
+ break;
+ case 6:
+ ds_line = DriverStationLCD::kUser_Line6;
+ break;
+ default:
+ LOG(ERROR, "illegal line number %hhd\n", line);
+ return false;
+ }
+ // TODO(brians) see if this mess with not creating the DriverStationLCD for a
+ // bit is actually needed
+ static int ds_lcd_counts = 0; // to prevent crashes at startup
+ if (ds_lcd == NULL) {
+ if (ds_lcd_counts < 100) {
+ ++ds_lcd_counts;
+ } else {
+ ++ds_lcd_counts;
+ ds_lcd = DriverStationLCD::GetInstance();
+ }
+ }
+ char buf[DriverStationLCD::kLineLength];
+ buff.read_string(buf, sizeof(buf));
+ buf[sizeof(buf) - 1] = 0;
+ if (ds_lcd != NULL) {
+ ds_lcd->PrintfLine(ds_line, "%s", buf);
+ }
+ return true;
+}
+
+void MotorServer::RegisterControlLoopGoal(
+ control_loops::SerializableControlLoop *control_loop) {
+ uint32_t unique_id = control_loop->UniqueID();
+
+ bool replaced;
+ {
+ MutexLocker control_loop_goals_locker(&control_loop_goals_lock);
+ replaced = !InsertIntoMap(&loops, unique_id, control_loop);
+ }
+ if (replaced) {
+ LOG(ERROR, "Replaced a key for unique id 0x%"PRIx32"\n", unique_id);
+ }
+}
+
+bool MotorServer::ProcessControlLoopGoal() {
+ // Read back a uint32_t with the hash.
+ uint32_t hash;
+ if (!buff.read_uint32(&hash)) return false;
+ MutexLocker control_loop_goals_locker(&control_loop_goals_lock);
+
+ control_loops::SerializableControlLoop *loop;
+ if (!GetFromMap(loops, hash, &loop)) {
+ return false;
+ }
+ const size_t length = loop->SeralizedSize();
+ char *const goal_bytes = buff.get_bytes(length);
+ if (goal_bytes == NULL) {
+ return false;
+ } else {
+ loop->Deserialize(goal_bytes);
+ }
+ return true;
+}
+
+void MotorServer::RunReaderTask() {
+ while (true) {
+ if (buff.recv_from_sock(sock)) {
+ ProcessBuf();
+ }
+ }
+}
+void MotorServer::WriteOutputs() {
+ static int last_count = 0, bad_counts = 0;
+ semTake(motorSync, WAIT_FOREVER);
+ if (last_count != count) {
+ bad_counts = 0;
+ } else {
+ ++bad_counts;
+ }
+ last_count = count;
+ // both loops iterate over all elements of output_devices by indexing off the
+ // end of output_devices[0]
+ if (bad_counts > 2) {
+ LOG(WARNING, "no new values. stopping all outputs\n");
+ for (size_t i = 0; i < sizeof(output_devices) / sizeof(output_devices[0][0]); ++i) {
+ if (output_devices[0][i] != NULL) {
+ output_devices[0][i]->NoValue();
+ }
+ }
+ } else {
+ for (size_t i = 0; i < sizeof(output_devices) / sizeof(output_devices[0][0]); ++i) {
+ if (output_devices[0][i] != NULL) {
+ output_devices[0][i]->SetValue();
+ }
+ }
+ }
+ if (ds_lcd != NULL) {
+ ds_lcd->UpdateLCD();
+ }
+ semGive(motorSync);
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/motor_server/MotorServer.h b/aos/crio/motor_server/MotorServer.h
new file mode 100644
index 0000000..0126663
--- /dev/null
+++ b/aos/crio/motor_server/MotorServer.h
@@ -0,0 +1,87 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_MOTOR_SERVER_H_
+#define AOS_CRIO_MOTOR_SERVER_MOTOR_SERVER_H_
+
+#include <vxWorks.h>
+#include <timers.h>
+#include <string.h>
+#include "WPILib/Task.h"
+#include "WPILib/Victor.h"
+#include "WPILib/Jaguar.h"
+#include "WPILib/Solenoid.h"
+#include "sockLib.h"
+#include <inetLib.h>
+#include <stdio.h>
+#include <selectLib.h>
+#include <stdlib.h>
+#include <time.h>
+#include <map>
+#include <string>
+
+#include "WPILib/DriverStationLCD.h"
+
+#include "aos/common/control_loop/ControlLoop.h"
+#include "aos/common/inttypes.h"
+#include "aos/common/messages/QueueHolder.h"
+#include "aos/common/mutex.h"
+#include "aos/common/network/ReceiveSocket.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/crio/motor_server/ControlLoopGoals.h"
+#include "aos/crio/motor_server/OutputDevice.h"
+#include "aos/crio/motor_server/SensorSender.h"
+#include "aos/crio/shared_libs/ByteBuffer.h"
+#include "aos/map_utils.h"
+
+namespace aos {
+namespace crio {
+
+class CRIOControlLoopRunner;
+class MotorServer {
+ public:
+ static void Start();
+
+ // Adds the given control loop's goal queue to the list of ones to process.
+ static void RegisterControlLoopGoal(
+ control_loops::SerializableControlLoop *control_loop);
+
+ static const int32_t WORK_PRIORITY = 100;
+
+ private:
+ friend class CRIOControlLoopRunner;
+ // Counter for how many times new values come in. Used to stop all the
+ // outputs if values stop.
+ // Would take days to overflow.
+ static int count;
+ static SEM_ID motorSync;
+ // Gets called by CRIOControlLoopRunner every 10ms after it runs all of the
+ // control loops.
+ static void WriteOutputs();
+
+ static void RunReaderTask();
+ static Task *tcpTask;
+ static ReceiveSocket *sock;
+ static ByteBuffer buff;
+
+ static DriverStationLCD *ds_lcd;
+ static bool ProcessDSLine();
+
+ static const size_t kMaxOutputDeviceNumber = 10;
+ static OutputDevice *output_devices[256][kMaxOutputDeviceNumber];
+ static bool ProcessOutputDevice(const int type);
+
+ // Go through the whole buffer and call the appropriate Process* methods to
+ // process each part.
+ static void ProcessBuf();
+
+ static bool ProcessControlLoopGoal();
+ // Locked whenever adding/using the control loop goals maps.
+ // Also used by CRIOControlLoopRunner while modifying any of the data
+ // structures. Used by both of them while reading/writing from
+ // the goal queues.
+ static Mutex control_loop_goals_lock;
+ static ::std::map<uint32_t, control_loops::SerializableControlLoop *> loops;
+};
+
+} // namespace crio
+} // namespace aos
+
+#endif
diff --git a/aos/crio/motor_server/OutputDevice.h b/aos/crio/motor_server/OutputDevice.h
new file mode 100644
index 0000000..0789a68
--- /dev/null
+++ b/aos/crio/motor_server/OutputDevice.h
@@ -0,0 +1,26 @@
+#ifndef __CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
+#define __CRIO_MOTOR_SERVER_OUTPUT_DEVICE_H_
+
+#include <stdint.h>
+#include "aos/crio/shared_libs/ByteBuffer.h"
+
+namespace aos {
+
+class OutputDevice {
+ protected:
+ OutputDevice() {
+ }
+ public:
+ // Reads the value out of buff and stores it somewhere for SetValue to use.
+ // Returns whether or not it successfully read a whole value out of buff.
+ virtual bool ReadValue(ByteBuffer &buff) = 0;
+ // Actually sets the output device to the value saved by ReadValue.
+ virtual void SetValue() = 0;
+ // Gets called when no values come in for a while.
+ virtual void NoValue() = 0;
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/motor_server/SensorOutput-tmpl.h b/aos/crio/motor_server/SensorOutput-tmpl.h
new file mode 100644
index 0000000..d6b8b69
--- /dev/null
+++ b/aos/crio/motor_server/SensorOutput-tmpl.h
@@ -0,0 +1,27 @@
+#include "aos/common/input/SensorInput.h"
+
+namespace aos {
+
+template<class Values> std::vector<SensorOutput<Values> *> SensorOutput<Values>::output_running_;
+template<class Values> void SensorOutput<Values>::Run() {
+ semTake(lock_, WAIT_FOREVER);
+ output_running_.push_back(this);
+ outputs_running_.push_back(this);
+ semGive(lock_);
+}
+
+template<class Values> void SensorOutput<Values>::RunIterationAll(Values &vals) {
+ semTake(lock_, WAIT_FOREVER);
+ for (auto it = output_running_.begin(); it != output_running_.end(); ++it) {
+ (*it)->RunIteration(vals);
+ }
+ semGive(lock_);
+}
+template<class Values> void SensorOutput<Values>::Update() {
+ Values vals;
+ RunIteration(vals);
+ SensorInput<Values>::RunIterationAll(vals);
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/SensorOutput.cpp b/aos/crio/motor_server/SensorOutput.cpp
new file mode 100644
index 0000000..b887885
--- /dev/null
+++ b/aos/crio/motor_server/SensorOutput.cpp
@@ -0,0 +1,17 @@
+#include "aos/crio/motor_server/SensorOutput.h"
+
+namespace aos {
+
+SEM_ID SensorOutputs::lock_ = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
+std::vector<SensorOutputs *> SensorOutputs::outputs_running_;
+
+void SensorOutputs::UpdateAll() {
+ semTake(lock_, WAIT_FOREVER);
+ for (auto it = outputs_running_.begin(); it != outputs_running_.end(); ++it) {
+ (*it)->Update();
+ }
+ semGive(lock_);
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/SensorOutput.h b/aos/crio/motor_server/SensorOutput.h
new file mode 100644
index 0000000..9e30cb9
--- /dev/null
+++ b/aos/crio/motor_server/SensorOutput.h
@@ -0,0 +1,48 @@
+#ifndef AOS_CRIO_MOTOR_SERVER_SENSOR_OUTPUT_H_
+#define AOS_CRIO_MOTOR_SERVER_SENSOR_OUTPUT_H_
+
+#include <semLib.h>
+#include <vector>
+
+namespace aos {
+
+// Keeps track of instances of all instantiations.
+class SensorOutputs {
+ public:
+ // Calls RunIteration on all instances and then runs all SensorInput
+ // subclasses for that type.
+ static void UpdateAll();
+ private:
+ static SEM_ID lock_;
+ static std::vector<SensorOutputs *> outputs_running_;
+ protected:
+ // Calls RunIteration with a temporary Values instance and then runs all
+ // SensorInput subclasses with the same Values type.
+ virtual void Update() = 0;
+};
+
+// Class for implementing crio code that reads sensor values and puts them into
+// the sensor struct.
+template<class Values> class SensorOutput : public SensorOutputs {
+ protected:
+ // Fills out vals with the current data.
+ // May not be called at anything close to consistent intervals and may be
+ // called simultaneously with different arguments, so it must be reentrant.
+ virtual void RunIteration(Values &vals) = 0;
+ public:
+ // Sets it up so that RunIteration will get called when appropriate.
+ void Run();
+
+ // Calls RunIteration on all instances with vals.
+ static void RunIterationAll(Values &vals);
+ private:
+ static std::vector<SensorOutput<Values> *> output_running_;
+ virtual void Update();
+};
+
+} // namespace aos
+
+#include "SensorOutput-tmpl.h"
+
+#endif
+
diff --git a/aos/crio/motor_server/SensorSender-tmpl.h b/aos/crio/motor_server/SensorSender-tmpl.h
new file mode 100644
index 0000000..93fe73d
--- /dev/null
+++ b/aos/crio/motor_server/SensorSender-tmpl.h
@@ -0,0 +1,21 @@
+#include "WPILib/Task.h"
+#include "WPILib/Timer.h"
+#include "aos/crio/motor_server/SensorOutput.h"
+#include "aos/common/network/SendSocket.h"
+#include "aos/common/Configuration.h"
+
+namespace aos {
+
+template<class Values> void SensorSender<Values>::Run() {
+ SendSocket sock(NetworkPort::kSensors,
+ configuration::GetIPAddress(configuration::NetworkDevice::kAtom));
+ Values vals;
+ while (true) {
+ Wait(0.0015);
+ SensorOutput<Values>::RunIterationAll(vals);
+ sock.Send(&vals, sizeof(vals));
+ }
+}
+
+} // namespace aos
+
diff --git a/aos/crio/motor_server/SensorSender.h b/aos/crio/motor_server/SensorSender.h
new file mode 100644
index 0000000..135e1fa
--- /dev/null
+++ b/aos/crio/motor_server/SensorSender.h
@@ -0,0 +1,23 @@
+#ifndef __CRIO_SENSOR_SENDER_H_
+#define __CRIO_SENSOR_SENDER_H_
+
+namespace aos {
+
+// A class that handles sending all of the sensor values to the atom.
+// Designed for an instantiation (aos::SensorSender<X>) to be AOS_RUN_FORKed,
+// NOT a subclass.
+// Values is the type of the struct that will get sent out over the network.
+// Note: it should the same as the instance of TODO(brians) on the atom and any
+// SensorOutput instances that you want to feed into an instance of this.
+template<class Values> class SensorSender {
+ public:
+ // Loops forever.
+ void Run();
+};
+
+} // namespace aos
+
+#include "SensorSender-tmpl.h"
+
+#endif
+
diff --git a/aos/crio/motor_server/SolenoidOutput.h b/aos/crio/motor_server/SolenoidOutput.h
new file mode 100644
index 0000000..6a6c838
--- /dev/null
+++ b/aos/crio/motor_server/SolenoidOutput.h
@@ -0,0 +1,38 @@
+#ifndef __CRIO_MOTOR_SERVER_SOLENOID_OUTPUT_H_
+#define __CRIO_MOTOR_SERVER_SOLENOID_OUTPUT_H_
+
+#include "aos/crio/motor_server/OutputDevice.h"
+
+namespace aos {
+
+class SolenoidOutput : public OutputDevice {
+ private:
+ Solenoid solenoid;
+ bool value;
+ public:
+ SolenoidOutput(uint32_t port) : OutputDevice(), solenoid(port), value(false) {
+ }
+ protected:
+ virtual bool ReadValue(ByteBuffer &buff) {
+ const int on = buff.read_char();
+ if (on != 0 && on != 1) {
+ if (on != -1) {
+ LOG(ERROR, "illegal solenoid value %d\n", on);
+ }
+ return false;
+ }
+ value = on;
+ return true;
+ }
+ virtual void SetValue() {
+ solenoid.Set(value);
+ }
+ virtual void NoValue() {
+ // leave the solenoid in its previous state
+ }
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/motor_server/victor_drive.rb b/aos/crio/motor_server/victor_drive.rb
new file mode 100644
index 0000000..7de9706
--- /dev/null
+++ b/aos/crio/motor_server/victor_drive.rb
@@ -0,0 +1,30 @@
+require "socket"
+$sock = UDPSocket.new()
+$sock.connect("10.59.71.2",9123)
+def short(val)
+ val += 1.0
+ if(val < 0)
+ val = 0
+ end
+ val = (val * 256 * 128).to_i
+ v1 = val / 256
+ v2 = val % 256
+ if(v1 > 255)
+ v1 = 255
+ v2 = 255
+ end
+ return(v1.chr + v2.chr)
+end
+def jaguar(port,val)
+ $sock.send("j#{port.chr}#{short(val)}",0)
+end
+trap(2) do
+ jaguar(4,0)
+ jaguar(3,0)
+ exit!
+end
+while true
+ jaguar(4,Math.cos(Time.now.to_f))
+ jaguar(3,Math.cos(Time.now.to_f))
+ sleep(0.01)
+end
diff --git a/aos/crio/queue-tmpl.h b/aos/crio/queue-tmpl.h
new file mode 100644
index 0000000..94bc100
--- /dev/null
+++ b/aos/crio/queue-tmpl.h
@@ -0,0 +1,57 @@
+namespace aos {
+
+// The easiest way to hack this together is to have the scoped msg pointer not
+// manage the pointer, since it is a pointer to the only msg in the queue.
+template <class T>
+bool ScopedMessagePtr<T>::Send() {
+ msg_->SetTimeToNow();
+ reset();
+ return true;
+}
+
+template <class T>
+bool ScopedMessagePtr<T>::SendBlocking() {
+ msg_->SetTimeToNow();
+ reset();
+ return true;
+}
+
+template <class T>
+void ScopedMessagePtr<T>::reset(T *msg) {
+ msg_ = msg;
+}
+
+template <class T>
+void Queue<T>::Init() {}
+
+template <class T>
+bool Queue<T>::FetchNext() {
+ Init();
+ return true;
+}
+
+template <class T>
+bool Queue<T>::FetchNextBlocking() {
+ Init();
+ return true;
+}
+
+template <class T>
+bool Queue<T>::FetchLatest() {
+ Init();
+ return true;
+}
+
+template <class T>
+ScopedMessagePtr<T> Queue<T>::MakeMessage() {
+ Init();
+ return ScopedMessagePtr<T>(&msg_);
+}
+
+template <class T>
+aos::MessageBuilder<T> Queue<T>::MakeWithBuilder() {
+ Init();
+ return aos::MessageBuilder<T>(&msg_);
+}
+
+} // namespace aos
diff --git a/aos/crio/queue_test.cc b/aos/crio/queue_test.cc
new file mode 100644
index 0000000..39609bc
--- /dev/null
+++ b/aos/crio/queue_test.cc
@@ -0,0 +1,122 @@
+#include <unistd.h>
+
+#include <memory>
+
+#include "gtest/gtest.h"
+#define __TEST_VXWORKS__
+#include "aos/common/test_queue.q.h"
+
+using ::aos::time::Time;
+
+namespace aos {
+namespace common {
+namespace testing {
+
+class CRIOQueueTest : public ::testing::Test {
+ protected:
+ // Create a new instance of the test queue that is fresh.
+ ::aos::Queue<TestingMessage> my_test_queue;
+
+ CRIOQueueTest() : my_test_queue(".aos.common.testing.test_queue") {}
+};
+
+// Tests that we can send a message with the message pointer and get it back.
+TEST_F(CRIOQueueTest, SendMessage) {
+ ScopedMessagePtr<TestingMessage> msg = my_test_queue.MakeMessage();
+ msg->test_bool = true;
+ msg->test_int = 0x971;
+ msg.Send();
+
+ my_test_queue.FetchLatest();
+ EXPECT_TRUE(my_test_queue->test_bool);
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+}
+
+// Tests that we can send a message with the builder and get it back.
+TEST_F(CRIOQueueTest, SendWithBuilder) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ my_test_queue.FetchLatest();
+ EXPECT_EQ(true, my_test_queue->test_bool);
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+ EXPECT_EQ(true, my_test_queue.IsNewerThanMS(10000));
+}
+
+// Tests that various pointer deref functions at least seem to work.
+TEST_F(CRIOQueueTest, PointerDeref) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ my_test_queue.FetchLatest();
+ const TestingMessage *msg_ptr = my_test_queue.get();
+ ASSERT_NE(static_cast<TestingMessage*>(NULL), msg_ptr);
+ EXPECT_EQ(0x971, msg_ptr->test_int);
+ EXPECT_EQ(msg_ptr, &(*my_test_queue));
+}
+
+// Tests that FetchLatest skips a missing message.
+TEST_F(CRIOQueueTest, FetchLatest) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x254).Send();
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ my_test_queue.FetchLatest();
+ EXPECT_EQ(0x971, my_test_queue->test_int);
+}
+
+// Tests that age makes some sense.
+TEST_F(CRIOQueueTest, Age) {
+ my_test_queue.MakeWithBuilder().test_bool(true).test_int(0x971).Send();
+
+ my_test_queue.FetchLatest();
+ EXPECT_TRUE(my_test_queue.IsNewerThanMS(100));
+ const Time age = my_test_queue.Age();
+ EXPECT_EQ(0, age.sec());
+ EXPECT_GE(100000000, age.nsec());
+}
+
+
+class CRIOMessageTest : public ::testing::Test {
+ public:
+ TestingMessage msg;
+};
+
+TEST_F(CRIOMessageTest, Zeroing) {
+ msg.test_bool = true;
+ msg.test_int = 0x254;
+ msg.SetTimeToNow();
+
+ msg.Zero();
+
+ EXPECT_FALSE(msg.test_bool);
+ EXPECT_EQ(0, msg.test_int);
+ EXPECT_EQ(0, msg.sent_time.sec());
+ EXPECT_EQ(0, msg.sent_time.nsec());
+}
+
+TEST_F(CRIOMessageTest, Size) {
+ EXPECT_EQ(static_cast<size_t>(13), msg.Size());
+}
+
+TEST_F(CRIOMessageTest, Serialize) {
+ char serialized_data[msg.Size()];
+ msg.test_bool = true;
+ msg.test_int = 0x254;
+ msg.SetTimeToNow();
+
+ msg.Serialize(serialized_data);
+
+ TestingMessage new_msg;
+ new_msg.Deserialize(serialized_data);
+
+ EXPECT_EQ(msg.test_bool, new_msg.test_bool);
+ EXPECT_EQ(msg.test_int, new_msg.test_int);
+ EXPECT_EQ(msg.sent_time, new_msg.sent_time);
+}
+
+TEST_F(CRIOMessageTest, SetNow) {
+ msg.SetTimeToNow();
+ EXPECT_LE(msg.sent_time - Time::Now(), Time::InMS(20));
+}
+
+} // namespace testing
+} // namespace common
+} // namespace aos
diff --git a/aos/crio/shared_libs/ByteBuffer.h b/aos/crio/shared_libs/ByteBuffer.h
new file mode 100644
index 0000000..b5c4902
--- /dev/null
+++ b/aos/crio/shared_libs/ByteBuffer.h
@@ -0,0 +1,91 @@
+#ifndef __CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+#define __CRIO_SHARED_LIBS_BYTE_BUFFER_H_
+
+#include "aos/common/network/ReceiveSocket.h"
+#include <algorithm>
+
+namespace aos {
+
+class ByteBuffer {
+ public:
+ int m_size;
+ int m_length;
+ int m_i;
+ char *m_buffer;
+ bool recv_from_sock(ReceiveSocket *sock) {
+ m_length = sock->Recv(m_buffer, m_size, 40000);
+ if (m_length < 0) {
+ m_length = 0;
+ }
+ m_i = 0;
+ return m_length != 0;
+ }
+ ByteBuffer(int size) {
+ m_buffer = new char(size);
+ m_size = size;
+ }
+ ~ByteBuffer() {
+ delete m_buffer;
+ }
+ // Reads an uint32_t into *number and returns true on success. *number is
+ // unmodified on failure.
+ bool read_uint32(uint32_t *number) {
+ uint32_t vals[4];
+ if (m_i + 4 > m_length) {
+ m_i = m_length;
+ return false;
+ }
+ for (int i = 0; i < 4; ++i) {
+ vals[i] = read_char();
+ }
+ *number = vals[3] + (vals[2] << 8) + (vals[1] << 16) + (vals[0] << 24);
+ return true;
+ }
+ float read_float() {
+ if (m_i + 4 <= m_length) {
+ float r;
+ memcpy(&r, &m_buffer[m_i], 4);
+ m_i += 4;
+ return r;
+ } else {
+ return 1.0 / 0.0;
+ }
+ }
+ int read_char() {
+ if (m_i < m_length) {
+ int val = m_buffer[m_i];
+ m_i ++;
+ return val;
+ } else {
+ return -1;
+ }
+ }
+
+ int read_string(char *buf, size_t max_len) {
+ int data_len = read_char();
+ if (data_len <= 0) {
+ return -1;
+ }
+ size_t to_read = std::min<size_t>(static_cast<uint8_t>(data_len), max_len);
+ memcpy(buf, &m_buffer[m_i], to_read);
+ m_i += to_read;
+ return 0;
+ }
+ // Returns success or not.
+ bool read_bytes(void *buf, size_t bytes) {
+ if (m_length - m_i < static_cast<ssize_t>(bytes)) return false;
+ memcpy(buf, &m_buffer[m_i], bytes);
+ m_i += bytes;
+ return true;
+ }
+ char *get_bytes(size_t number) {
+ if (m_length - m_i < static_cast<ssize_t>(number)) return NULL;
+ m_i += number;
+ return &m_buffer[m_i - number];
+ }
+};
+
+} // namespace aos
+
+#endif
+
diff --git a/aos/crio/shared_libs/interrupt_bridge-tmpl.h b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
new file mode 100644
index 0000000..3adba38
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge-tmpl.h
@@ -0,0 +1,243 @@
+#include <sysLib.h>
+#include <usrLib.h>
+#include <sigevent.h>
+
+#include "WPILib/Task.h"
+
+#include "aos/aos_core.h"
+#include "aos/crio/motor_server/MotorServer.h"
+#include "aos/common/time.h"
+
+extern "C" {
+// A really simple function implemented in a .c file because the header that
+// declares the variable doesn't work in C++.
+// Returns the value of the global variable excExtendedVectors declared by
+// <usrConfig.h>.
+int aos_getExcExtendedVectors();
+}
+
+namespace aos {
+namespace crio {
+
+// Conceptually a TimerNotifier*[]. Used by the signal handler so that it can
+// figure out which instance it's supposed to be using.
+// Doesn't need a lock because a value is put in here before the signal handler
+// is set up, so everything that might have concurrency issues will be reads
+// which will happen after the set to each index.
+// Declared like this instead of as an actual array because its size might be
+// determined dynamically (SIGRTMAX and SIGRTMIN can be function calls).
+extern void **const timer_notifiers;
+
+template<typename T>
+InterruptBridge<T>::InterruptBridge(Handler handler,
+ T *param, int priority)
+ : handler_(handler),
+ param_(param),
+ task_(new Task("_NotifierLooper", reinterpret_cast<FUNCPTR>(HandlerLoop),
+ priority)),
+ sem_(semBCreate(SEM_Q_PRIORITY, SEM_EMPTY)) {
+ // Basically, makes sure that it does -mlongcall for calling interrupt handler
+ // functions.
+ // See <http://www.vxdev.com/docs/vx55man/vxworks/ppc/powerpc.html#91321> for
+ // details about why this is important.
+ if (!aos_getExcExtendedVectors()) {
+ LOG(FATAL, "extended-call interrupts are not enabled\n");
+ }
+}
+
+template<typename T>
+InterruptBridge<T>::~InterruptBridge() {
+ // Can't call Stop() because that calls StopNotifications(), which is virtual,
+ // so you can't call it after the subclass's destructor has run.
+ semDelete(sem_);
+}
+
+template<typename T>
+void InterruptBridge<T>::StartTask() {
+ // The inner cast to InterruptBridge<T>* is so that Loop knows what to
+ // cast the void* to (the implementation of polymorphism means that
+ // casting the pointer might actually change its value).
+ task_->Start(reinterpret_cast<UINT32>(
+ static_cast<InterruptBridge<T> *>(this)));
+}
+
+template<typename T>
+void InterruptBridge<T>::Stop() {
+ StopNotifications();
+ task_->Stop();
+}
+
+template<typename T>
+void InterruptBridge<T>::HandlerLoop(void *self_in) {
+ InterruptBridge<T> *const self = static_cast<InterruptBridge<T> *>(self_in);
+ while (true) {
+ semTake(self->sem_, WAIT_FOREVER);
+ self->handler_(self->param_);
+ }
+}
+
+template<typename T>
+PeriodicNotifier<T>::PeriodicNotifier(
+ typename InterruptBridge<T>::Handler handler,
+ T *param, int priority)
+ : InterruptBridge<T>(handler, param, priority) {}
+
+template<typename T>
+void PeriodicNotifier<T>::StartPeriodic(double period) {
+ this->StartTask();
+ StartNotifications(period);
+}
+
+template<typename T>
+TimerNotifier<T>::TimerNotifier(typename InterruptBridge<T>::Handler handler,
+ T *param, int unique, int priority)
+ : PeriodicNotifier<T>(handler, param, priority),
+ signal_(SIGRTMIN + unique) {
+ timer_notifiers[signal_] = static_cast<void *>(this);
+
+ struct sigaction sa;
+ sa.sa_flags = 0;
+ sa.sa_handler = StaticNotify;
+ sigemptyset(&sa.sa_mask);
+ if (sigaction(signal_, &sa, &old_sa_) != OK) {
+ LOG(FATAL, "sigaction(%d, %p, %p) failed with %d: %s\n",
+ signal_, &sa, &old_sa_, errno, strerror(errno));
+ }
+
+ sigevent event;
+ event.sigev_notify = SIGEV_TASK_SIGNAL;
+ event.sigev_signo = signal_;
+ event.sigev_value.sival_ptr = this;
+ if (timer_create(CLOCK_REALTIME, &event, &timer_) != OK) {
+ LOG(FATAL, "timer_create(CLOCK_REALTIME, %p, %p) failed with %d: %s\n",
+ &event, &timer_, errno, strerror(errno));
+ }
+}
+
+template<typename T>
+TimerNotifier<T>::~TimerNotifier() {
+ if (timer_delete(timer_) != OK) {
+ LOG(FATAL, "timer_delete(%p) failed with %d: %s\n", timer_,
+ errno, strerror(errno));
+ }
+ if (sigaction(signal_, &old_sa_, NULL) != OK) {
+ LOG(FATAL, "sigaction(%d, %p, NULL) failed with %d: %s\n",
+ signal_, &old_sa_, errno, strerror(errno));
+ }
+ StopNotifications();
+}
+
+template<typename T>
+void TimerNotifier<T>::StartNotifications(double period) {
+ itimerspec timer_spec;
+ timer_spec.it_value.tv_sec = 0;
+ timer_spec.it_value.tv_nsec = 1; // 0 would mean to disarm the timer
+ timer_spec.it_interval = time::Time::InSeconds(period).ToTimespec();
+ if (timer_settime(timer_, 0, &timer_spec, NULL) != OK) {
+ LOG(FATAL, "timer_settime(%p, 0, %p, NULL) failed with %d: %s\n",
+ timer_, &timer_spec, errno, strerror(errno));
+ }
+}
+
+template<typename T>
+void TimerNotifier<T>::StopNotifications() {
+ timer_cancel(timer_);
+}
+
+template<typename T>
+WDInterruptNotifier<T>::WDInterruptNotifier(
+ typename InterruptBridge<T>::Handler handler, T *param, int priority)
+ : PeriodicNotifier<T>(handler, param, priority), wd_(wdCreate()) {
+ if (wd_ == NULL) {
+ LOG(FATAL, "wdCreate() failed with %d: %s\n", errno, strerror(errno));
+ }
+}
+
+template<typename T>
+WDInterruptNotifier<T>::~WDInterruptNotifier() {
+ if (wdDelete(wd_) != OK) {
+ LOG(FATAL, "wdDelete(%p) failed with %d: %s\n",
+ wd_, errno, strerror(errno));
+ }
+ StopNotifications();
+}
+
+template<typename T>
+void WDInterruptNotifier<T>::StartNotifications(double period) {
+ delay_ = time::Time::InSeconds(period).ToTicks();
+
+ if (wdStart(wd_,
+ 1, // run it really soon
+ (FUNCPTR)StaticNotify,
+ (UINT32)this) != OK) {
+ LOG(FATAL, "wdStart(%p) failed with %d: %s", wd_, errno, strerror(errno));
+ }
+}
+
+template<typename T>
+void WDInterruptNotifier<T>::StopNotifications() {
+ wdCancel(wd_);
+}
+
+// THESE GET CALLED AT INTERRUPT LEVEL. BE CAREFUL CHANGING THEM!!
+//
+// It might be tempting to use floating point math here, but DON'T!
+//
+// Also, do NOT use 64-bit integers (at least not without checking the assembly
+// code again). GCC optimizes those using the floating point registers (see
+// <http://compgroups.net/comp.os.vxworks/int64-where-gcc-ppc-and-vxworks-don-t-play-we/1110156>
+// for an example of that being a problem).
+//
+// The assembly code comments are in there to make looking at assembly code
+// dumps to verify that there aren't any floating point instructions easier.
+// brians did that on 10/14/12 to his then-uncommited code and didn't find any.
+// For future reference, here is a list of powerpc instruction mnemonics (what
+// g++ -S gives) that do not involve any floating point:
+// lwz
+// lis
+// mflr
+// la
+// stwu
+// stw
+// mr
+// mtctr
+// bctrl
+// addi
+// mtlr
+// blr
+// cmpwi
+// bne
+// li
+// NOTE: This macro is used in interrupt_notifier-tmpl.h too.
+#define ASM_COMMENT(str) __asm__("# " str)
+template<typename T>
+void WDInterruptNotifier<T>::StaticNotify(void *self_in) {
+ ASM_COMMENT("beginning of WDInterruptNotifier::StaticNotify");
+ WDInterruptNotifier<T> *const self =
+ static_cast<WDInterruptNotifier<T> *>(self_in);
+ wdStart(self->wd_,
+ self->delay_,
+ (FUNCPTR)StaticNotify,
+ (UINT32)self); // call the same thing again
+ self->Notify();
+ ASM_COMMENT("end of WDInterruptNotifier::StaticNotify");
+}
+
+template<typename T>
+void TimerNotifier<T>::StaticNotify(int signum) {
+ ASM_COMMENT("beginning of TimerNotifier::StaticNotify");
+ TimerNotifier<T> *const self =
+ static_cast<TimerNotifier<T> *>(timer_notifiers[signum]);
+ self->Notify();
+ ASM_COMMENT("end of TimerNotifier::StaticNotify");
+}
+
+template<typename T>
+void InterruptBridge<T>::Notify() {
+ ASM_COMMENT("beginning of InterruptBridge::Notify");
+ semGive(sem_);
+ ASM_COMMENT("end of InterruptBridge::Notify");
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_bridge.cc b/aos/crio/shared_libs/interrupt_bridge.cc
new file mode 100644
index 0000000..a905c55
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge.cc
@@ -0,0 +1,9 @@
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+
+namespace aos {
+namespace crio {
+
+void **const timer_notifiers = new void *[SIGRTMAX - SIGRTMIN];
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_bridge.h b/aos/crio/shared_libs/interrupt_bridge.h
new file mode 100644
index 0000000..85ecd0f
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge.h
@@ -0,0 +1,140 @@
+#ifndef AOS_CRIO_SHARED_LIBS_INTERRUPT_BRIDGE_H_
+#define AOS_CRIO_SHARED_LIBS_INTERRUPT_BRIDGE_H_
+
+#include <wdLib.h>
+#include <semLib.h>
+#include <timers.h>
+#include <signal.h>
+
+#include "aos/common/scoped_ptr.h"
+
+#include "aos/aos_core.h"
+
+class Task;
+
+namespace aos {
+namespace crio {
+
+// Handles creating a separate Task at a high priority with a semaphore to
+// link it to something else that provides timing information (likely in an ISR
+// context).
+template<typename T>
+class InterruptBridge {
+ public:
+ // The default priority. Here as a constant for subclasses to use as a default
+ // too.
+ static const int kDefaultPriority = 96;
+ typedef void (*Handler)(T *param);
+
+ // Stops calling the handler at all until a Start function is called again.
+ void Stop();
+
+ protected:
+ // < 95 priority does not work, and < 100 isn't guaranteed to work
+ InterruptBridge(Handler handler, T *param, int priority);
+ // Subclasses should call StopNotifications.
+ virtual ~InterruptBridge();
+
+ // Subclasses should call this whenever the Notifier triggers.
+ // It is safe to call from an ISR.
+ void Notify();
+ // Starts the actual Task.
+ void StartTask();
+
+ private:
+ const Handler handler_;
+ T *const param_;
+
+ // Subclasses should do whatever they have to to stop calling Notify().
+ virtual void StopNotifications() = 0;
+
+ // The function that the Task runs.
+ // Loops forever, waiting for sem_ and then calling handler_.
+ static void HandlerLoop(void *self_in);
+ const scoped_ptr<Task> task_;
+ // For synchronizing between the Task and Notify().
+ SEM_ID sem_;
+ DISALLOW_COPY_AND_ASSIGN(InterruptBridge<T>);
+};
+
+// An accurate version of WPILib's Notifier class.
+template<typename T>
+class PeriodicNotifier : public InterruptBridge<T> {
+ public:
+ // Period is how much (in seconds) to wait between running the handler.
+ void StartPeriodic(double period);
+
+ protected:
+ PeriodicNotifier(typename InterruptBridge<T>::Handler handler, T *param,
+ int priority);
+ virtual ~PeriodicNotifier() {}
+
+ private:
+ virtual void StopNotifications() = 0;
+ // Subclasses should do whatever they have to to start calling Notify() every
+ // period seconds.
+ virtual void StartNotifications(double period) = 0;
+};
+
+// This one works accurately, but it has the potential to drift over time.
+// It has only sysClockRateGet() resolution.
+template<typename T>
+class WDInterruptNotifier : public PeriodicNotifier<T> {
+ public:
+ WDInterruptNotifier(typename InterruptBridge<T>::Handler handler,
+ T *param = NULL,
+ int priority = InterruptBridge<T>::kDefaultPriority);
+ virtual ~WDInterruptNotifier();
+
+ private:
+ // The argument is the general callback parameter which will be a pointer to
+ // an instance. This function calls Notify() on that instance.
+ static void StaticNotify(void *self_in);
+ virtual void StopNotifications();
+ virtual void StartNotifications(double period);
+
+ WDOG_ID wd_;
+ int delay_; // what to pass to wdStart
+ DISALLOW_COPY_AND_ASSIGN(WDInterruptNotifier<T>);
+};
+
+// Vxworks (really really) should take care of making sure that this one
+// doesn't drift over time, but IT DOESN'T!! Brian found the implementation
+// online (file timerLib.c), and it's just doing the same thing as
+// WDInterruptNotifier, except with extra conversions and overhead to implement
+// the POSIX standard. There really is no reason to use this.
+// It has only sysClockRateGet() resolution.
+template<typename T>
+class TimerNotifier : public PeriodicNotifier<T> {
+ public:
+ // unique should be different for all instances created in the same Task. It
+ // can range from 0 to (SIGRTMAX - SIGRTMIN). This instance will use the
+ // signal (SIGRTMIN + unique), so nothing else should use that signal.
+ TimerNotifier(typename InterruptBridge<T>::Handler handler,
+ T *param = NULL,
+ int unique = 0,
+ int priority = InterruptBridge<T>::kDefaultPriority);
+ virtual ~TimerNotifier();
+
+ private:
+ // The first argument is the signal number that is being triggered. This
+ // function looks up a pointer to an instance in timer_notifiers using that
+ // and calls Notify() on that instance.
+ static void StaticNotify(int signum);
+ virtual void StopNotifications();
+ virtual void StartNotifications(double period);
+
+ timer_t timer_;
+ // Which signal timer_ will notify on.
+ const int signal_;
+ // What the action for signal_ was before we started messing with it.
+ struct sigaction old_sa_;
+ DISALLOW_COPY_AND_ASSIGN(TimerNotifier<T>);
+};
+
+} // namespace crio
+} // namespace aos
+
+#include "aos/crio/shared_libs/interrupt_bridge-tmpl.h"
+
+#endif // AOS_CRIO_SHARED_LIBS_INTERRUPT_BRIDGE_H_
diff --git a/aos/crio/shared_libs/interrupt_bridge_c.c b/aos/crio/shared_libs/interrupt_bridge_c.c
new file mode 100644
index 0000000..7a74ea1
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge_c.c
@@ -0,0 +1,9 @@
+#include <vxWorksCommon.h> // or else it blows up...
+// This header ends up trying to #include a header which declares a struct with
+// a member named "delete". This means that it can not be used from code
+// compiled with the C++ compiler.
+#include <usrConfig.h>
+
+int aos_getExcExtendedVectors(void) {
+ return excExtendedVectors;
+}
diff --git a/aos/crio/shared_libs/interrupt_bridge_demo.cc b/aos/crio/shared_libs/interrupt_bridge_demo.cc
new file mode 100644
index 0000000..d7e66be
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_bridge_demo.cc
@@ -0,0 +1,45 @@
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+
+#include <unistd.h>
+
+#include "WPILib/Timer.h"
+
+#include "aos/aos_core.h"
+#include "aos/common/time.h"
+
+using aos::time::Time;
+
+namespace aos {
+namespace crio {
+
+class InterruptBridgeTest {
+ public:
+ void Run(double seconds) {
+ int interruptVal = 1;
+ WDInterruptNotifier<int> interrupt(Notify, &interruptVal);
+ interrupt.StartPeriodic(0.01);
+ int timerVal = 2;
+ TimerNotifier<int> timer(Notify, &timerVal);
+ time::SleepFor(Time::InSeconds(0.005)); // get them offset by ~180 degrees
+ timer.StartPeriodic(0.01);
+ time::SleepFor(Time::InSeconds(seconds));
+ }
+ private:
+ static void Notify(int *value) {
+ printf("notified %f,%d\n", Timer::GetFPGATimestamp(), *value);
+ }
+};
+
+} // namespace crio
+} // namespace aos
+
+// Designed to be called from the vxworks shell if somebody wants to run this.
+extern "C" int interrupt_bridge_demo(int seconds) {
+ if (seconds == 0) {
+ printf("arguments: number of seconds\n");
+ return -1;
+ }
+ aos::crio::InterruptBridgeTest runner;
+ runner.Run(seconds);
+ return 0;
+}
diff --git a/aos/crio/shared_libs/interrupt_notifier-tmpl.h b/aos/crio/shared_libs/interrupt_notifier-tmpl.h
new file mode 100644
index 0000000..78e7ca8
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_notifier-tmpl.h
@@ -0,0 +1,48 @@
+#include <intLib.h>
+#include <logLib.h>
+
+namespace aos {
+namespace crio {
+
+template<typename T>
+InterruptNotifier<T>::InterruptNotifier(
+ typename InterruptBridge<T>::Handler handler,
+ InterruptableSensorBase *sensor, T *param, int priority)
+ : InterruptBridge<T>(handler, param, priority), sensor_(sensor) {
+ sensor_->RequestInterrupts(StaticNotify, this);
+}
+
+template<typename T>
+InterruptNotifier<T>::~InterruptNotifier() {
+ sensor_->CancelInterrupts();
+}
+
+template<typename T>
+void InterruptNotifier<T>::Start() {
+ this->StartTask();
+ sensor_->EnableInterrupts();
+}
+
+template<typename T>
+void InterruptNotifier<T>::StopNotifications() {
+ sensor_->DisableInterrupts();
+}
+
+// WARNING: This IS called from an ISR. Don't use floating point. Look in
+// interrupt_bridge-tmpl.h for details.
+template<typename T>
+void InterruptNotifier<T>::StaticNotify(uint32_t, void *self_in) {
+ ASM_COMMENT("beginning of InterruptNotifier::StaticNotify");
+ if (!intContext()) { // if we're not in an actual ISR
+ logMsg(const_cast<char *>("WPILib is not calling callbacks"
+ " in actual ISRs any more!!\n"),
+ 0, 0, 0, 0, 0, 0);
+ }
+ InterruptNotifier<T> *const self =
+ static_cast<InterruptNotifier<T> *>(self_in);
+ self->Notify();
+ ASM_COMMENT("end of InterruptNotifier::StaticNotify");
+}
+
+} // namespace crio
+} // namespace aos
diff --git a/aos/crio/shared_libs/interrupt_notifier.h b/aos/crio/shared_libs/interrupt_notifier.h
new file mode 100644
index 0000000..87aa4a3
--- /dev/null
+++ b/aos/crio/shared_libs/interrupt_notifier.h
@@ -0,0 +1,49 @@
+#ifndef AOS_CRIO_SHARED_LIBS_INTERRUPT_NOTIFIER_H_
+#define AOS_CRIO_SHARED_LIBS_INTERRUPT_NOTIFIER_H_
+
+#include "WPILib/InterruptableSensorBase.h"
+
+#include "aos/crio/shared_libs/interrupt_bridge.h"
+
+namespace aos {
+namespace crio {
+
+// An InterruptBridge that notifies based on interrupts from a WPILib
+// InterruptableSensorBase object (which DigitalInput is an example of).
+template<typename T>
+class InterruptNotifier : public InterruptBridge<T> {
+ public:
+ // This object will hold a reference to sensor, but will not free it. This
+ // object will take ownership of everything related to interrupts for sensor
+ // (ie this constructor will call sensor->RequestInterrupts).
+ // Interrupts should be cancelled (the state InterruptableSensorBases are
+ // constructed in) when this constructor is called.
+ // Any setup of sensor that is required should happen before Start() is
+ // called, but after this constructor (ie SetUpSourceEdge).
+ InterruptNotifier(typename InterruptBridge<T>::Handler handler,
+ InterruptableSensorBase *sensor,
+ T *param = NULL,
+ int priority = InterruptBridge<T>::kDefaultPriority);
+ virtual ~InterruptNotifier();
+
+ // Starts calling the handler whenever the interrupt triggers.
+ void Start();
+
+ private:
+ // The only docs that seem to exist on the first arg is that it's named
+ // interruptAssertedMask in WPILib/ChipObject/tInterruptManager.h
+ // The second arg is the general callback parameter which will be a pointer to
+ // an instance. This function calls Notify() on that instance.
+ static void StaticNotify(uint32_t, void *self_in);
+ virtual void StopNotifications();
+
+ InterruptableSensorBase *const sensor_;
+ DISALLOW_COPY_AND_ASSIGN(InterruptNotifier<T>);
+};
+
+} // namespace crio
+} // namespace aos
+
+#include "aos/crio/shared_libs/interrupt_notifier-tmpl.h"
+
+#endif // AOS_CRIO_SHARED_LIBS_INTERRUPT_NOTIFIER_H_
diff --git a/aos/crio/shared_libs/mutex.cpp b/aos/crio/shared_libs/mutex.cpp
new file mode 100644
index 0000000..35ac4e3
--- /dev/null
+++ b/aos/crio/shared_libs/mutex.cpp
@@ -0,0 +1,48 @@
+#include "aos/common/mutex.h"
+
+#include <semLib.h>
+
+namespace aos {
+
+Mutex::Mutex() : impl_(semBCreate(SEM_Q_PRIORITY, SEM_FULL)) {
+ if (impl_ == NULL) {
+ LOG(FATAL,
+ "semBCreate(SEM_Q_PRIORITY, SEM_FULL) failed because of %d: %s\n",
+ errno, strerror(errno));
+ }
+}
+
+Mutex::~Mutex() {
+ if (semDelete(impl_) != 0) {
+ LOG(FATAL, "semDelete(%p) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+}
+
+void Mutex::Lock() {
+ if (semTake(impl_, WAIT_FOREVER) != 0) {
+ LOG(FATAL, "semTake(%p, WAIT_FOREVER) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+}
+
+void Mutex::Unlock() {
+ if (semGive(impl_) != 0) {
+ LOG(FATAL, "semGive(%p) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+}
+
+bool Mutex::TryLock() {
+ if (semTake(impl_, NO_WAIT) == 0) {
+ return true;
+ }
+ // The semLib documention is wrong about what the errno will be.
+ if (errno != S_objLib_OBJ_UNAVAILABLE) {
+ LOG(FATAL, "semTake(%p, WAIT_FOREVER) failed because of %d: %s\n",
+ impl_, errno, strerror(errno));
+ }
+ return false;
+}
+
+} // namespace aos
diff --git a/aos/crio/type_traits/tr1_impl/type_traits b/aos/crio/type_traits/tr1_impl/type_traits
new file mode 100644
index 0000000..47f5e8f
--- /dev/null
+++ b/aos/crio/type_traits/tr1_impl/type_traits
@@ -0,0 +1,502 @@
+// TR1 type_traits -*- C++ -*-
+
+// Copyright (C) 2007, 2008, 2009 Free Software Foundation, Inc.
+//
+// This file is part of the GNU ISO C++ Library. This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 3, 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 General Public License for more details.
+
+// Under Section 7 of GPL version 3, you are granted additional
+// permissions described in the GCC Runtime Library Exception, version
+// 3.1, as published by the Free Software Foundation.
+
+// You should have received a copy of the GNU General Public License and
+// a copy of the GCC Runtime Library Exception along with this program;
+// see the files COPYING3 and COPYING.RUNTIME respectively. If not, see
+// <http://www.gnu.org/licenses/>.
+
+/** @file tr1_impl/type_traits
+* This is an internal header file, included by other library headers.
+* You should not attempt to use it directly.
+*/
+
+namespace std
+{
+_GLIBCXX_BEGIN_NAMESPACE_TR1
+
+ /**
+ * @defgroup metaprogramming Type Traits
+ * @ingroup utilities
+ *
+ * Compile time type transformation and information.
+ * @{
+ */
+
+ // For use in __is_convertible_simple.
+ struct __sfinae_types
+ {
+ typedef char __one;
+ typedef struct { char __arr[2]; } __two;
+ };
+
+#define _DEFINE_SPEC_0_HELPER \
+ template<>
+
+#define _DEFINE_SPEC_1_HELPER \
+ template<typename _Tp>
+
+#define _DEFINE_SPEC_2_HELPER \
+ template<typename _Tp, typename _Cp>
+
+#define _DEFINE_SPEC(_Order, _Trait, _Type, _Value) \
+ _DEFINE_SPEC_##_Order##_HELPER \
+ struct _Trait<_Type> \
+ : public integral_constant<bool, _Value> { };
+
+ // helper classes [4.3].
+
+ /// integral_constant
+ template<typename _Tp, _Tp __v>
+ struct integral_constant
+ {
+ static const _Tp value = __v;
+ typedef _Tp value_type;
+ typedef integral_constant<_Tp, __v> type;
+ };
+
+ /// typedef for true_type
+ typedef integral_constant<bool, true> true_type;
+
+ /// typedef for false_type
+ typedef integral_constant<bool, false> false_type;
+
+ template<typename _Tp, _Tp __v>
+ const _Tp integral_constant<_Tp, __v>::value;
+
+ /// remove_cv
+ template<typename>
+ struct remove_cv;
+
+ template<typename>
+ struct __is_void_helper
+ : public false_type { };
+ _DEFINE_SPEC(0, __is_void_helper, void, true)
+
+ // primary type categories [4.5.1].
+
+ /// is_void
+ template<typename _Tp>
+ struct is_void
+ : public integral_constant<bool, (__is_void_helper<typename
+ remove_cv<_Tp>::type>::value)>
+ { };
+
+ template<typename>
+ struct __is_integral_helper
+ : public false_type { };
+ _DEFINE_SPEC(0, __is_integral_helper, bool, true)
+ _DEFINE_SPEC(0, __is_integral_helper, char, true)
+ _DEFINE_SPEC(0, __is_integral_helper, signed char, true)
+ _DEFINE_SPEC(0, __is_integral_helper, unsigned char, true)
+#ifdef _GLIBCXX_USE_WCHAR_T
+ _DEFINE_SPEC(0, __is_integral_helper, wchar_t, true)
+#endif
+#ifdef _GLIBCXX_INCLUDE_AS_CXX0X
+ _DEFINE_SPEC(0, __is_integral_helper, char16_t, true)
+ _DEFINE_SPEC(0, __is_integral_helper, char32_t, true)
+#endif
+ _DEFINE_SPEC(0, __is_integral_helper, short, true)
+ _DEFINE_SPEC(0, __is_integral_helper, unsigned short, true)
+ _DEFINE_SPEC(0, __is_integral_helper, int, true)
+ _DEFINE_SPEC(0, __is_integral_helper, unsigned int, true)
+ _DEFINE_SPEC(0, __is_integral_helper, long, true)
+ _DEFINE_SPEC(0, __is_integral_helper, unsigned long, true)
+ _DEFINE_SPEC(0, __is_integral_helper, long long, true)
+ _DEFINE_SPEC(0, __is_integral_helper, unsigned long long, true)
+
+ /// is_integral
+ template<typename _Tp>
+ struct is_integral
+ : public integral_constant<bool, (__is_integral_helper<typename
+ remove_cv<_Tp>::type>::value)>
+ { };
+
+ template<typename>
+ struct __is_floating_point_helper
+ : public false_type { };
+ _DEFINE_SPEC(0, __is_floating_point_helper, float, true)
+ _DEFINE_SPEC(0, __is_floating_point_helper, double, true)
+ _DEFINE_SPEC(0, __is_floating_point_helper, long double, true)
+
+ /// is_floating_point
+ template<typename _Tp>
+ struct is_floating_point
+ : public integral_constant<bool, (__is_floating_point_helper<typename
+ remove_cv<_Tp>::type>::value)>
+ { };
+
+ /// is_array
+ template<typename>
+ struct is_array
+ : public false_type { };
+
+ template<typename _Tp, std::size_t _Size>
+ struct is_array<_Tp[_Size]>
+ : public true_type { };
+
+ template<typename _Tp>
+ struct is_array<_Tp[]>
+ : public true_type { };
+
+ template<typename>
+ struct __is_pointer_helper
+ : public false_type { };
+ _DEFINE_SPEC(1, __is_pointer_helper, _Tp*, true)
+
+ /// is_pointer
+ template<typename _Tp>
+ struct is_pointer
+ : public integral_constant<bool, (__is_pointer_helper<typename
+ remove_cv<_Tp>::type>::value)>
+ { };
+
+ /// is_reference
+ template<typename _Tp>
+ struct is_reference;
+
+ /// is_function
+ template<typename _Tp>
+ struct is_function;
+
+ template<typename>
+ struct __is_member_object_pointer_helper
+ : public false_type { };
+ _DEFINE_SPEC(2, __is_member_object_pointer_helper, _Tp _Cp::*,
+ !is_function<_Tp>::value)
+
+ /// is_member_object_pointer
+ template<typename _Tp>
+ struct is_member_object_pointer
+ : public integral_constant<bool, (__is_member_object_pointer_helper<
+ typename remove_cv<_Tp>::type>::value)>
+ { };
+
+ template<typename>
+ struct __is_member_function_pointer_helper
+ : public false_type { };
+ _DEFINE_SPEC(2, __is_member_function_pointer_helper, _Tp _Cp::*,
+ is_function<_Tp>::value)
+
+ /// is_member_function_pointer
+ template<typename _Tp>
+ struct is_member_function_pointer
+ : public integral_constant<bool, (__is_member_function_pointer_helper<
+ typename remove_cv<_Tp>::type>::value)>
+ { };
+
+ /// is_enum
+ template<typename _Tp>
+ struct is_enum
+ : public integral_constant<bool, __is_enum(_Tp)>
+ { };
+
+ /// is_union
+ template<typename _Tp>
+ struct is_union
+ : public integral_constant<bool, __is_union(_Tp)>
+ { };
+
+ /// is_class
+ template<typename _Tp>
+ struct is_class
+ : public integral_constant<bool, __is_class(_Tp)>
+ { };
+
+ /// is_function
+ template<typename>
+ struct is_function
+ : public false_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes...)>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes......)>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes...) const>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes......) const>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes...) volatile>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes......) volatile>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes...) const volatile>
+ : public true_type { };
+ template<typename _Res, typename... _ArgTypes>
+ struct is_function<_Res(_ArgTypes......) const volatile>
+ : public true_type { };
+
+ // composite type traits [4.5.2].
+
+ /// is_arithmetic
+ template<typename _Tp>
+ struct is_arithmetic
+ : public integral_constant<bool, (is_integral<_Tp>::value
+ || is_floating_point<_Tp>::value)>
+ { };
+
+ /// is_fundamental
+ template<typename _Tp>
+ struct is_fundamental
+ : public integral_constant<bool, (is_arithmetic<_Tp>::value
+ || is_void<_Tp>::value)>
+ { };
+
+ /// is_object
+ template<typename _Tp>
+ struct is_object
+ : public integral_constant<bool, !(is_function<_Tp>::value
+ || is_reference<_Tp>::value
+ || is_void<_Tp>::value)>
+ { };
+
+ /// is_member_pointer
+ template<typename _Tp>
+ struct is_member_pointer;
+
+ /// is_scalar
+ template<typename _Tp>
+ struct is_scalar
+ : public integral_constant<bool, (is_arithmetic<_Tp>::value
+ || is_enum<_Tp>::value
+ || is_pointer<_Tp>::value
+ || is_member_pointer<_Tp>::value)>
+ { };
+
+ /// is_compound
+ template<typename _Tp>
+ struct is_compound
+ : public integral_constant<bool, !is_fundamental<_Tp>::value> { };
+
+ /// is_member_pointer
+ template<typename _Tp>
+ struct __is_member_pointer_helper
+ : public false_type { };
+ _DEFINE_SPEC(2, __is_member_pointer_helper, _Tp _Cp::*, true)
+
+ template<typename _Tp>
+ struct is_member_pointer
+ : public integral_constant<bool, (__is_member_pointer_helper<
+ typename remove_cv<_Tp>::type>::value)>
+ { };
+
+ // type properties [4.5.3].
+ /// is_const
+ template<typename>
+ struct is_const
+ : public false_type { };
+
+ template<typename _Tp>
+ struct is_const<_Tp const>
+ : public true_type { };
+
+ /// is_volatile
+ template<typename>
+ struct is_volatile
+ : public false_type { };
+
+ template<typename _Tp>
+ struct is_volatile<_Tp volatile>
+ : public true_type { };
+
+ /// is_empty
+ template<typename _Tp>
+ struct is_empty
+ : public integral_constant<bool, __is_empty(_Tp)>
+ { };
+
+ /// is_polymorphic
+ template<typename _Tp>
+ struct is_polymorphic
+ : public integral_constant<bool, __is_polymorphic(_Tp)>
+ { };
+
+ /// is_abstract
+ template<typename _Tp>
+ struct is_abstract
+ : public integral_constant<bool, __is_abstract(_Tp)>
+ { };
+
+ /// has_virtual_destructor
+ template<typename _Tp>
+ struct has_virtual_destructor
+ : public integral_constant<bool, __has_virtual_destructor(_Tp)>
+ { };
+
+ /// alignment_of
+ template<typename _Tp>
+ struct alignment_of
+ : public integral_constant<std::size_t, __alignof__(_Tp)> { };
+
+ /// rank
+ template<typename>
+ struct rank
+ : public integral_constant<std::size_t, 0> { };
+
+ template<typename _Tp, std::size_t _Size>
+ struct rank<_Tp[_Size]>
+ : public integral_constant<std::size_t, 1 + rank<_Tp>::value> { };
+
+ template<typename _Tp>
+ struct rank<_Tp[]>
+ : public integral_constant<std::size_t, 1 + rank<_Tp>::value> { };
+
+ /// extent
+ template<typename, unsigned _Uint = 0>
+ struct extent
+ : public integral_constant<std::size_t, 0> { };
+
+ template<typename _Tp, unsigned _Uint, std::size_t _Size>
+ struct extent<_Tp[_Size], _Uint>
+ : public integral_constant<std::size_t,
+ _Uint == 0 ? _Size : extent<_Tp,
+ _Uint - 1>::value>
+ { };
+
+ template<typename _Tp, unsigned _Uint>
+ struct extent<_Tp[], _Uint>
+ : public integral_constant<std::size_t,
+ _Uint == 0 ? 0 : extent<_Tp,
+ _Uint - 1>::value>
+ { };
+
+ // relationships between types [4.6].
+
+ /// is_same
+ template<typename, typename>
+ struct is_same
+ : public false_type { };
+
+ template<typename _Tp>
+ struct is_same<_Tp, _Tp>
+ : public true_type { };
+
+ // const-volatile modifications [4.7.1].
+
+ /// remove_const
+ template<typename _Tp>
+ struct remove_const
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct remove_const<_Tp const>
+ { typedef _Tp type; };
+
+ /// remove_volatile
+ template<typename _Tp>
+ struct remove_volatile
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct remove_volatile<_Tp volatile>
+ { typedef _Tp type; };
+
+ /// remove_cv
+ template<typename _Tp>
+ struct remove_cv
+ {
+ typedef typename
+ remove_const<typename remove_volatile<_Tp>::type>::type type;
+ };
+
+ /// add_const
+ template<typename _Tp>
+ struct add_const
+ { typedef _Tp const type; };
+
+ /// add_volatile
+ template<typename _Tp>
+ struct add_volatile
+ { typedef _Tp volatile type; };
+
+ /// add_cv
+ template<typename _Tp>
+ struct add_cv
+ {
+ typedef typename
+ add_const<typename add_volatile<_Tp>::type>::type type;
+ };
+
+ // array modifications [4.7.3].
+
+ /// remove_extent
+ template<typename _Tp>
+ struct remove_extent
+ { typedef _Tp type; };
+
+ template<typename _Tp, std::size_t _Size>
+ struct remove_extent<_Tp[_Size]>
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct remove_extent<_Tp[]>
+ { typedef _Tp type; };
+
+ /// remove_all_extents
+ template<typename _Tp>
+ struct remove_all_extents
+ { typedef _Tp type; };
+
+ template<typename _Tp, std::size_t _Size>
+ struct remove_all_extents<_Tp[_Size]>
+ { typedef typename remove_all_extents<_Tp>::type type; };
+
+ template<typename _Tp>
+ struct remove_all_extents<_Tp[]>
+ { typedef typename remove_all_extents<_Tp>::type type; };
+
+ // pointer modifications [4.7.4].
+
+ template<typename _Tp, typename>
+ struct __remove_pointer_helper
+ { typedef _Tp type; };
+
+ template<typename _Tp, typename _Up>
+ struct __remove_pointer_helper<_Tp, _Up*>
+ { typedef _Up type; };
+
+ /// remove_pointer
+ template<typename _Tp>
+ struct remove_pointer
+ : public __remove_pointer_helper<_Tp, typename remove_cv<_Tp>::type>
+ { };
+
+ template<typename>
+ struct remove_reference;
+
+ /// add_pointer
+ template<typename _Tp>
+ struct add_pointer
+ { typedef typename remove_reference<_Tp>::type* type; };
+
+#undef _DEFINE_SPEC_0_HELPER
+#undef _DEFINE_SPEC_1_HELPER
+#undef _DEFINE_SPEC_2_HELPER
+#undef _DEFINE_SPEC
+
+ // @} group metaprogramming
+
+_GLIBCXX_END_NAMESPACE_TR1
+}
diff --git a/aos/crio/type_traits/type_traits b/aos/crio/type_traits/type_traits
new file mode 100644
index 0000000..afef722
--- /dev/null
+++ b/aos/crio/type_traits/type_traits
@@ -0,0 +1,634 @@
+// C++0x type_traits -*- C++ -*-
+
+// Copyright (C) 2007, 2008, 2009, 2010 Free Software Foundation, Inc.
+//
+// This file is part of the GNU ISO C++ Library. This library is free
+// software; you can redistribute it and/or modify it under the
+// terms of the GNU General Public License as published by the
+// Free Software Foundation; either version 3, 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 General Public License for more details.
+
+// Under Section 7 of GPL version 3, you are granted additional
+// permissions described in the GCC Runtime Library Exception, version
+// 3.1, as published by the Free Software Foundation.
+
+// You should have received a copy of the GNU General Public License and
+// a copy of the GCC Runtime Library Exception along with this program;
+// see the files COPYING3 and COPYING.RUNTIME respectively. If not, see
+// <http://www.gnu.org/licenses/>.
+
+/** @file include/type_traits
+ * This is a Standard C++ Library header.
+ */
+
+#ifndef _GLIBCXX_TYPE_TRAITS
+#define _GLIBCXX_TYPE_TRAITS 1
+
+#pragma GCC system_header
+
+#ifndef __GXX_EXPERIMENTAL_CXX0X__
+# include <bits/c++0x_warning.h>
+#else
+
+#if defined(_GLIBCXX_INCLUDE_AS_TR1)
+# error C++0x header cannot be included from TR1 header
+#endif
+
+#include <cstddef>
+
+#if defined(_GLIBCXX_INCLUDE_AS_CXX0X)
+# include "aos/crio/type_traits/tr1_impl/type_traits"
+#else
+# define _GLIBCXX_INCLUDE_AS_CXX0X
+# define _GLIBCXX_BEGIN_NAMESPACE_TR1
+# define _GLIBCXX_END_NAMESPACE_TR1
+# define _GLIBCXX_TR1
+# include "aos/crio/type_traits/tr1_impl/type_traits"
+# undef _GLIBCXX_TR1
+# undef _GLIBCXX_END_NAMESPACE_TR1
+# undef _GLIBCXX_BEGIN_NAMESPACE_TR1
+# undef _GLIBCXX_INCLUDE_AS_CXX0X
+#endif
+
+namespace std
+{
+ /**
+ * @addtogroup metaprogramming
+ * @{
+ */
+
+ // Primary classification traits.
+
+ /// is_lvalue_reference
+ template<typename>
+ struct is_lvalue_reference
+ : public false_type { };
+
+ template<typename _Tp>
+ struct is_lvalue_reference<_Tp&>
+ : public true_type { };
+
+ /// is_rvalue_reference
+ template<typename>
+ struct is_rvalue_reference
+ : public false_type { };
+
+ template<typename _Tp>
+ struct is_rvalue_reference<_Tp&&>
+ : public true_type { };
+
+ // Secondary classification traits.
+
+ /// is_reference
+ template<typename _Tp>
+ struct is_reference
+ : public integral_constant<bool, (is_lvalue_reference<_Tp>::value
+ || is_rvalue_reference<_Tp>::value)>
+ { };
+
+ // Reference transformations.
+
+ /// remove_reference
+ template<typename _Tp>
+ struct remove_reference
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct remove_reference<_Tp&>
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct remove_reference<_Tp&&>
+ { typedef _Tp type; };
+
+ template<typename _Tp,
+ bool = !is_reference<_Tp>::value && !is_void<_Tp>::value,
+ bool = is_rvalue_reference<_Tp>::value>
+ struct __add_lvalue_reference_helper
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct __add_lvalue_reference_helper<_Tp, true, false>
+ { typedef _Tp& type; };
+
+ template<typename _Tp>
+ struct __add_lvalue_reference_helper<_Tp, false, true>
+ { typedef typename remove_reference<_Tp>::type& type; };
+
+ /// add_lvalue_reference
+ template<typename _Tp>
+ struct add_lvalue_reference
+ : public __add_lvalue_reference_helper<_Tp>
+ { };
+
+ template<typename _Tp,
+ bool = !is_reference<_Tp>::value && !is_void<_Tp>::value>
+ struct __add_rvalue_reference_helper
+ { typedef _Tp type; };
+
+ template<typename _Tp>
+ struct __add_rvalue_reference_helper<_Tp, true>
+ { typedef _Tp&& type; };
+
+ /// add_rvalue_reference
+ template<typename _Tp>
+ struct add_rvalue_reference
+ : public __add_rvalue_reference_helper<_Tp>
+ { };
+
+ // Scalar properties and transformations.
+
+ template<typename _Tp,
+ bool = is_integral<_Tp>::value,
+ bool = is_floating_point<_Tp>::value>
+ struct __is_signed_helper
+ : public false_type { };
+
+ template<typename _Tp>
+ struct __is_signed_helper<_Tp, false, true>
+ : public true_type { };
+
+ template<typename _Tp>
+ struct __is_signed_helper<_Tp, true, false>
+ : public integral_constant<bool, static_cast<bool>(_Tp(-1) < _Tp(0))>
+ { };
+
+ /// is_signed
+ template<typename _Tp>
+ struct is_signed
+ : public integral_constant<bool, __is_signed_helper<_Tp>::value>
+ { };
+
+ /// is_unsigned
+ template<typename _Tp>
+ struct is_unsigned
+ : public integral_constant<bool, (is_arithmetic<_Tp>::value
+ && !is_signed<_Tp>::value)>
+ { };
+
+ // Member introspection.
+
+ /// is_trivial
+ template<typename _Tp>
+ struct is_trivial
+ : public integral_constant<bool, __is_trivial(_Tp)>
+ { };
+
+ /// is_standard_layout
+ template<typename _Tp>
+ struct is_standard_layout
+ : public integral_constant<bool, __is_standard_layout(_Tp)>
+ { };
+
+ /// is_pod
+ // Could use is_standard_layout && is_trivial instead of the builtin.
+ template<typename _Tp>
+ struct is_pod
+ : public integral_constant<bool, __is_pod(_Tp)>
+ { };
+
+ template<typename _Tp>
+ typename add_rvalue_reference<_Tp>::type declval();
+
+ template<typename _Tp, typename... _Args>
+ class __is_constructible_helper
+ : public __sfinae_types
+ {
+ template<typename _Tp1, typename... _Args1>
+ static decltype(_Tp1(declval<_Args1>()...), __one()) __test(int);
+
+ template<typename, typename...>
+ static __two __test(...);
+
+ public:
+ static const bool __value = sizeof(__test<_Tp, _Args...>(0)) == 1;
+ };
+
+ template<typename _Tp, typename _Arg>
+ class __is_constructible_helper<_Tp, _Arg>
+ : public __sfinae_types
+ {
+ template<typename _Tp1, typename _Arg1>
+ static decltype(static_cast<_Tp1>(declval<_Arg1>()), __one())
+ __test(int);
+
+ template<typename, typename>
+ static __two __test(...);
+
+ public:
+ static const bool __value = sizeof(__test<_Tp, _Arg>(0)) == 1;
+ };
+
+ /// is_constructible
+ // XXX FIXME
+ // The C++0x specifications require front-end support, see N2255.
+ template<typename _Tp, typename... _Args>
+ struct is_constructible
+ : public integral_constant<bool,
+ __is_constructible_helper<_Tp,
+ _Args...>::__value>
+ { };
+
+ /// has_trivial_default_constructor
+ template<typename _Tp>
+ struct has_trivial_default_constructor
+ : public integral_constant<bool, __has_trivial_constructor(_Tp)>
+ { };
+
+ /// has_trivial_copy_constructor
+ template<typename _Tp>
+ struct has_trivial_copy_constructor
+ : public integral_constant<bool, __has_trivial_copy(_Tp)>
+ { };
+
+ /// has_trivial_assign
+ template<typename _Tp>
+ struct has_trivial_assign
+ : public integral_constant<bool, __has_trivial_assign(_Tp)>
+ { };
+
+ /// has_trivial_destructor
+ template<typename _Tp>
+ struct has_trivial_destructor
+ : public integral_constant<bool, __has_trivial_destructor(_Tp)>
+ { };
+
+ /// has_nothrow_default_constructor
+ template<typename _Tp>
+ struct has_nothrow_default_constructor
+ : public integral_constant<bool, __has_nothrow_constructor(_Tp)>
+ { };
+
+ /// has_nothrow_copy_constructor
+ template<typename _Tp>
+ struct has_nothrow_copy_constructor
+ : public integral_constant<bool, __has_nothrow_copy(_Tp)>
+ { };
+
+ /// has_nothrow_assign
+ template<typename _Tp>
+ struct has_nothrow_assign
+ : public integral_constant<bool, __has_nothrow_assign(_Tp)>
+ { };
+
+ // Relationships between types.
+
+ /// is_base_of
+ template<typename _Base, typename _Derived>
+ struct is_base_of
+ : public integral_constant<bool, __is_base_of(_Base, _Derived)>
+ { };
+
+ template<typename _From, typename _To,
+ bool = (is_void<_From>::value || is_void<_To>::value
+ || is_function<_To>::value || is_array<_To>::value)>
+ struct __is_convertible_helper
+ { static const bool __value = (is_void<_From>::value
+ && is_void<_To>::value); };
+
+ template<typename _From, typename _To>
+ class __is_convertible_helper<_From, _To, false>
+ : public __sfinae_types
+ {
+ static __one __test(_To);
+ static __two __test(...);
+
+ public:
+ static const bool __value = sizeof(__test(declval<_From>())) == 1;
+ };
+
+ /// is_convertible
+ // XXX FIXME
+ // The C++0x specifications require front-end support, see N2255.
+ template<typename _From, typename _To>
+ struct is_convertible
+ : public integral_constant<bool,
+ __is_convertible_helper<_From, _To>::__value>
+ { };
+
+ /// is_explicitly_convertible
+ template<typename _From, typename _To>
+ struct is_explicitly_convertible
+ : public is_constructible<_To, _From>
+ { };
+
+ template<std::size_t _Len>
+ struct __aligned_storage_msa
+ {
+ union __type
+ {
+ unsigned char __data[_Len];
+ struct __attribute__((__aligned__)) { } __align;
+ };
+ };
+
+ /**
+ * @brief Alignment type.
+ *
+ * The value of _Align is a default-alignment which shall be the
+ * most stringent alignment requirement for any C++ object type
+ * whose size is no greater than _Len (3.9). The member typedef
+ * type shall be a POD type suitable for use as uninitialized
+ * storage for any object whose size is at most _Len and whose
+ * alignment is a divisor of _Align.
+ */
+ template<std::size_t _Len, std::size_t _Align =
+ __alignof__(typename __aligned_storage_msa<_Len>::__type)>
+ struct aligned_storage
+ {
+ union type
+ {
+ unsigned char __data[_Len];
+ struct __attribute__((__aligned__((_Align)))) { } __align;
+ };
+ };
+
+
+ // Define a nested type if some predicate holds.
+ // Primary template.
+ /// enable_if
+ template<bool, typename _Tp = void>
+ struct enable_if
+ { };
+
+ // Partial specialization for true.
+ template<typename _Tp>
+ struct enable_if<true, _Tp>
+ { typedef _Tp type; };
+
+
+ // A conditional expression, but for types. If true, first, if false, second.
+ // Primary template.
+ /// conditional
+ template<bool _Cond, typename _Iftrue, typename _Iffalse>
+ struct conditional
+ { typedef _Iftrue type; };
+
+ // Partial specialization for false.
+ template<typename _Iftrue, typename _Iffalse>
+ struct conditional<false, _Iftrue, _Iffalse>
+ { typedef _Iffalse type; };
+
+
+ // Decay trait for arrays and functions, used for perfect forwarding
+ // in make_pair, make_tuple, etc.
+ template<typename _Up,
+ bool _IsArray = is_array<_Up>::value,
+ bool _IsFunction = is_function<_Up>::value>
+ struct __decay_selector;
+
+ // NB: DR 705.
+ template<typename _Up>
+ struct __decay_selector<_Up, false, false>
+ { typedef typename remove_cv<_Up>::type __type; };
+
+ template<typename _Up>
+ struct __decay_selector<_Up, true, false>
+ { typedef typename remove_extent<_Up>::type* __type; };
+
+ template<typename _Up>
+ struct __decay_selector<_Up, false, true>
+ { typedef typename add_pointer<_Up>::type __type; };
+
+ /// decay
+ template<typename _Tp>
+ class decay
+ {
+ typedef typename remove_reference<_Tp>::type __remove_type;
+
+ public:
+ typedef typename __decay_selector<__remove_type>::__type type;
+ };
+
+
+ // Utility for constructing identically cv-qualified types.
+ template<typename _Unqualified, bool _IsConst, bool _IsVol>
+ struct __cv_selector;
+
+ template<typename _Unqualified>
+ struct __cv_selector<_Unqualified, false, false>
+ { typedef _Unqualified __type; };
+
+ template<typename _Unqualified>
+ struct __cv_selector<_Unqualified, false, true>
+ { typedef volatile _Unqualified __type; };
+
+ template<typename _Unqualified>
+ struct __cv_selector<_Unqualified, true, false>
+ { typedef const _Unqualified __type; };
+
+ template<typename _Unqualified>
+ struct __cv_selector<_Unqualified, true, true>
+ { typedef const volatile _Unqualified __type; };
+
+ template<typename _Qualified, typename _Unqualified,
+ bool _IsConst = is_const<_Qualified>::value,
+ bool _IsVol = is_volatile<_Qualified>::value>
+ class __match_cv_qualifiers
+ {
+ typedef __cv_selector<_Unqualified, _IsConst, _IsVol> __match;
+
+ public:
+ typedef typename __match::__type __type;
+ };
+
+
+ // Utility for finding the unsigned versions of signed integral types.
+ template<typename _Tp>
+ struct __make_unsigned
+ { typedef _Tp __type; };
+
+ template<>
+ struct __make_unsigned<char>
+ { typedef unsigned char __type; };
+
+ template<>
+ struct __make_unsigned<signed char>
+ { typedef unsigned char __type; };
+
+ template<>
+ struct __make_unsigned<short>
+ { typedef unsigned short __type; };
+
+ template<>
+ struct __make_unsigned<int>
+ { typedef unsigned int __type; };
+
+ template<>
+ struct __make_unsigned<long>
+ { typedef unsigned long __type; };
+
+ template<>
+ struct __make_unsigned<long long>
+ { typedef unsigned long long __type; };
+
+
+ // Select between integral and enum: not possible to be both.
+ template<typename _Tp,
+ bool _IsInt = is_integral<_Tp>::value,
+ bool _IsEnum = is_enum<_Tp>::value>
+ class __make_unsigned_selector;
+
+ template<typename _Tp>
+ class __make_unsigned_selector<_Tp, true, false>
+ {
+ typedef __make_unsigned<typename remove_cv<_Tp>::type> __unsignedt;
+ typedef typename __unsignedt::__type __unsigned_type;
+ typedef __match_cv_qualifiers<_Tp, __unsigned_type> __cv_unsigned;
+
+ public:
+ typedef typename __cv_unsigned::__type __type;
+ };
+
+ template<typename _Tp>
+ class __make_unsigned_selector<_Tp, false, true>
+ {
+ // With -fshort-enums, an enum may be as small as a char.
+ typedef unsigned char __smallest;
+ static const bool __b0 = sizeof(_Tp) <= sizeof(__smallest);
+ static const bool __b1 = sizeof(_Tp) <= sizeof(unsigned short);
+ static const bool __b2 = sizeof(_Tp) <= sizeof(unsigned int);
+ typedef conditional<__b2, unsigned int, unsigned long> __cond2;
+ typedef typename __cond2::type __cond2_type;
+ typedef conditional<__b1, unsigned short, __cond2_type> __cond1;
+ typedef typename __cond1::type __cond1_type;
+
+ public:
+ typedef typename conditional<__b0, __smallest, __cond1_type>::type __type;
+ };
+
+ // Given an integral/enum type, return the corresponding unsigned
+ // integer type.
+ // Primary template.
+ /// make_unsigned
+ template<typename _Tp>
+ struct make_unsigned
+ { typedef typename __make_unsigned_selector<_Tp>::__type type; };
+
+ // Integral, but don't define.
+ template<>
+ struct make_unsigned<bool>;
+
+
+ // Utility for finding the signed versions of unsigned integral types.
+ template<typename _Tp>
+ struct __make_signed
+ { typedef _Tp __type; };
+
+ template<>
+ struct __make_signed<char>
+ { typedef signed char __type; };
+
+ template<>
+ struct __make_signed<unsigned char>
+ { typedef signed char __type; };
+
+ template<>
+ struct __make_signed<unsigned short>
+ { typedef signed short __type; };
+
+ template<>
+ struct __make_signed<unsigned int>
+ { typedef signed int __type; };
+
+ template<>
+ struct __make_signed<unsigned long>
+ { typedef signed long __type; };
+
+ template<>
+ struct __make_signed<unsigned long long>
+ { typedef signed long long __type; };
+
+
+ // Select between integral and enum: not possible to be both.
+ template<typename _Tp,
+ bool _IsInt = is_integral<_Tp>::value,
+ bool _IsEnum = is_enum<_Tp>::value>
+ class __make_signed_selector;
+
+ template<typename _Tp>
+ class __make_signed_selector<_Tp, true, false>
+ {
+ typedef __make_signed<typename remove_cv<_Tp>::type> __signedt;
+ typedef typename __signedt::__type __signed_type;
+ typedef __match_cv_qualifiers<_Tp, __signed_type> __cv_signed;
+
+ public:
+ typedef typename __cv_signed::__type __type;
+ };
+
+ template<typename _Tp>
+ class __make_signed_selector<_Tp, false, true>
+ {
+ // With -fshort-enums, an enum may be as small as a char.
+ typedef signed char __smallest;
+ static const bool __b0 = sizeof(_Tp) <= sizeof(__smallest);
+ static const bool __b1 = sizeof(_Tp) <= sizeof(signed short);
+ static const bool __b2 = sizeof(_Tp) <= sizeof(signed int);
+ typedef conditional<__b2, signed int, signed long> __cond2;
+ typedef typename __cond2::type __cond2_type;
+ typedef conditional<__b1, signed short, __cond2_type> __cond1;
+ typedef typename __cond1::type __cond1_type;
+
+ public:
+ typedef typename conditional<__b0, __smallest, __cond1_type>::type __type;
+ };
+
+ // Given an integral/enum type, return the corresponding signed
+ // integer type.
+ // Primary template.
+ /// make_signed
+ template<typename _Tp>
+ struct make_signed
+ { typedef typename __make_signed_selector<_Tp>::__type type; };
+
+ // Integral, but don't define.
+ template<>
+ struct make_signed<bool>;
+
+ /// common_type
+ template<typename... _Tp>
+ struct common_type;
+
+ template<typename _Tp>
+ struct common_type<_Tp>
+ { typedef _Tp type; };
+
+ template<typename _Tp, typename _Up>
+ struct common_type<_Tp, _Up>
+ { typedef decltype(true ? declval<_Tp>() : declval<_Up>()) type; };
+
+ template<typename _Tp, typename _Up, typename... _Vp>
+ struct common_type<_Tp, _Up, _Vp...>
+ {
+ typedef typename
+ common_type<typename common_type<_Tp, _Up>::type, _Vp...>::type type;
+ };
+ // @} group metaprogramming
+
+ /// declval
+ template<typename _Tp>
+ struct __declval_protector
+ {
+ static const bool __stop = false;
+ static typename add_rvalue_reference<_Tp>::type __delegate();
+ };
+
+ template<typename _Tp>
+ inline typename add_rvalue_reference<_Tp>::type
+ declval()
+ {
+ static_assert(__declval_protector<_Tp>::__stop,
+ "declval() must not be used!");
+ return __declval_protector<_Tp>::__delegate();
+ }
+}
+
+#endif // __GXX_EXPERIMENTAL_CXX0X__
+
+#endif // _GLIBCXX_TYPE_TRAITS