diff --git a/hal/include/HAL/cpp/priority_mutex.h b/hal/include/HAL/cpp/priority_mutex.h index ae8d73cb0f..b4b84c3d79 100644 --- a/hal/include/HAL/cpp/priority_mutex.h +++ b/hal/include/HAL/cpp/priority_mutex.h @@ -1,10 +1,17 @@ #pragma once -#include - // Allows usage with std::unique_lock without including separately #include +#ifdef FRC_SIMULATOR +// We do not want to use pthreads if in the simulator; however, in the +// simulator, we do not care about priority inversion. +typedef ::std::mutex priority_mutex; +typedef ::std::recursive_mutex priority_recursive_mutex; +#else // Covers rest of file. + +#include + class priority_recursive_mutex { public: typedef pthread_mutex_t *native_handle_type; @@ -65,3 +72,5 @@ class priority_mutex { {0, 0, 0, 0x20, 0, {0}}}; #endif }; + +#endif // FRC_SIMULATOR diff --git a/hal/lib/Athena/NetworkCommunication/FRCComm.h b/hal/lib/Athena/NetworkCommunication/FRCComm.h index d7081365a5..26df7e18e1 100644 --- a/hal/lib/Athena/NetworkCommunication/FRCComm.h +++ b/hal/lib/Athena/NetworkCommunication/FRCComm.h @@ -1,14 +1,14 @@ /************************************************************* * NOTICE - * + * * These are the only externally exposed functions to the * NetworkCommunication library - * + * * This is an implementation of FRC Spec for Comm Protocol * Revision 4.5, June 30, 2008 * * Copyright (c) National Instruments 2008. All Rights Reserved. - * + * *************************************************************/ #ifndef __FRC_COMM_H__ @@ -92,7 +92,7 @@ extern "C" { const char *userDataHigh, int userDataHighLength, const char *userDataLow, int userDataLowLength, int wait_ms); int EXPORT_FUNC setErrorData(const char *errors, int errorsLength, int wait_ms); - + #ifdef SIMULATION void EXPORT_FUNC setNewDataSem(HANDLE); #else diff --git a/hal/lib/Athena/ctre/CtreCanNode.h b/hal/lib/Athena/ctre/CtreCanNode.h index 7a2d69054e..70ce567e05 100644 --- a/hal/lib/Athena/ctre/CtreCanNode.h +++ b/hal/lib/Athena/ctre/CtreCanNode.h @@ -2,7 +2,6 @@ #define CtreCanNode_H_ #include "ctre.h" //BIT Defines + Typedefs #include //CAN Comm -#include #include #include // memcpy #include diff --git a/hal/lib/Athena/ctre/PCM.h b/hal/lib/Athena/ctre/PCM.h index 4b6a2dc980..a3c61ef30f 100644 --- a/hal/lib/Athena/ctre/PCM.h +++ b/hal/lib/Athena/ctre/PCM.h @@ -3,13 +3,12 @@ #include "ctre.h" //BIT Defines + Typedefs #include //CAN Comm #include "CtreCanNode.h" -#include class PCM : public CtreCanNode { public: PCM(UINT8 deviceNumber=0); ~PCM(); - + /* Set PCM solenoid state * * @Return - CTR_Code - Error code (if any) for setting solenoid @@ -28,7 +27,7 @@ public: * @Return - CTR_Code - Error code (if any) for setting solenoid */ CTR_Code ClearStickyFaults(); - + /* Get solenoid state * * @Return - CTR_Code - Error code (if any) @@ -103,13 +102,13 @@ public: CTR_Code GetCompressorShortedFault(bool &status); /* Get compressor is not connected sticky fault value * @Return - CTR_Code - Error code (if any) - * @Param - status - True if compressor current is too low, + * @Param - status - True if compressor current is too low, * indicating compressor is not connected, false if otherwise */ CTR_Code GetCompressorNotConnectedStickyFault(bool &status); /* Get compressor is not connected fault value * @Return - CTR_Code - Error code (if any) - * @Param - status - True if compressor current is too low, + * @Param - status - True if compressor current is too low, * indicating compressor is not connected, false if otherwise */ CTR_Code GetCompressorNotConnectedFault(bool &status); @@ -126,7 +125,7 @@ public: * @Param - status - Voltage across PCM power ports in Volts (V) */ CTR_Code GetBatteryVoltage(float &status); - + /* Set PCM Device Number and according CAN frame IDs * @Return - void * @Param - deviceNumber - Device number of PCM to control @@ -140,7 +139,7 @@ public: * See function EnableSeekDebugFrames */ CTR_Code GetNumberOfFailedControlFrames(UINT16 &status); - + /* Get raw Solenoid Blacklist * @Return - CTR_Code - Error code (if any) * @Param - status - Raw binary breakdown of Solenoid Blacklist diff --git a/hal/lib/Athena/ctre/PDP.h b/hal/lib/Athena/ctre/PDP.h index a289d1a1fe..42b62a299e 100644 --- a/hal/lib/Athena/ctre/PDP.h +++ b/hal/lib/Athena/ctre/PDP.h @@ -3,7 +3,6 @@ #include "ctre.h" //BIT Defines + Typedefs #include //CAN Comm #include "CtreCanNode.h" -#include class PDP : public CtreCanNode { public: diff --git a/wpilibc/wpilibC++/include/Controller.h b/wpilibc/wpilibC++/include/Controller.h index 7ececd1151..d852307892 100644 --- a/wpilibc/wpilibC++/include/Controller.h +++ b/wpilibc/wpilibC++/include/Controller.h @@ -5,11 +5,6 @@ /*----------------------------------------------------------------------------*/ #pragma once -#include -#include -#include -#include - /** * Interface for Controllers * Common interface for controllers. Controllers run control loops, the most diff --git a/wpilibc/wpilibC++/include/Notifier.h b/wpilibc/wpilibC++/include/Notifier.h index 0f0e4bda06..bd16a1cf2d 100644 --- a/wpilibc/wpilibC++/include/Notifier.h +++ b/wpilibc/wpilibC++/include/Notifier.h @@ -7,8 +7,9 @@ #pragma once #include "ErrorBase.h" -#include "Task.h" #include "HAL/cpp/priority_mutex.h" +#include +#include typedef void (*TimerEventHandler)(void *param); @@ -45,6 +46,9 @@ class Notifier : public ErrorBase { DISALLOW_COPY_AND_ASSIGN(Notifier); - static Task *task; +#ifdef FRC_SIMULATOR + static ::std::thread m_task; + static ::std::atomic m_stopped; +#endif static void Run(); }; diff --git a/wpilibc/wpilibC++/src/Error.cpp b/wpilibc/wpilibC++/src/Error.cpp index 7f6e700f20..4b109d1820 100644 --- a/wpilibc/wpilibC++/src/Error.cpp +++ b/wpilibc/wpilibC++/src/Error.cpp @@ -12,7 +12,6 @@ #include #include #include -#include "HAL/Task.hpp" #include "DriverStation.h" #include "Timer.h" @@ -67,8 +66,6 @@ void Error::Set(Code code, const char* contextMessage, const char* filename, m_timestamp = GetTime(); Report(); } - - if (m_suspendOnErrorEnabled) suspendTask(nullptr); } void Error::Report() { diff --git a/wpilibc/wpilibC++Devices/include/DriverStation.h b/wpilibc/wpilibC++Devices/include/DriverStation.h index f98177d648..96338cde64 100644 --- a/wpilibc/wpilibC++Devices/include/DriverStation.h +++ b/wpilibc/wpilibC++Devices/include/DriverStation.h @@ -101,7 +101,9 @@ class DriverStation : public SensorBase, public RobotStateInterface { HALJoystickPOVs m_joystickPOVs[kJoystickPorts]; HALJoystickButtons m_joystickButtons[kJoystickPorts]; HALJoystickDescriptor m_joystickDescriptor[kJoystickPorts]; +#ifndef FRC_SIMULATOR Task m_task{"DriverStation", (FUNCPTR)DriverStation::InitTask}; +#endif mutable Semaphore m_newControlData{Semaphore::kEmpty}; mutable priority_condition_variable m_packetDataAvailableCond; priority_mutex m_packetDataAvailableMutex; diff --git a/wpilibc/wpilibC++Sim/CMakeLists.txt b/wpilibc/wpilibC++Sim/CMakeLists.txt index 2c6b8394ce..b2163ac59b 100644 --- a/wpilibc/wpilibC++Sim/CMakeLists.txt +++ b/wpilibc/wpilibC++Sim/CMakeLists.txt @@ -15,10 +15,7 @@ endif() find_package(gazebo REQUIRED) file(GLOB_RECURSE SRC_FILES src/*.cpp ../../networktables/cpp/lib/share/*.cpp - ../../networktables/cpp/lib/Athena/*.cpp - ../../hal/lib/Athena/Task.cpp - ../../hal/lib/Athena/Semaphore.cpp - ../../hal/lib/Athena/cpp/Synchronized.cpp) + ../../networktables/cpp/lib/Athena/*.cpp) file(GLOB_RECURSE COM_SRC_FILES ../wpilibC++/src/*.cpp) diff --git a/wpilibc/wpilibC++Sim/include/DriverStation.h b/wpilibc/wpilibC++Sim/include/DriverStation.h index 28c5713b6e..3edc389f53 100644 --- a/wpilibc/wpilibC++Sim/include/DriverStation.h +++ b/wpilibc/wpilibC++Sim/include/DriverStation.h @@ -9,9 +9,8 @@ #include #include "SensorBase.h" #include "RobotState.h" -#include "Task.h" -#include "HAL/cpp/priority_mutex.h" -#include "HAL/cpp/priority_condition_variable.h" +#include +#include struct HALCommonControlData; class AnalogInput; @@ -62,7 +61,7 @@ public: double GetMatchTime() const; float GetBatteryVoltage() const; uint16_t GetTeamNumber() const; - + void IncrementUpdateNumber() @@ -119,10 +118,10 @@ private: void joystickCallback5(const msgs::ConstJoystickPtr &msg); uint8_t m_digitalOut = 0; - priority_condition_variable m_waitForDataCond; - priority_mutex m_waitForDataMutex; - mutable priority_recursive_mutex m_stateMutex; - priority_recursive_mutex m_joystickMutex; + ::std::condition_variable m_waitForDataCond; + ::std::mutex m_waitForDataMutex; + mutable ::std::recursive_mutex m_stateMutex; + ::std::recursive_mutex m_joystickMutex; double m_approxMatchTimeOffset = 0; bool m_userInDisabled = false; bool m_userInAutonomous = false; diff --git a/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h b/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h index 6933aac8c3..81377ab87b 100644 --- a/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h +++ b/wpilibc/wpilibC++Sim/include/MotorSafetyHelper.h @@ -6,7 +6,6 @@ #pragma once #include "ErrorBase.h" -#include "HAL/cpp/priority_mutex.h" class MotorSafety; diff --git a/wpilibc/wpilibC++Sim/include/simulation/simTime.h b/wpilibc/wpilibC++Sim/include/simulation/simTime.h index 5f53ab081b..b0dee13d54 100644 --- a/wpilibc/wpilibC++Sim/include/simulation/simTime.h +++ b/wpilibc/wpilibC++Sim/include/simulation/simTime.h @@ -1,10 +1,11 @@ #pragma once -#include "HAL/Semaphore.hpp" +#include +#include namespace wpilib { namespace internal { extern double simTime; - extern MULTIWAIT_ID time_wait; - extern MUTEX_ID time_wait_mutex; + extern ::std::condition_variable time_wait; + extern ::std::mutex time_wait_mutex; // transport::SubscriberPtr time_sub; }} diff --git a/wpilibc/wpilibC++Sim/src/DriverStation.cpp b/wpilibc/wpilibC++Sim/src/DriverStation.cpp index f1ea0bbef2..d17f7fbce3 100644 --- a/wpilibc/wpilibC++Sim/src/DriverStation.cpp +++ b/wpilibc/wpilibC++Sim/src/DriverStation.cpp @@ -29,7 +29,7 @@ uint8_t DriverStation::m_updateNumber = 0; /** * DriverStation contructor. - * + * * This is only called once the first time GetInstance() is called */ DriverStation::DriverStation() { @@ -81,7 +81,7 @@ float DriverStation::GetBatteryVoltage() const /** * Get the value of the axis on a joystick. * This depends on the mapping of the joystick connected to the specified port. - * + * * @param stick The joystick to read. * @param axis The analog axis value to read from the joystick. * @return The value of the axis on the joystick. @@ -99,7 +99,7 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) return 0.0; } - std::unique_lock lock(m_joystickMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_joystickMutex); if (joysticks[stick] == nullptr || axis >= joysticks[stick]->axes().size()) { return 0.0; @@ -110,7 +110,7 @@ float DriverStation::GetStickAxis(uint32_t stick, uint32_t axis) /** * The state of a specific button (1 - 12) on the joystick. * This method only works in simulation, but is more efficient than GetStickButtons. - * + * * @param stick The joystick to read. * @param button The button number to check. * @return If the button is pressed. @@ -123,7 +123,7 @@ bool DriverStation::GetStickButton(uint32_t stick, uint32_t button) return false; } - std::unique_lock lock(m_joystickMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_joystickMutex); if (joysticks[stick] == nullptr || button >= joysticks[stick]->buttons().size()) { return false; @@ -147,7 +147,7 @@ short DriverStation::GetStickButtons(uint32_t stick) } short btns = 0, btnid; - std::unique_lock lock(m_joystickMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_joystickMutex); msgs::JoystickPtr joy = joysticks[stick]; for (btnid = 0; btnid < joy->buttons().size() && btnid < 12; btnid++) { @@ -191,10 +191,10 @@ bool DriverStation::GetDigitalIn(uint32_t channel) /** * Set a value for the digital outputs on the Driver Station. - * + * * Control digital outputs on the Drivers Station. These values are typically used for * giving feedback on a custom operator station such as LEDs. - * + * * @param channel The digital output to set. Valid range is 1 - 8. * @param value The state to set the digital output. */ @@ -215,7 +215,7 @@ bool DriverStation::GetDigitalOut(uint32_t channel) bool DriverStation::IsEnabled() const { - std::unique_lock lock(m_stateMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_stateMutex); return state != nullptr ? state->enabled() : false; } @@ -226,7 +226,7 @@ bool DriverStation::IsDisabled() const bool DriverStation::IsAutonomous() const { - std::unique_lock lock(m_stateMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_stateMutex); return state != nullptr ? state->state() == msgs::DriverStation_State_AUTO : false; } @@ -238,7 +238,7 @@ bool DriverStation::IsOperatorControl() const bool DriverStation::IsTest() const { - std::unique_lock lock(m_stateMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_stateMutex); return state != nullptr ? state->state() == msgs::DriverStation_State_TEST : false; } @@ -283,7 +283,7 @@ uint32_t DriverStation::GetLocation() const */ void DriverStation::WaitForData() { - std::unique_lock lock(m_waitForDataMutex); + ::std::unique_lock<::std::mutex> lock(m_waitForDataMutex); m_waitForDataCond.wait(lock); } @@ -308,9 +308,9 @@ double DriverStation::GetMatchTime() const * Report an error to the DriverStation messages window. * The error is also printed to the program console. */ -void DriverStation::ReportError(std::string error) +void DriverStation::ReportError(::std::string error) { - std::cout << error << std::endl; + ::std::cout << error << ::std::endl; } /** @@ -325,7 +325,7 @@ uint16_t DriverStation::GetTeamNumber() const void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg) { { - std::unique_lock lock(m_stateMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_stateMutex); *state = *msg; } m_waitForDataCond.notify_all(); @@ -334,7 +334,7 @@ void DriverStation::stateCallback(const msgs::ConstDriverStationPtr &msg) void DriverStation::joystickCallback(const msgs::ConstJoystickPtr &msg, int i) { - std::unique_lock lock(m_joystickMutex); + ::std::unique_lock<::std::recursive_mutex> lock(m_joystickMutex); *(joysticks[i]) = *msg; } diff --git a/wpilibc/wpilibC++Sim/src/Notifier.cpp b/wpilibc/wpilibC++Sim/src/Notifier.cpp index ab9cae0240..94df0b10ab 100644 --- a/wpilibc/wpilibC++Sim/src/Notifier.cpp +++ b/wpilibc/wpilibC++Sim/src/Notifier.cpp @@ -11,8 +11,8 @@ Notifier *Notifier::timerQueueHead = nullptr; priority_recursive_mutex Notifier::queueMutex; -Task* Notifier::task = nullptr; int Notifier::refcount = 0; +::std::atomic Notifier::m_stopped(false); /** * Create a Notifier for timer event notification. @@ -35,8 +35,7 @@ Notifier::Notifier(TimerEventHandler handler, void *param) // do the first time intialization of static variables if (refcount == 0) { - task = new Task("NotifierTask", (FUNCPTR)Notifier::Run, Task::kDefaultPriority, 64000); - task->Start(); + m_task = ::std::thread(Run); } refcount++; } @@ -56,12 +55,12 @@ Notifier::~Notifier() // Delete the static variables when the last one is going away if (!(--refcount)) { - task->Stop(); - delete task; + m_stopped = true; + m_task.join(); } } - // Acquire the semaphore; this makes certain that the handler is + // Acquire the semaphore; this makes certain that the handler is // not being executed by the interrupt manager. std::unique_lock lock(m_handlerMutex); } @@ -80,7 +79,7 @@ void Notifier::UpdateAlarm() /** * ProcessQueue is called whenever there is a timer interrupt. * We need to wake up and process the current top item in the timer queue as long - * as its scheduled time is after the current time. Then the item is removed or + * as its scheduled time is after the current time. Then the item is removed or * rescheduled (repetitive events) in the queue. */ void Notifier::ProcessQueue(uint32_t mask, void *params) @@ -249,7 +248,7 @@ void Notifier::Stop() } void Notifier::Run() { - while (true) { + while (!m_stopped) { Notifier::ProcessQueue(0, nullptr); if (timerQueueHead != nullptr) { diff --git a/wpilibc/wpilibC++Sim/src/Timer.cpp b/wpilibc/wpilibC++Sim/src/Timer.cpp index 2bdf4ba349..536cc2f6bb 100644 --- a/wpilibc/wpilibC++Sim/src/Timer.cpp +++ b/wpilibc/wpilibC++Sim/src/Timer.cpp @@ -13,11 +13,11 @@ /** * Pause the task for a specified time. - * + * * Pause the execution of the program for a specified period of time given in seconds. * Motors will continue to run at their last assigned values, and sensors will continue to * update. Only the task containing the wait will pause until the wait time is expired. - * + * * @param seconds Length of time to pause, in seconds. */ void Wait(double seconds) @@ -25,10 +25,10 @@ void Wait(double seconds) if (seconds < 0.0) return; double start = wpilib::internal::simTime; - + while ((wpilib::internal::simTime - start) < seconds) { - takeMultiWait(wpilib::internal::time_wait, - wpilib::internal::time_wait_mutex, 0); + ::std::unique_lock<::std::mutex> lock(wpilib::internal::time_wait_mutex); + wpilib::internal::time_wait.wait(lock); } } @@ -53,7 +53,7 @@ double GetTime() /** * Create a new timer object. - * + * * Create a new timer object and reset the time to zero. The timer is initially not running and * must be started. */ @@ -71,7 +71,7 @@ Timer::Timer() * Get the current time from the timer. If the clock is running it is derived from * the current system clock the start time stored in the timer class. If the clock * is not running, then return the time when it was last stopped. - * + * * @return unsigned Current time value for this timer in seconds */ double Timer::Get() const @@ -96,7 +96,7 @@ double Timer::Get() const /** * Reset the timer by setting the time to 0. - * + * * Make the timer startTime the current time so new requests will be relative to now */ void Timer::Reset() @@ -162,7 +162,7 @@ bool Timer::HasPeriodPassed(double period) /* * Return the FPGA system clock time in seconds. - * + * * Return the time from the FPGA hardware clock in seconds since the FPGA * started. * Rolls over after 71 minutes. @@ -198,7 +198,7 @@ namespace wpilib { namespace internal { void time_callback(const msgs::ConstFloat64Ptr &msg) { simTime = msg->data(); - giveMultiWait(time_wait); + time_wait.notify_all(); } transport::SubscriberPtr time_pub = MainNode::Subscribe("~/time", &time_callback); diff --git a/wpilibc/wpilibC++Sim/src/Utility.cpp b/wpilibc/wpilibC++Sim/src/Utility.cpp index c5e78c2d8e..1046cd60bf 100644 --- a/wpilibc/wpilibC++Sim/src/Utility.cpp +++ b/wpilibc/wpilibC++Sim/src/Utility.cpp @@ -6,7 +6,6 @@ #include "Utility.h" -#include "Task.h" #include "Timer.h" #include "simulation/simTime.h" #include @@ -57,33 +56,32 @@ static void wpi_handleTracing() * This allows breakpoints to be set on an assert. * The users don't call this, but instead use the wpi_assert macros in Utility.h. */ -bool wpi_assert_impl(bool conditionValue, +bool wpi_assert_impl(bool conditionValue, const char *conditionText, const char *message, const char *fileName, - uint32_t lineNumber, + uint32_t lineNumber, const char *funcName) { if (!conditionValue) - { + { // Error string buffer char error[256]; - + // If an error message was specified, include it // Build error string if(message != nullptr) { - sprintf(error, "Assertion failed: \"%s\", \"%s\" failed in %s() in %s at line %dd\n", + sprintf(error, "Assertion failed: \"%s\", \"%s\" failed in %s() in %s at line %dd\n", message, conditionText, funcName, fileName, lineNumber); } else { - sprintf(error, "Assertion failed: \"%s\" in %s() in %s at line %dd\n", + sprintf(error, "Assertion failed: \"%s\" in %s() in %s at line %dd\n", conditionText, funcName, fileName, lineNumber); } - + // Print to console and send to remote dashboard printf("\n\n>>>>%s", error); - + wpi_handleTracing(); - if (suspendOnAssertEnabled) suspendTask(0); } return conditionValue; } @@ -98,27 +96,26 @@ void wpi_assertEqual_common_impl(int valueA, const char *equalityType, const char *message, const char *fileName, - uint32_t lineNumber, + uint32_t lineNumber, const char *funcName) { // Error string buffer char error[256]; - + // If an error message was specified, include it // Build error string if(message != nullptr) { - sprintf(error, "Assertion failed: \"%s\", \"%d\" %s \"%d\" in %s() in %s at line %d\n", + sprintf(error, "Assertion failed: \"%s\", \"%d\" %s \"%d\" in %s() in %s at line %d\n", message, valueA, equalityType, valueB, funcName, fileName, lineNumber); } else { - sprintf(error, "Assertion failed: \"%d\" %s \"%d\" in %s() in %s at line %d\n", + sprintf(error, "Assertion failed: \"%d\" %s \"%d\" in %s() in %s at line %d\n", valueA, equalityType, valueB, funcName, fileName, lineNumber); } - + // Print to console and send to remote dashboard printf("\n\n>>>>%s", error); - + wpi_handleTracing(); - if (suspendOnAssertEnabled) suspendTask(0); } /** @@ -131,7 +128,7 @@ bool wpi_assertEqual_impl(int valueA, int valueB, const char *message, const char *fileName, - uint32_t lineNumber, + uint32_t lineNumber, const char *funcName) { if(!(valueA == valueB)) @@ -151,7 +148,7 @@ bool wpi_assertNotEqual_impl(int valueA, int valueB, const char *message, const char *fileName, - uint32_t lineNumber, + uint32_t lineNumber, const char *funcName) { if(!(valueA != valueB)) @@ -163,7 +160,7 @@ bool wpi_assertNotEqual_impl(int valueA, /** * Read the microsecond-resolution timer on the FPGA. - * + * * @return The current time in microseconds according to the FPGA (since FPGA reset). */ uint32_t GetFPGATime()