mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-19 00:41:43 +00:00
Enable log macros to work with no args (#4475)
This is enabled by the C++20 __VA_OPT__ feature.
Uses of "{}" format string were updated.
Some warning suppressions were required for older clang versions.
Also improve codegen of wpi::Logger::Log(), frc::ReportError(), and frc::MakeError();
these generate better and less redundant code if they use fmt::string_view for the
format string instead of templating on it.
This commit is contained in:
@@ -110,7 +110,7 @@ void CvSinkImpl::ThreadMain() {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
continue;
|
||||
}
|
||||
SDEBUG4("{}", "waiting for frame");
|
||||
SDEBUG4("waiting for frame");
|
||||
Frame frame = source->GetNextFrame(); // blocks
|
||||
if (!m_active) {
|
||||
break;
|
||||
|
||||
@@ -85,7 +85,7 @@ void HttpCameraImpl::MonitorThreadMain() {
|
||||
// (this will result in an error at the read point, and ultimately
|
||||
// a reconnect attempt)
|
||||
if (m_streamConn && m_frameCount == 0) {
|
||||
SWARNING("{}", "Monitor detected stream hung, disconnecting");
|
||||
SWARNING("Monitor detected stream hung, disconnecting");
|
||||
m_streamConn->stream->close();
|
||||
}
|
||||
|
||||
@@ -93,7 +93,7 @@ void HttpCameraImpl::MonitorThreadMain() {
|
||||
m_frameCount = 0;
|
||||
}
|
||||
|
||||
SDEBUG("{}", "Monitor Thread exiting");
|
||||
SDEBUG("Monitor Thread exiting");
|
||||
}
|
||||
|
||||
void HttpCameraImpl::StreamThreadMain() {
|
||||
@@ -141,7 +141,7 @@ void HttpCameraImpl::StreamThreadMain() {
|
||||
}
|
||||
}
|
||||
|
||||
SDEBUG("{}", "Camera Thread exiting");
|
||||
SDEBUG("Camera Thread exiting");
|
||||
SetConnected(false);
|
||||
}
|
||||
|
||||
@@ -152,7 +152,7 @@ wpi::HttpConnection* HttpCameraImpl::DeviceStreamConnect(
|
||||
{
|
||||
std::scoped_lock lock(m_mutex);
|
||||
if (m_locations.empty()) {
|
||||
SERROR("{}", "locations array is empty!?");
|
||||
SERROR("locations array is empty!?");
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
return nullptr;
|
||||
}
|
||||
@@ -273,7 +273,7 @@ bool HttpCameraImpl::DeviceStreamFrame(wpi::raw_istream& is,
|
||||
wpi::SmallString<64> contentTypeBuf;
|
||||
wpi::SmallString<64> contentLengthBuf;
|
||||
if (!ParseHttpHeaders(is, &contentTypeBuf, &contentLengthBuf)) {
|
||||
SWARNING("{}", "disconnected during headers");
|
||||
SWARNING("disconnected during headers");
|
||||
PutError("disconnected during headers", wpi::Now());
|
||||
return false;
|
||||
}
|
||||
@@ -295,7 +295,7 @@ bool HttpCameraImpl::DeviceStreamFrame(wpi::raw_istream& is,
|
||||
// Ugh, no Content-Length? Read the blocks of the JPEG file.
|
||||
int width, height;
|
||||
if (!ReadJpeg(is, imageBuf, &width, &height)) {
|
||||
SWARNING("{}", "did not receive a JPEG image");
|
||||
SWARNING("did not receive a JPEG image");
|
||||
PutError("did not receive a JPEG image", wpi::Now());
|
||||
return false;
|
||||
}
|
||||
@@ -314,7 +314,7 @@ bool HttpCameraImpl::DeviceStreamFrame(wpi::raw_istream& is,
|
||||
}
|
||||
int width, height;
|
||||
if (!GetJpegSize(image->str(), &width, &height)) {
|
||||
SWARNING("{}", "did not receive a JPEG image");
|
||||
SWARNING("did not receive a JPEG image");
|
||||
PutError("did not receive a JPEG image", wpi::Now());
|
||||
return false;
|
||||
}
|
||||
@@ -344,7 +344,7 @@ void HttpCameraImpl::SettingsThreadMain() {
|
||||
DeviceSendSettings(req);
|
||||
}
|
||||
|
||||
SDEBUG("{}", "Settings Thread exiting");
|
||||
SDEBUG("Settings Thread exiting");
|
||||
}
|
||||
|
||||
void HttpCameraImpl::DeviceSendSettings(wpi::HttpRequest& req) {
|
||||
|
||||
@@ -27,26 +27,37 @@ inline void NamedLog(wpi::Logger& logger, unsigned int level, const char* file,
|
||||
|
||||
} // namespace cs
|
||||
|
||||
#define LOG(level, format, ...) WPI_LOG(m_logger, level, format, __VA_ARGS__)
|
||||
#define LOG(level, format, ...) \
|
||||
WPI_LOG(m_logger, level, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#undef ERROR
|
||||
#define ERROR(format, ...) WPI_ERROR(m_logger, format, __VA_ARGS__)
|
||||
#define WARNING(format, ...) WPI_WARNING(m_logger, format, __VA_ARGS__)
|
||||
#define INFO(format, ...) WPI_INFO(m_logger, format, __VA_ARGS__)
|
||||
#define ERROR(format, ...) \
|
||||
WPI_ERROR(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WARNING(format, ...) \
|
||||
WPI_WARNING(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define INFO(format, ...) WPI_INFO(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#define DEBUG0(format, ...) WPI_DEBUG(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG1(format, ...) WPI_DEBUG1(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG2(format, ...) WPI_DEBUG2(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG3(format, ...) WPI_DEBUG3(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG4(format, ...) WPI_DEBUG4(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG0(format, ...) \
|
||||
WPI_DEBUG(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG1(format, ...) \
|
||||
WPI_DEBUG1(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG2(format, ...) \
|
||||
WPI_DEBUG2(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG3(format, ...) \
|
||||
WPI_DEBUG3(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG4(format, ...) \
|
||||
WPI_DEBUG4(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#define SLOG(level, format, ...) \
|
||||
NamedLog(m_logger, level, __FILE__, __LINE__, GetName(), FMT_STRING(format), \
|
||||
__VA_ARGS__)
|
||||
#define SLOG(level, format, ...) \
|
||||
NamedLog(m_logger, level, __FILE__, __LINE__, GetName(), \
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#define SERROR(format, ...) SLOG(::wpi::WPI_LOG_ERROR, format, __VA_ARGS__)
|
||||
#define SWARNING(format, ...) SLOG(::wpi::WPI_LOG_WARNING, format, __VA_ARGS__)
|
||||
#define SINFO(format, ...) SLOG(::wpi::WPI_LOG_INFO, format, __VA_ARGS__)
|
||||
#define SERROR(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_ERROR, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define SWARNING(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_WARNING, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define SINFO(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_INFO, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#ifdef NDEBUG
|
||||
#define SDEBUG(format, ...) \
|
||||
@@ -65,11 +76,16 @@ inline void NamedLog(wpi::Logger& logger, unsigned int level, const char* file,
|
||||
do { \
|
||||
} while (0)
|
||||
#else
|
||||
#define SDEBUG(format, ...) SLOG(::wpi::WPI_LOG_DEBUG, format, __VA_ARGS__)
|
||||
#define SDEBUG1(format, ...) SLOG(::wpi::WPI_LOG_DEBUG1, format, __VA_ARGS__)
|
||||
#define SDEBUG2(format, ...) SLOG(::wpi::WPI_LOG_DEBUG2, format, __VA_ARGS__)
|
||||
#define SDEBUG3(format, ...) SLOG(::wpi::WPI_LOG_DEBUG3, format, __VA_ARGS__)
|
||||
#define SDEBUG4(format, ...) SLOG(::wpi::WPI_LOG_DEBUG4, format, __VA_ARGS__)
|
||||
#define SDEBUG(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_DEBUG, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define SDEBUG1(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_DEBUG1, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define SDEBUG2(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_DEBUG2, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define SDEBUG3(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_DEBUG3, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define SDEBUG4(format, ...) \
|
||||
SLOG(::wpi::WPI_LOG_DEBUG4, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#endif
|
||||
|
||||
#endif // CSCORE_LOG_H_
|
||||
|
||||
@@ -650,7 +650,7 @@ void MjpegServerImpl::Stop() {
|
||||
// Send HTTP response and a stream of JPG-frames
|
||||
void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
|
||||
if (m_noStreaming) {
|
||||
SERROR("{}", "Too many simultaneous client streams");
|
||||
SERROR("Too many simultaneous client streams");
|
||||
SendError(os, 503, "Too many simultaneous streams");
|
||||
return;
|
||||
}
|
||||
@@ -663,7 +663,7 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
|
||||
SendHeader(oss, 200, "OK", "multipart/x-mixed-replace;boundary=" BOUNDARY);
|
||||
os << oss.str();
|
||||
|
||||
SDEBUG("{}", "Headers send, sending stream now");
|
||||
SDEBUG("Headers send, sending stream now");
|
||||
|
||||
Frame::Time lastFrameTime = 0;
|
||||
Frame::Time timePerFrame = 0;
|
||||
@@ -685,7 +685,7 @@ void MjpegServerImpl::ConnThread::SendStream(wpi::raw_socket_ostream& os) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(200));
|
||||
continue;
|
||||
}
|
||||
SDEBUG4("{}", "waiting for frame");
|
||||
SDEBUG4("waiting for frame");
|
||||
Frame frame = source->GetNextFrame(0.225); // blocks
|
||||
if (!m_active) {
|
||||
break;
|
||||
@@ -783,7 +783,7 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
|
||||
wpi::SmallString<128> reqBuf;
|
||||
std::string_view req = is.getline(reqBuf, 4096);
|
||||
if (is.has_error()) {
|
||||
SDEBUG("{}", "error getting request string");
|
||||
SDEBUG("error getting request string");
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -824,7 +824,7 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
|
||||
} else if (req.find("GET / ") != std::string_view::npos || req == "GET /\n") {
|
||||
kind = kRootPage;
|
||||
} else {
|
||||
SDEBUG("{}", "HTTP request resource not found");
|
||||
SDEBUG("HTTP request resource not found");
|
||||
SendError(os, 404, "Resource not found");
|
||||
return;
|
||||
}
|
||||
@@ -866,11 +866,11 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
|
||||
SendHeader(os, 200, "OK", "text/plain");
|
||||
os << "Ignored due to no connected source."
|
||||
<< "\r\n";
|
||||
SDEBUG("{}", "Ignored due to no connected source.");
|
||||
SDEBUG("Ignored due to no connected source.");
|
||||
}
|
||||
break;
|
||||
case kGetSettings:
|
||||
SDEBUG("{}", "request for JSON file");
|
||||
SDEBUG("request for JSON file");
|
||||
if (auto source = GetSource()) {
|
||||
SendJSON(os, *source, true);
|
||||
} else {
|
||||
@@ -878,7 +878,7 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
|
||||
}
|
||||
break;
|
||||
case kGetSourceConfig:
|
||||
SDEBUG("{}", "request for JSON file");
|
||||
SDEBUG("request for JSON file");
|
||||
if (auto source = GetSource()) {
|
||||
SendHeader(os, 200, "OK", "application/json");
|
||||
CS_Status status = CS_OK;
|
||||
@@ -889,7 +889,7 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
|
||||
}
|
||||
break;
|
||||
case kRootPage:
|
||||
SDEBUG("{}", "request for root page");
|
||||
SDEBUG("request for root page");
|
||||
SendHeader(os, 200, "OK", "text/html");
|
||||
if (auto source = GetSource()) {
|
||||
SendHTML(os, *source, false);
|
||||
@@ -900,7 +900,7 @@ void MjpegServerImpl::ConnThread::ProcessRequest() {
|
||||
break;
|
||||
}
|
||||
|
||||
SDEBUG("{}", "leaving HTTP client thread");
|
||||
SDEBUG("leaving HTTP client thread");
|
||||
}
|
||||
|
||||
// worker thread for clients that connected to this server
|
||||
@@ -927,7 +927,7 @@ void MjpegServerImpl::ServerThreadMain() {
|
||||
return;
|
||||
}
|
||||
|
||||
SDEBUG("{}", "waiting for clients to connect");
|
||||
SDEBUG("waiting for clients to connect");
|
||||
while (m_active) {
|
||||
auto stream = m_acceptor->accept();
|
||||
if (!stream) {
|
||||
@@ -977,7 +977,7 @@ void MjpegServerImpl::ServerThreadMain() {
|
||||
thr->m_cond.notify_one();
|
||||
}
|
||||
|
||||
SDEBUG("{}", "leaving server thread");
|
||||
SDEBUG("leaving server thread");
|
||||
}
|
||||
|
||||
void MjpegServerImpl::SetSourceImpl(std::shared_ptr<SourceImpl> source) {
|
||||
|
||||
@@ -127,7 +127,7 @@ void RawSinkImpl::ThreadMain() {
|
||||
std::this_thread::sleep_for(std::chrono::seconds(1));
|
||||
continue;
|
||||
}
|
||||
SDEBUG4("{}", "waiting for frame");
|
||||
SDEBUG4("waiting for frame");
|
||||
Frame frame = source->GetNextFrame(); // blocks
|
||||
if (!m_active) {
|
||||
break;
|
||||
|
||||
@@ -454,7 +454,7 @@ void UsbCameraImpl::CameraThreadMain() {
|
||||
|
||||
// Handle notify events
|
||||
if (notify_fd >= 0 && FD_ISSET(notify_fd, &readfds)) {
|
||||
SDEBUG4("{}", "notify event");
|
||||
SDEBUG4("notify event");
|
||||
struct inotify_event event;
|
||||
do {
|
||||
// Read the event structure
|
||||
@@ -483,7 +483,7 @@ void UsbCameraImpl::CameraThreadMain() {
|
||||
|
||||
// Handle commands
|
||||
if (command_fd >= 0 && FD_ISSET(command_fd, &readfds)) {
|
||||
SDEBUG4("{}", "got command");
|
||||
SDEBUG4("got command");
|
||||
// Read it to clear
|
||||
eventfd_t val;
|
||||
eventfd_read(command_fd, &val);
|
||||
@@ -493,7 +493,7 @@ void UsbCameraImpl::CameraThreadMain() {
|
||||
|
||||
// Handle frames
|
||||
if (m_streaming && fd >= 0 && FD_ISSET(fd, &readfds)) {
|
||||
SDEBUG4("{}", "grabbing image");
|
||||
SDEBUG4("grabbing image");
|
||||
|
||||
// Dequeue buffer
|
||||
struct v4l2_buffer buf;
|
||||
@@ -501,7 +501,7 @@ void UsbCameraImpl::CameraThreadMain() {
|
||||
buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||
buf.memory = V4L2_MEMORY_MMAP;
|
||||
if (DoIoctl(fd, VIDIOC_DQBUF, &buf) != 0) {
|
||||
SWARNING("{}", "could not dequeue buffer");
|
||||
SWARNING("could not dequeue buffer");
|
||||
wasStreaming = m_streaming;
|
||||
DeviceStreamOff();
|
||||
DeviceDisconnect();
|
||||
@@ -525,7 +525,7 @@ void UsbCameraImpl::CameraThreadMain() {
|
||||
bool good = true;
|
||||
if (m_mode.pixelFormat == VideoMode::kMJPEG &&
|
||||
!GetJpegSize(image, &width, &height)) {
|
||||
SWARNING("{}", "invalid JPEG image received from camera");
|
||||
SWARNING("invalid JPEG image received from camera");
|
||||
good = false;
|
||||
}
|
||||
if (good) {
|
||||
@@ -536,7 +536,7 @@ void UsbCameraImpl::CameraThreadMain() {
|
||||
|
||||
// Requeue buffer
|
||||
if (DoIoctl(fd, VIDIOC_QBUF, &buf) != 0) {
|
||||
SWARNING("{}", "could not requeue buffer");
|
||||
SWARNING("could not requeue buffer");
|
||||
wasStreaming = m_streaming;
|
||||
DeviceStreamOff();
|
||||
DeviceDisconnect();
|
||||
@@ -579,7 +579,7 @@ void UsbCameraImpl::DeviceConnect() {
|
||||
}
|
||||
|
||||
// Try to open the device
|
||||
SDEBUG3("{}", "opening device");
|
||||
SDEBUG3("opening device");
|
||||
int fd = open(m_path.c_str(), O_RDWR);
|
||||
if (fd < 0) {
|
||||
return;
|
||||
@@ -587,7 +587,7 @@ void UsbCameraImpl::DeviceConnect() {
|
||||
m_fd = fd;
|
||||
|
||||
// Get capabilities
|
||||
SDEBUG3("{}", "getting capabilities");
|
||||
SDEBUG3("getting capabilities");
|
||||
struct v4l2_capability vcap;
|
||||
std::memset(&vcap, 0, sizeof(vcap));
|
||||
if (DoIoctl(fd, VIDIOC_QUERYCAP, &vcap) >= 0) {
|
||||
@@ -599,18 +599,18 @@ void UsbCameraImpl::DeviceConnect() {
|
||||
|
||||
// Get or restore video mode
|
||||
if (!m_properties_cached) {
|
||||
SDEBUG3("{}", "caching properties");
|
||||
SDEBUG3("caching properties");
|
||||
DeviceCacheProperties();
|
||||
DeviceCacheVideoModes();
|
||||
DeviceCacheMode();
|
||||
m_properties_cached = true;
|
||||
} else {
|
||||
SDEBUG3("{}", "restoring video mode");
|
||||
SDEBUG3("restoring video mode");
|
||||
DeviceSetMode();
|
||||
DeviceSetFPS();
|
||||
|
||||
// Restore settings
|
||||
SDEBUG3("{}", "restoring settings");
|
||||
SDEBUG3("restoring settings");
|
||||
std::unique_lock lock2(m_mutex);
|
||||
for (size_t i = 0; i < m_propertyData.size(); ++i) {
|
||||
const auto prop =
|
||||
@@ -625,21 +625,21 @@ void UsbCameraImpl::DeviceConnect() {
|
||||
}
|
||||
|
||||
// Request buffers
|
||||
SDEBUG3("{}", "allocating buffers");
|
||||
SDEBUG3("allocating buffers");
|
||||
struct v4l2_requestbuffers rb;
|
||||
std::memset(&rb, 0, sizeof(rb));
|
||||
rb.count = kNumBuffers;
|
||||
rb.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||
rb.memory = V4L2_MEMORY_MMAP;
|
||||
if (DoIoctl(fd, VIDIOC_REQBUFS, &rb) != 0) {
|
||||
SWARNING("{}", "could not allocate buffers");
|
||||
SWARNING("could not allocate buffers");
|
||||
close(fd);
|
||||
m_fd = -1;
|
||||
return;
|
||||
}
|
||||
|
||||
// Map buffers
|
||||
SDEBUG3("{}", "mapping buffers");
|
||||
SDEBUG3("mapping buffers");
|
||||
for (int i = 0; i < kNumBuffers; ++i) {
|
||||
struct v4l2_buffer buf;
|
||||
std::memset(&buf, 0, sizeof(buf));
|
||||
@@ -689,7 +689,7 @@ bool UsbCameraImpl::DeviceStreamOn() {
|
||||
}
|
||||
|
||||
// Queue buffers
|
||||
SDEBUG3("{}", "queuing buffers");
|
||||
SDEBUG3("queuing buffers");
|
||||
for (int i = 0; i < kNumBuffers; ++i) {
|
||||
struct v4l2_buffer buf;
|
||||
std::memset(&buf, 0, sizeof(buf));
|
||||
@@ -708,7 +708,6 @@ bool UsbCameraImpl::DeviceStreamOn() {
|
||||
if (errno == ENOSPC) {
|
||||
// indicates too much USB bandwidth requested
|
||||
SERROR(
|
||||
"{}",
|
||||
"could not start streaming due to USB bandwidth limitations; try a "
|
||||
"lower resolution or a different pixel format (VIDIOC_STREAMON: "
|
||||
"No space left on device)");
|
||||
@@ -718,7 +717,7 @@ bool UsbCameraImpl::DeviceStreamOn() {
|
||||
}
|
||||
return false;
|
||||
}
|
||||
SDEBUG4("{}", "enabled streaming");
|
||||
SDEBUG4("enabled streaming");
|
||||
m_streaming = true;
|
||||
return true;
|
||||
}
|
||||
@@ -735,7 +734,7 @@ bool UsbCameraImpl::DeviceStreamOff() {
|
||||
if (DoIoctl(fd, VIDIOC_STREAMOFF, &type) != 0) {
|
||||
return false;
|
||||
}
|
||||
SDEBUG4("{}", "disabled streaming");
|
||||
SDEBUG4("disabled streaming");
|
||||
m_streaming = false;
|
||||
return true;
|
||||
}
|
||||
@@ -1000,7 +999,7 @@ void UsbCameraImpl::DeviceCacheMode() {
|
||||
#endif
|
||||
vfmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
|
||||
if (DoIoctl(fd, VIDIOC_G_FMT, &vfmt) != 0) {
|
||||
SERROR("{}", "could not read current video mode");
|
||||
SERROR("could not read current video mode");
|
||||
std::scoped_lock lock(m_mutex);
|
||||
m_mode = VideoMode{VideoMode::kMJPEG, 320, 240, 30};
|
||||
return;
|
||||
@@ -1668,7 +1667,7 @@ std::vector<UsbCameraInfo> EnumerateUsbCameras(CS_Status* status) {
|
||||
::closedir(dp);
|
||||
} else {
|
||||
// *status = ;
|
||||
WPI_ERROR(Instance::GetInstance().logger, "{}", "Could not open /dev");
|
||||
WPI_ERROR(Instance::GetInstance().logger, "Could not open /dev");
|
||||
return retval;
|
||||
}
|
||||
|
||||
|
||||
@@ -486,7 +486,7 @@ bool UsbCameraImpl::DeviceConnect() {
|
||||
SINFO("Connecting to USB camera on {}", m_path);
|
||||
}
|
||||
|
||||
SDEBUG3("{}", "opening device");
|
||||
SDEBUG3("opening device");
|
||||
|
||||
const wchar_t* path = m_widePath.c_str();
|
||||
m_mediaSource = CreateVideoCaptureDevice(path);
|
||||
@@ -520,13 +520,13 @@ bool UsbCameraImpl::DeviceConnect() {
|
||||
}
|
||||
|
||||
if (!m_properties_cached) {
|
||||
SDEBUG3("{}", "caching properties");
|
||||
SDEBUG3("caching properties");
|
||||
DeviceCacheProperties();
|
||||
DeviceCacheVideoModes();
|
||||
DeviceCacheMode();
|
||||
m_properties_cached = true;
|
||||
} else {
|
||||
SDEBUG3("{}", "restoring video mode");
|
||||
SDEBUG3("restoring video mode");
|
||||
DeviceSetMode();
|
||||
}
|
||||
|
||||
|
||||
@@ -6,15 +6,23 @@
|
||||
|
||||
#include <wpi/Logger.h>
|
||||
|
||||
#define LOG(level, format, ...) WPI_LOG(m_logger, level, format, __VA_ARGS__)
|
||||
#define LOG(level, format, ...) \
|
||||
WPI_LOG(m_logger, level, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#undef ERROR
|
||||
#define ERROR(format, ...) WPI_ERROR(m_logger, format, __VA_ARGS__)
|
||||
#define WARNING(format, ...) WPI_WARNING(m_logger, format, __VA_ARGS__)
|
||||
#define INFO(format, ...) WPI_INFO(m_logger, format, __VA_ARGS__)
|
||||
#define ERROR(format, ...) \
|
||||
WPI_ERROR(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WARNING(format, ...) \
|
||||
WPI_WARNING(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define INFO(format, ...) WPI_INFO(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#define DEBUG0(format, ...) WPI_DEBUG(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG1(format, ...) WPI_DEBUG1(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG2(format, ...) WPI_DEBUG2(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG3(format, ...) WPI_DEBUG3(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG4(format, ...) WPI_DEBUG4(m_logger, format, __VA_ARGS__)
|
||||
#define DEBUG0(format, ...) \
|
||||
WPI_DEBUG(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG1(format, ...) \
|
||||
WPI_DEBUG1(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG2(format, ...) \
|
||||
WPI_DEBUG2(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG3(format, ...) \
|
||||
WPI_DEBUG3(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define DEBUG4(format, ...) \
|
||||
WPI_DEBUG4(m_logger, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
@@ -131,7 +131,7 @@ NCImpl::NCImpl(int inst, std::string_view id, net::ILocalStorage& localStorage,
|
||||
m_loop{*m_loopRunner.GetLoop()} {
|
||||
m_localMsgs.reserve(net::NetworkLoopQueue::kInitialQueueSize);
|
||||
|
||||
INFO("{}", "starting network client");
|
||||
INFO("starting network client");
|
||||
}
|
||||
|
||||
void NCImpl::SetServers(
|
||||
@@ -285,13 +285,13 @@ void NCImpl3::TcpConnected(uv::Tcp& tcp) {
|
||||
}
|
||||
});
|
||||
tcp.end.connect([this, &tcp] {
|
||||
DEBUG3("{}", "NT3 TCP read ended");
|
||||
DEBUG3("NT3 TCP read ended");
|
||||
if (!tcp.IsLoopClosing()) {
|
||||
Disconnect("remote end closed connection");
|
||||
}
|
||||
});
|
||||
tcp.closed.connect([this, &tcp] {
|
||||
DEBUG3("{}", "NT3 TCP connection closed");
|
||||
DEBUG3("NT3 TCP connection closed");
|
||||
if (!tcp.IsLoopClosing()) {
|
||||
Disconnect(m_wire->GetDisconnectReason());
|
||||
}
|
||||
|
||||
@@ -360,7 +360,7 @@ void NSImpl::LoadPersistent() {
|
||||
is.readinto(m_persistentData, size);
|
||||
DEBUG4("read data: {}", m_persistentData);
|
||||
if (is.has_error()) {
|
||||
WARNING("{}", "error reading persistent file");
|
||||
WARNING("error reading persistent file");
|
||||
return;
|
||||
}
|
||||
}
|
||||
@@ -453,7 +453,7 @@ void NSImpl::Init() {
|
||||
if (uv::AddrToName(tcp->GetPeer(), &peerAddr, &peerPort) == 0) {
|
||||
INFO("Got a NT3 connection from {} port {}", peerAddr, peerPort);
|
||||
} else {
|
||||
INFO("{}", "Got a NT3 connection from unknown");
|
||||
INFO("Got a NT3 connection from unknown");
|
||||
}
|
||||
auto conn = std::make_shared<ServerConnection3>(tcp, *this, peerAddr,
|
||||
peerPort, m_logger);
|
||||
@@ -485,7 +485,7 @@ void NSImpl::Init() {
|
||||
if (uv::AddrToName(tcp->GetPeer(), &peerAddr, &peerPort) == 0) {
|
||||
INFO("Got a NT4 connection from {} port {}", peerAddr, peerPort);
|
||||
} else {
|
||||
INFO("{}", "Got a NT4 connection from unknown");
|
||||
INFO("Got a NT4 connection from unknown");
|
||||
}
|
||||
auto conn = std::make_shared<ServerConnection4>(tcp, *this, peerAddr,
|
||||
peerPort, m_logger);
|
||||
@@ -496,7 +496,7 @@ void NSImpl::Init() {
|
||||
}
|
||||
|
||||
if (m_initDone) {
|
||||
DEBUG4("{}", "NetworkServer initDone()");
|
||||
DEBUG4("NetworkServer initDone()");
|
||||
m_initDone();
|
||||
m_initDone = nullptr;
|
||||
}
|
||||
|
||||
@@ -171,7 +171,7 @@ void CImpl::ProcessIncomingBinary(std::span<const uint8_t> data) {
|
||||
}
|
||||
|
||||
void CImpl::HandleLocal(std::vector<ClientMessage>&& msgs) {
|
||||
DEBUG4("{}", "HandleLocal()");
|
||||
DEBUG4("HandleLocal()");
|
||||
for (auto&& elem : msgs) {
|
||||
// common case is value
|
||||
if (auto msg = std::get_if<ClientValueMsg>(&elem.contents)) {
|
||||
|
||||
@@ -45,7 +45,7 @@ void NetworkLoopQueue::SetValue(NT_Publisher pubHandle, const Value& value) {
|
||||
m_size += sizeof(ClientMessage);
|
||||
if (m_size > kMaxSize) {
|
||||
if (!m_sizeErrored) {
|
||||
WPI_ERROR(m_logger, "{}", "NT: dropping value set due to memory limits");
|
||||
WPI_ERROR(m_logger, "NT: dropping value set due to memory limits");
|
||||
m_sizeErrored = true;
|
||||
}
|
||||
return; // avoid potential out of memory
|
||||
|
||||
@@ -758,7 +758,7 @@ void ClientDataLocal::SendPropertiesUpdate(TopicData* topic,
|
||||
}
|
||||
|
||||
void ClientDataLocal::HandleLocal(std::span<const ClientMessage> msgs) {
|
||||
DEBUG4("{}", "HandleLocal()");
|
||||
DEBUG4("HandleLocal()");
|
||||
// just map as a normal client into client=0 calls
|
||||
for (const auto& elem : msgs) { // NOLINT
|
||||
// common case is value, so check that first
|
||||
@@ -2145,7 +2145,7 @@ void SImpl::UpdateMetaClients(const std::vector<ConnectionInfo>& conns) {
|
||||
if (mpack_writer_destroy(&w) == mpack_ok) {
|
||||
SetValue(nullptr, m_metaClients, Value::MakeRaw(std::move(w.bytes)));
|
||||
} else {
|
||||
DEBUG4("{}", "failed to encode $clients");
|
||||
DEBUG4("failed to encode $clients");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -119,7 +119,7 @@ static void WireDecodeTextImpl(std::string_view in, T& out,
|
||||
}
|
||||
|
||||
if (!j.is_array()) {
|
||||
WPI_WARNING(logger, "{}", "expected JSON array at top level");
|
||||
WPI_WARNING(logger, "expected JSON array at top level");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -238,7 +238,7 @@ void CImpl::SendPeriodic(uint64_t curTimeMs, bool initial) {
|
||||
if (!CheckNetworkReady()) {
|
||||
return;
|
||||
}
|
||||
DEBUG4("{}", "Sending keep alive");
|
||||
DEBUG4("Sending keep alive");
|
||||
WireEncodeKeepAlive(out.stream());
|
||||
// drift isn't critical here, so just go from current time
|
||||
m_nextKeepAliveTimeMs = curTimeMs + kKeepAliveIntervalMs;
|
||||
@@ -274,7 +274,7 @@ void CImpl::SendPeriodic(uint64_t curTimeMs, bool initial) {
|
||||
}
|
||||
|
||||
if (initial) {
|
||||
DEBUG4("{}", "Sending ClientHelloDone");
|
||||
DEBUG4("Sending ClientHelloDone");
|
||||
WireEncodeClientHelloDone(out.stream());
|
||||
}
|
||||
|
||||
@@ -403,7 +403,7 @@ void CImpl::SetValue(NT_Publisher pubHandle, const Value& value) {
|
||||
}
|
||||
|
||||
void CImpl::KeepAlive() {
|
||||
DEBUG4("{}", "KeepAlive()");
|
||||
DEBUG4("KeepAlive()");
|
||||
if (m_state != kStateRunning && m_state != kStateInitialAssignments) {
|
||||
m_decoder.SetError("received unexpected KeepAlive message");
|
||||
return;
|
||||
@@ -412,7 +412,7 @@ void CImpl::KeepAlive() {
|
||||
}
|
||||
|
||||
void CImpl::ServerHelloDone() {
|
||||
DEBUG4("{}", "ServerHelloDone()");
|
||||
DEBUG4("ServerHelloDone()");
|
||||
if (m_state != kStateInitialAssignments) {
|
||||
m_decoder.SetError("received unexpected ServerHelloDone message");
|
||||
return;
|
||||
@@ -426,7 +426,7 @@ void CImpl::ServerHelloDone() {
|
||||
}
|
||||
|
||||
void CImpl::ClientHelloDone() {
|
||||
DEBUG4("{}", "ClientHelloDone()");
|
||||
DEBUG4("ClientHelloDone()");
|
||||
m_decoder.SetError("received unexpected ClientHelloDone message");
|
||||
}
|
||||
|
||||
@@ -572,7 +572,7 @@ void CImpl::EntryDelete(unsigned int id) {
|
||||
}
|
||||
|
||||
void CImpl::ClearEntries() {
|
||||
DEBUG4("{}", "ClearEntries()");
|
||||
DEBUG4("ClearEntries()");
|
||||
if (m_state != kStateRunning) {
|
||||
m_decoder.SetError("received ClearEntries message before ServerHelloDone");
|
||||
return;
|
||||
@@ -609,7 +609,7 @@ ClientImpl3::ClientImpl3(uint64_t curTimeMs, int inst, WireConnection3& wire,
|
||||
std::move(setPeriodic))} {}
|
||||
|
||||
ClientImpl3::~ClientImpl3() {
|
||||
WPI_DEBUG4(m_impl->m_logger, "{}", "NT3 ClientImpl destroyed");
|
||||
WPI_DEBUG4(m_impl->m_logger, "NT3 ClientImpl destroyed");
|
||||
}
|
||||
|
||||
void ClientImpl3::Start(std::string_view selfId,
|
||||
|
||||
@@ -79,7 +79,7 @@ bool DeploySession::ChangeTeamNumber(const std::string& macAddress,
|
||||
session.Open();
|
||||
DEBUG("SSH connection to {} was successful.", ip);
|
||||
|
||||
SUCCESS("{}", "roboRIO Connected!");
|
||||
SUCCESS("roboRIO Connected!");
|
||||
|
||||
try {
|
||||
session.Execute(fmt::format(
|
||||
@@ -123,7 +123,7 @@ bool DeploySession::Reboot(const std::string& macAddress,
|
||||
session.Open();
|
||||
DEBUG("SSH connection to {} was successful.", ip);
|
||||
|
||||
SUCCESS("{}", "roboRIO Connected!");
|
||||
SUCCESS("roboRIO Connected!");
|
||||
|
||||
try {
|
||||
session.Execute(fmt::format("sync ; shutdown -r now"));
|
||||
@@ -162,7 +162,7 @@ bool DeploySession::Blink(const std::string& macAddress,
|
||||
session.Open();
|
||||
DEBUG("SSH connection to {} was successful.", ip);
|
||||
|
||||
SUCCESS("{}", "roboRIO Connected!");
|
||||
SUCCESS("roboRIO Connected!");
|
||||
|
||||
try {
|
||||
session.Execute(fmt::format(
|
||||
|
||||
@@ -18,7 +18,7 @@
|
||||
|
||||
using namespace sysid;
|
||||
|
||||
#define INFO(fmt, ...) WPI_INFO(m_logger, fmt, __VA_ARGS__)
|
||||
#define INFO(fmt, ...) WPI_INFO(m_logger, fmt __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
SshSession::SshSession(std::string_view host, int port, std::string_view user,
|
||||
std::string_view pass, wpi::Logger& logger)
|
||||
|
||||
@@ -49,7 +49,7 @@ index ec1a1633a..8de7b726d 100644
|
||||
- dbgs() << " at " << file << ":" << line;
|
||||
- dbgs() << "!\n";
|
||||
+ fmt::print(stderr, " at {}:{}", file, line);
|
||||
+ fmt::print(stderr, "{}", "!\n");
|
||||
+ fmt::print(stderr, "!\n");
|
||||
abort();
|
||||
#ifdef LLVM_BUILTIN_UNREACHABLE
|
||||
// Windows systems and possibly others don't declare abort() to be noreturn,
|
||||
|
||||
@@ -9,7 +9,7 @@ using namespace frc2;
|
||||
bool CommandGroupBase::RequireUngrouped(const Command& command) {
|
||||
if (command.IsGrouped()) {
|
||||
throw FRC_MakeError(
|
||||
frc::err::CommandIllegalUse, "{}",
|
||||
frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to more than one CommandGroup");
|
||||
}
|
||||
return true;
|
||||
@@ -27,7 +27,7 @@ bool CommandGroupBase::RequireUngrouped(
|
||||
}
|
||||
if (!allUngrouped) {
|
||||
throw FRC_MakeError(
|
||||
frc::err::CommandIllegalUse, "{}",
|
||||
frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to more than one CommandGroup");
|
||||
}
|
||||
return allUngrouped;
|
||||
@@ -41,7 +41,7 @@ bool CommandGroupBase::RequireUngrouped(
|
||||
}
|
||||
if (!allUngrouped) {
|
||||
throw FRC_MakeError(
|
||||
frc::err::CommandIllegalUse, "{}",
|
||||
frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to more than one CommandGroup");
|
||||
}
|
||||
return allUngrouped;
|
||||
|
||||
@@ -119,7 +119,7 @@ void CommandScheduler::Schedule(Command* command) {
|
||||
}
|
||||
|
||||
if (command->IsGrouped()) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"A command that is part of a command group "
|
||||
"cannot be independently scheduled");
|
||||
return;
|
||||
|
||||
@@ -65,7 +65,7 @@ void ParallelCommandGroup::AddCommands(
|
||||
}
|
||||
|
||||
if (isRunning) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
@@ -77,7 +77,7 @@ void ParallelCommandGroup::AddCommands(
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands.emplace_back(std::move(command), false);
|
||||
} else {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Multiple commands in a parallel group cannot "
|
||||
"require the same subsystems");
|
||||
}
|
||||
|
||||
@@ -60,7 +60,7 @@ void ParallelDeadlineGroup::AddCommands(
|
||||
}
|
||||
|
||||
if (!m_finished) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
@@ -72,7 +72,7 @@ void ParallelDeadlineGroup::AddCommands(
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands.emplace_back(std::move(command), false);
|
||||
} else {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Multiple commands in a parallel group cannot "
|
||||
"require the same subsystems");
|
||||
}
|
||||
|
||||
@@ -50,7 +50,7 @@ void ParallelRaceGroup::AddCommands(
|
||||
}
|
||||
|
||||
if (isRunning) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
@@ -62,7 +62,7 @@ void ParallelRaceGroup::AddCommands(
|
||||
m_runWhenDisabled &= command->RunsWhenDisabled();
|
||||
m_commands.emplace_back(std::move(command));
|
||||
} else {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Multiple commands in a parallel group cannot "
|
||||
"require the same subsystems");
|
||||
}
|
||||
|
||||
@@ -60,7 +60,7 @@ void SequentialCommandGroup::AddCommands(
|
||||
}
|
||||
|
||||
if (m_currentCommandIndex != invalid_index) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Commands cannot be added to a CommandGroup "
|
||||
"while the group is running");
|
||||
}
|
||||
|
||||
@@ -179,11 +179,11 @@ class CommandScheduler final : public nt::NTSendable,
|
||||
Command, std::remove_reference_t<T>>>>
|
||||
void SetDefaultCommand(Subsystem* subsystem, T&& defaultCommand) {
|
||||
if (!defaultCommand.HasRequirement(subsystem)) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Default commands must require their subsystem!");
|
||||
}
|
||||
if (defaultCommand.IsFinished()) {
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse, "{}",
|
||||
throw FRC_MakeError(frc::err::CommandIllegalUse,
|
||||
"Default commands should not end!");
|
||||
}
|
||||
SetDefaultCommandImpl(subsystem,
|
||||
|
||||
@@ -61,9 +61,9 @@ inline void ADISReportError(int32_t status, const char* file, int line,
|
||||
} // namespace
|
||||
|
||||
#define REPORT_WARNING(msg) \
|
||||
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
|
||||
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, msg);
|
||||
#define REPORT_ERROR(msg) \
|
||||
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
|
||||
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, msg)
|
||||
|
||||
ADIS16448_IMU::ADIS16448_IMU()
|
||||
: ADIS16448_IMU(kZ, SPI::Port::kMXP, CalibrationTime::_512ms) {}
|
||||
|
||||
@@ -55,9 +55,9 @@ inline void ADISReportError(int32_t status, const char* file, int line,
|
||||
} // namespace
|
||||
|
||||
#define REPORT_WARNING(msg) \
|
||||
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
|
||||
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, msg);
|
||||
#define REPORT_ERROR(msg) \
|
||||
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
|
||||
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, msg)
|
||||
|
||||
/**
|
||||
* Constructor.
|
||||
|
||||
@@ -55,7 +55,7 @@ ADXL362::ADXL362(SPI::Port port, Range range)
|
||||
commands[2] = 0;
|
||||
m_spi.Transaction(commands, commands, 3);
|
||||
if (commands[2] != 0xF2) {
|
||||
FRC_ReportError(err::Error, "{}", "could not find ADXL362");
|
||||
FRC_ReportError(err::Error, "could not find ADXL362");
|
||||
m_gsPerLSB = 0.0;
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -44,7 +44,7 @@ ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
|
||||
if (!m_simDevice) {
|
||||
// Validate the part ID
|
||||
if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
|
||||
FRC_ReportError(err::Error, "{}", "could not find ADXRS450 gyro");
|
||||
FRC_ReportError(err::Error, "could not find ADXRS450 gyro");
|
||||
return;
|
||||
}
|
||||
|
||||
|
||||
@@ -21,7 +21,7 @@ AnalogAccelerometer::AnalogAccelerometer(int channel)
|
||||
AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
|
||||
: m_analogInput(channel, wpi::NullDeleter<AnalogInput>()) {
|
||||
if (!channel) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "channel");
|
||||
throw FRC_MakeError(err::NullParameter, "channel");
|
||||
}
|
||||
InitAccelerometer();
|
||||
}
|
||||
@@ -29,7 +29,7 @@ AnalogAccelerometer::AnalogAccelerometer(AnalogInput* channel)
|
||||
AnalogAccelerometer::AnalogAccelerometer(std::shared_ptr<AnalogInput> channel)
|
||||
: m_analogInput(channel) {
|
||||
if (!channel) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "channel");
|
||||
throw FRC_MakeError(err::NullParameter, "channel");
|
||||
}
|
||||
InitAccelerometer();
|
||||
}
|
||||
|
||||
@@ -83,7 +83,7 @@ units::turn_t AnalogEncoder::Get() const {
|
||||
}
|
||||
|
||||
FRC_ReportError(
|
||||
warn::Warning, "{}",
|
||||
warn::Warning,
|
||||
"Failed to read Analog Encoder. Potential Speed Overrun. Returning last "
|
||||
"value");
|
||||
return m_lastPosition;
|
||||
|
||||
@@ -33,7 +33,7 @@ AnalogGyro::AnalogGyro(AnalogInput* channel)
|
||||
AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
|
||||
: m_analog(channel) {
|
||||
if (!channel) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "channel");
|
||||
throw FRC_MakeError(err::NullParameter, "channel");
|
||||
}
|
||||
InitGyro();
|
||||
Calibrate();
|
||||
@@ -48,7 +48,7 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, int center,
|
||||
double offset)
|
||||
: m_analog(channel) {
|
||||
if (!channel) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "channel");
|
||||
throw FRC_MakeError(err::NullParameter, "channel");
|
||||
}
|
||||
InitGyro();
|
||||
int32_t status = 0;
|
||||
|
||||
@@ -180,13 +180,13 @@ void AnalogInput::GetAccumulatorOutput(int64_t& value, int64_t& count) const {
|
||||
void AnalogInput::SetSampleRate(double samplesPerSecond) {
|
||||
int32_t status = 0;
|
||||
HAL_SetAnalogSampleRate(samplesPerSecond, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetSampleRate");
|
||||
FRC_CheckErrorStatus(status, "SetSampleRate");
|
||||
}
|
||||
|
||||
double AnalogInput::GetSampleRate() {
|
||||
int32_t status = 0;
|
||||
double sampleRate = HAL_GetAnalogSampleRate(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetSampleRate");
|
||||
FRC_CheckErrorStatus(status, "GetSampleRate");
|
||||
return sampleRate;
|
||||
}
|
||||
|
||||
|
||||
@@ -18,7 +18,7 @@ bool AnalogTriggerOutput::Get() const {
|
||||
bool result = HAL_GetAnalogTriggerOutput(
|
||||
m_trigger->m_trigger, static_cast<HAL_AnalogTriggerType>(m_outputType),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Get");
|
||||
FRC_CheckErrorStatus(status, "Get");
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
@@ -23,7 +23,7 @@ BuiltInAccelerometer::BuiltInAccelerometer(Range range) {
|
||||
|
||||
void BuiltInAccelerometer::SetRange(Range range) {
|
||||
if (range == kRange_16G) {
|
||||
throw FRC_MakeError(err::ParameterOutOfRange, "{}",
|
||||
throw FRC_MakeError(err::ParameterOutOfRange,
|
||||
"16G range not supported (use k2G, k4G, or k8G)");
|
||||
}
|
||||
|
||||
|
||||
@@ -45,20 +45,20 @@ CAN::~CAN() {
|
||||
void CAN::WritePacket(const uint8_t* data, int length, int apiId) {
|
||||
int32_t status = 0;
|
||||
HAL_WriteCANPacket(m_handle, data, length, apiId, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "WritePacket");
|
||||
FRC_CheckErrorStatus(status, "WritePacket");
|
||||
}
|
||||
|
||||
void CAN::WritePacketRepeating(const uint8_t* data, int length, int apiId,
|
||||
int repeatMs) {
|
||||
int32_t status = 0;
|
||||
HAL_WriteCANPacketRepeating(m_handle, data, length, apiId, repeatMs, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "WritePacketRepeating");
|
||||
FRC_CheckErrorStatus(status, "WritePacketRepeating");
|
||||
}
|
||||
|
||||
void CAN::WriteRTRFrame(int length, int apiId) {
|
||||
int32_t status = 0;
|
||||
HAL_WriteCANRTRFrame(m_handle, length, apiId, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "WriteRTRFrame");
|
||||
FRC_CheckErrorStatus(status, "WriteRTRFrame");
|
||||
}
|
||||
|
||||
int CAN::WritePacketNoError(const uint8_t* data, int length, int apiId) {
|
||||
@@ -83,7 +83,7 @@ int CAN::WriteRTRFrameNoError(int length, int apiId) {
|
||||
void CAN::StopPacketRepeating(int apiId) {
|
||||
int32_t status = 0;
|
||||
HAL_StopCANPacketRepeating(m_handle, apiId, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "StopPacketRepeating");
|
||||
FRC_CheckErrorStatus(status, "StopPacketRepeating");
|
||||
}
|
||||
|
||||
bool CAN::ReadPacketNew(int apiId, CANData* data) {
|
||||
@@ -94,7 +94,7 @@ bool CAN::ReadPacketNew(int apiId, CANData* data) {
|
||||
return false;
|
||||
}
|
||||
if (status != 0) {
|
||||
FRC_CheckErrorStatus(status, "{}", "ReadPacketNew");
|
||||
FRC_CheckErrorStatus(status, "ReadPacketNew");
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
@@ -109,7 +109,7 @@ bool CAN::ReadPacketLatest(int apiId, CANData* data) {
|
||||
return false;
|
||||
}
|
||||
if (status != 0) {
|
||||
FRC_CheckErrorStatus(status, "{}", "ReadPacketLatest");
|
||||
FRC_CheckErrorStatus(status, "ReadPacketLatest");
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
@@ -125,7 +125,7 @@ bool CAN::ReadPacketTimeout(int apiId, int timeoutMs, CANData* data) {
|
||||
return false;
|
||||
}
|
||||
if (status != 0) {
|
||||
FRC_CheckErrorStatus(status, "{}", "ReadPacketTimeout");
|
||||
FRC_CheckErrorStatus(status, "ReadPacketTimeout");
|
||||
return false;
|
||||
} else {
|
||||
return true;
|
||||
|
||||
@@ -22,7 +22,7 @@ Counter::Counter(Mode mode) {
|
||||
int32_t status = 0;
|
||||
m_counter = HAL_InitializeCounter(static_cast<HAL_Counter_Mode>(mode),
|
||||
&m_index, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitializeCounter");
|
||||
FRC_CheckErrorStatus(status, "InitializeCounter");
|
||||
|
||||
SetMaxPeriod(0.5_s);
|
||||
|
||||
@@ -64,7 +64,7 @@ Counter::Counter(EncodingType encodingType,
|
||||
std::shared_ptr<DigitalSource> downSource, bool inverted)
|
||||
: Counter(kExternalDirection) {
|
||||
if (encodingType != k1X && encodingType != k2X) {
|
||||
throw FRC_MakeError(err::ParameterOutOfRange, "{}",
|
||||
throw FRC_MakeError(err::ParameterOutOfRange,
|
||||
"Counter only supports 1X and 2X quadrature decoding");
|
||||
}
|
||||
SetUpSource(upSource);
|
||||
@@ -79,7 +79,7 @@ Counter::Counter(EncodingType encodingType,
|
||||
HAL_SetCounterAverageSize(m_counter, 2, &status);
|
||||
}
|
||||
|
||||
FRC_CheckErrorStatus(status, "{}", "Counter constructor");
|
||||
FRC_CheckErrorStatus(status, "Counter constructor");
|
||||
SetDownSourceEdge(inverted, true);
|
||||
}
|
||||
|
||||
@@ -92,7 +92,7 @@ Counter::~Counter() {
|
||||
|
||||
int32_t status = 0;
|
||||
HAL_FreeCounter(m_counter, &status);
|
||||
FRC_ReportError(status, "{}", "Counter destructor");
|
||||
FRC_ReportError(status, "Counter destructor");
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(int channel) {
|
||||
@@ -124,7 +124,7 @@ void Counter::SetUpSource(std::shared_ptr<DigitalSource> source) {
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
source->GetAnalogTriggerTypeForRouting()),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetUpSource");
|
||||
FRC_CheckErrorStatus(status, "SetUpSource");
|
||||
}
|
||||
|
||||
void Counter::SetUpSource(DigitalSource& source) {
|
||||
@@ -135,19 +135,19 @@ void Counter::SetUpSource(DigitalSource& source) {
|
||||
void Counter::SetUpSourceEdge(bool risingEdge, bool fallingEdge) {
|
||||
if (m_upSource == nullptr) {
|
||||
throw FRC_MakeError(
|
||||
err::NullParameter, "{}",
|
||||
err::NullParameter,
|
||||
"Must set non-nullptr UpSource before setting UpSourceEdge");
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpSourceEdge(m_counter, risingEdge, fallingEdge, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetUpSourceEdge");
|
||||
FRC_CheckErrorStatus(status, "SetUpSourceEdge");
|
||||
}
|
||||
|
||||
void Counter::ClearUpSource() {
|
||||
m_upSource.reset();
|
||||
int32_t status = 0;
|
||||
HAL_ClearCounterUpSource(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "ClearUpSource");
|
||||
FRC_CheckErrorStatus(status, "ClearUpSource");
|
||||
}
|
||||
|
||||
void Counter::SetDownSource(int channel) {
|
||||
@@ -184,37 +184,37 @@ void Counter::SetDownSource(std::shared_ptr<DigitalSource> source) {
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
source->GetAnalogTriggerTypeForRouting()),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetDownSource");
|
||||
FRC_CheckErrorStatus(status, "SetDownSource");
|
||||
}
|
||||
|
||||
void Counter::SetDownSourceEdge(bool risingEdge, bool fallingEdge) {
|
||||
if (m_downSource == nullptr) {
|
||||
throw FRC_MakeError(
|
||||
err::NullParameter, "{}",
|
||||
err::NullParameter,
|
||||
"Must set non-nullptr DownSource before setting DownSourceEdge");
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterDownSourceEdge(m_counter, risingEdge, fallingEdge, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetDownSourceEdge");
|
||||
FRC_CheckErrorStatus(status, "SetDownSourceEdge");
|
||||
}
|
||||
|
||||
void Counter::ClearDownSource() {
|
||||
m_downSource.reset();
|
||||
int32_t status = 0;
|
||||
HAL_ClearCounterDownSource(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "ClearDownSource");
|
||||
FRC_CheckErrorStatus(status, "ClearDownSource");
|
||||
}
|
||||
|
||||
void Counter::SetUpDownCounterMode() {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpDownMode(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetUpDownCounterMode");
|
||||
FRC_CheckErrorStatus(status, "SetUpDownCounterMode");
|
||||
}
|
||||
|
||||
void Counter::SetExternalDirectionMode() {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterExternalDirectionMode(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetExternalDirectionMode");
|
||||
FRC_CheckErrorStatus(status, "SetExternalDirectionMode");
|
||||
}
|
||||
|
||||
void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
|
||||
@@ -227,7 +227,7 @@ void Counter::SetSemiPeriodMode(bool highSemiPeriod) {
|
||||
void Counter::SetPulseLengthMode(double threshold) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterPulseLengthMode(m_counter, threshold, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetPulseLengthMode");
|
||||
FRC_CheckErrorStatus(status, "SetPulseLengthMode");
|
||||
}
|
||||
|
||||
void Counter::SetReverseDirection(bool reverseDirection) {
|
||||
@@ -252,7 +252,7 @@ void Counter::SetSamplesToAverage(int samplesToAverage) {
|
||||
int Counter::GetSamplesToAverage() const {
|
||||
int32_t status = 0;
|
||||
int samples = HAL_GetCounterSamplesToAverage(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetSamplesToAverage");
|
||||
FRC_CheckErrorStatus(status, "GetSamplesToAverage");
|
||||
return samples;
|
||||
}
|
||||
|
||||
@@ -263,46 +263,46 @@ int Counter::GetFPGAIndex() const {
|
||||
int Counter::Get() const {
|
||||
int32_t status = 0;
|
||||
int value = HAL_GetCounter(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Get");
|
||||
FRC_CheckErrorStatus(status, "Get");
|
||||
return value;
|
||||
}
|
||||
|
||||
void Counter::Reset() {
|
||||
int32_t status = 0;
|
||||
HAL_ResetCounter(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Reset");
|
||||
FRC_CheckErrorStatus(status, "Reset");
|
||||
}
|
||||
|
||||
units::second_t Counter::GetPeriod() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetCounterPeriod(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetPeriod");
|
||||
FRC_CheckErrorStatus(status, "GetPeriod");
|
||||
return units::second_t{value};
|
||||
}
|
||||
|
||||
void Counter::SetMaxPeriod(units::second_t maxPeriod) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterMaxPeriod(m_counter, maxPeriod.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod");
|
||||
FRC_CheckErrorStatus(status, "SetMaxPeriod");
|
||||
}
|
||||
|
||||
void Counter::SetUpdateWhenEmpty(bool enabled) {
|
||||
int32_t status = 0;
|
||||
HAL_SetCounterUpdateWhenEmpty(m_counter, enabled, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetUpdateWhenEmpty");
|
||||
FRC_CheckErrorStatus(status, "SetUpdateWhenEmpty");
|
||||
}
|
||||
|
||||
bool Counter::GetStopped() const {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetCounterStopped(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetStopped");
|
||||
FRC_CheckErrorStatus(status, "GetStopped");
|
||||
return value;
|
||||
}
|
||||
|
||||
bool Counter::GetDirection() const {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetCounterDirection(m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetDirection");
|
||||
FRC_CheckErrorStatus(status, "GetDirection");
|
||||
return value;
|
||||
}
|
||||
|
||||
|
||||
@@ -27,7 +27,7 @@ using namespace frc;
|
||||
DMA::DMA() {
|
||||
int32_t status = 0;
|
||||
dmaHandle = HAL_InitializeDMA(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitializeDMA");
|
||||
FRC_CheckErrorStatus(status, "InitializeDMA");
|
||||
}
|
||||
|
||||
DMA::~DMA() {
|
||||
@@ -37,74 +37,74 @@ DMA::~DMA() {
|
||||
void DMA::SetPause(bool pause) {
|
||||
int32_t status = 0;
|
||||
HAL_SetDMAPause(dmaHandle, pause, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetPause");
|
||||
FRC_CheckErrorStatus(status, "SetPause");
|
||||
}
|
||||
|
||||
void DMA::SetTimedTrigger(units::second_t seconds) {
|
||||
int32_t status = 0;
|
||||
HAL_SetDMATimedTrigger(dmaHandle, seconds.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetTimedTrigger");
|
||||
FRC_CheckErrorStatus(status, "SetTimedTrigger");
|
||||
}
|
||||
|
||||
void DMA::SetTimedTriggerCycles(int cycles) {
|
||||
int32_t status = 0;
|
||||
HAL_SetDMATimedTriggerCycles(dmaHandle, cycles, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetTimedTriggerCycles");
|
||||
FRC_CheckErrorStatus(status, "SetTimedTriggerCycles");
|
||||
}
|
||||
|
||||
void DMA::AddEncoder(const Encoder* encoder) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAEncoder(dmaHandle, encoder->m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddEncoder");
|
||||
FRC_CheckErrorStatus(status, "AddEncoder");
|
||||
}
|
||||
|
||||
void DMA::AddEncoderPeriod(const Encoder* encoder) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAEncoderPeriod(dmaHandle, encoder->m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddEncoderPeriod");
|
||||
FRC_CheckErrorStatus(status, "AddEncoderPeriod");
|
||||
}
|
||||
|
||||
void DMA::AddCounter(const Counter* counter) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMACounter(dmaHandle, counter->m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddCounter");
|
||||
FRC_CheckErrorStatus(status, "AddCounter");
|
||||
}
|
||||
|
||||
void DMA::AddCounterPeriod(const Counter* counter) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMACounterPeriod(dmaHandle, counter->m_counter, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddCounterPeriod");
|
||||
FRC_CheckErrorStatus(status, "AddCounterPeriod");
|
||||
}
|
||||
|
||||
void DMA::AddDigitalSource(const DigitalSource* digitalSource) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMADigitalSource(dmaHandle, digitalSource->GetPortHandleForRouting(),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddDigitalSource");
|
||||
FRC_CheckErrorStatus(status, "AddDigitalSource");
|
||||
}
|
||||
|
||||
void DMA::AddDutyCycle(const DutyCycle* dutyCycle) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMADutyCycle(dmaHandle, dutyCycle->m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddDutyCycle");
|
||||
FRC_CheckErrorStatus(status, "AddDutyCycle");
|
||||
}
|
||||
|
||||
void DMA::AddAnalogInput(const AnalogInput* analogInput) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAnalogInput(dmaHandle, analogInput->m_port, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddAnalogInput");
|
||||
FRC_CheckErrorStatus(status, "AddAnalogInput");
|
||||
}
|
||||
|
||||
void DMA::AddAveragedAnalogInput(const AnalogInput* analogInput) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAveragedAnalogInput(dmaHandle, analogInput->m_port, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddAveragedAnalogInput");
|
||||
FRC_CheckErrorStatus(status, "AddAveragedAnalogInput");
|
||||
}
|
||||
|
||||
void DMA::AddAnalogAccumulator(const AnalogInput* analogInput) {
|
||||
int32_t status = 0;
|
||||
HAL_AddDMAAnalogAccumulator(dmaHandle, analogInput->m_port, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "AddAnalogAccumulator");
|
||||
FRC_CheckErrorStatus(status, "AddAnalogAccumulator");
|
||||
}
|
||||
|
||||
int DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
|
||||
@@ -114,7 +114,7 @@ int DMA::SetExternalTrigger(DigitalSource* source, bool rising, bool falling) {
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
source->GetAnalogTriggerTypeForRouting()),
|
||||
rising, falling, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetExternalTrigger");
|
||||
FRC_CheckErrorStatus(status, "SetExternalTrigger");
|
||||
return idx;
|
||||
}
|
||||
|
||||
@@ -124,7 +124,7 @@ int DMA::SetPwmEdgeTrigger(PWMMotorController* source, bool rising,
|
||||
int32_t idx = HAL_SetDMAExternalTrigger(
|
||||
dmaHandle, source->GetPwm()->m_handle,
|
||||
HAL_AnalogTriggerType::HAL_Trigger_kInWindow, rising, falling, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetPWmEdgeTrigger");
|
||||
FRC_CheckErrorStatus(status, "SetPWmEdgeTrigger");
|
||||
return idx;
|
||||
}
|
||||
|
||||
@@ -133,30 +133,30 @@ int DMA::SetPwmEdgeTrigger(PWM* source, bool rising, bool falling) {
|
||||
int32_t idx = HAL_SetDMAExternalTrigger(
|
||||
dmaHandle, source->m_handle, HAL_AnalogTriggerType::HAL_Trigger_kInWindow,
|
||||
rising, falling, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetPWmEdgeTrigger");
|
||||
FRC_CheckErrorStatus(status, "SetPWmEdgeTrigger");
|
||||
return idx;
|
||||
}
|
||||
|
||||
void DMA::ClearSensors() {
|
||||
int32_t status = 0;
|
||||
HAL_ClearDMASensors(dmaHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "ClearSensors");
|
||||
FRC_CheckErrorStatus(status, "ClearSensors");
|
||||
}
|
||||
|
||||
void DMA::ClearExternalTriggers() {
|
||||
int32_t status = 0;
|
||||
HAL_ClearDMAExternalTriggers(dmaHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "ClearExternalTriggers");
|
||||
FRC_CheckErrorStatus(status, "ClearExternalTriggers");
|
||||
}
|
||||
|
||||
void DMA::Start(int queueDepth) {
|
||||
int32_t status = 0;
|
||||
HAL_StartDMA(dmaHandle, queueDepth, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Start");
|
||||
FRC_CheckErrorStatus(status, "Start");
|
||||
}
|
||||
|
||||
void DMA::Stop() {
|
||||
int32_t status = 0;
|
||||
HAL_StopDMA(dmaHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Stop");
|
||||
FRC_CheckErrorStatus(status, "Stop");
|
||||
}
|
||||
|
||||
@@ -56,7 +56,7 @@ void DigitalGlitchFilter::DoAdd(DigitalSource* input, int requestedIndex) {
|
||||
// We don't support GlitchFilters on AnalogTriggers.
|
||||
if (input->IsAnalogTrigger()) {
|
||||
throw FRC_MakeError(
|
||||
-1, "{}", "Analog Triggers not supported for DigitalGlitchFilters");
|
||||
-1, "Analog Triggers not supported for DigitalGlitchFilters");
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetFilterSelect(input->GetPortHandleForRouting(), requestedIndex,
|
||||
|
||||
@@ -635,7 +635,7 @@ double DriverStation::GetMatchTime() {
|
||||
double DriverStation::GetBatteryVoltage() {
|
||||
int32_t status = 0;
|
||||
double voltage = HAL_GetVinVoltage(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "getVinVoltage");
|
||||
FRC_CheckErrorStatus(status, "getVinVoltage");
|
||||
|
||||
return voltage;
|
||||
}
|
||||
|
||||
@@ -17,7 +17,7 @@ using namespace frc;
|
||||
DutyCycle::DutyCycle(DigitalSource* source)
|
||||
: m_source{source, wpi::NullDeleter<DigitalSource>()} {
|
||||
if (!m_source) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "source");
|
||||
throw FRC_MakeError(err::NullParameter, "source");
|
||||
}
|
||||
InitDutyCycle();
|
||||
}
|
||||
@@ -30,7 +30,7 @@ DutyCycle::DutyCycle(DigitalSource& source)
|
||||
DutyCycle::DutyCycle(std::shared_ptr<DigitalSource> source)
|
||||
: m_source{std::move(source)} {
|
||||
if (!m_source) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "source");
|
||||
throw FRC_MakeError(err::NullParameter, "source");
|
||||
}
|
||||
InitDutyCycle();
|
||||
}
|
||||
|
||||
@@ -105,7 +105,7 @@ units::turn_t DutyCycleEncoder::Get() const {
|
||||
}
|
||||
|
||||
FRC_ReportError(
|
||||
warn::Warning, "{}",
|
||||
warn::Warning,
|
||||
"Failed to read DutyCycle Encoder. Potential Speed Overrun. Returning "
|
||||
"last value");
|
||||
return m_lastPosition;
|
||||
|
||||
@@ -31,10 +31,10 @@ Encoder::Encoder(DigitalSource* aSource, DigitalSource* bSource,
|
||||
: m_aSource(aSource, wpi::NullDeleter<DigitalSource>()),
|
||||
m_bSource(bSource, wpi::NullDeleter<DigitalSource>()) {
|
||||
if (!m_aSource) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "aSource");
|
||||
throw FRC_MakeError(err::NullParameter, "aSource");
|
||||
}
|
||||
if (!m_bSource) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "bSource");
|
||||
throw FRC_MakeError(err::NullParameter, "bSource");
|
||||
}
|
||||
InitEncoder(reverseDirection, encodingType);
|
||||
}
|
||||
@@ -51,10 +51,10 @@ Encoder::Encoder(std::shared_ptr<DigitalSource> aSource,
|
||||
EncodingType encodingType)
|
||||
: m_aSource(std::move(aSource)), m_bSource(std::move(bSource)) {
|
||||
if (!m_aSource) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "aSource");
|
||||
throw FRC_MakeError(err::NullParameter, "aSource");
|
||||
}
|
||||
if (!m_bSource) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "bSource");
|
||||
throw FRC_MakeError(err::NullParameter, "bSource");
|
||||
}
|
||||
InitEncoder(reverseDirection, encodingType);
|
||||
}
|
||||
@@ -62,100 +62,100 @@ Encoder::Encoder(std::shared_ptr<DigitalSource> aSource,
|
||||
Encoder::~Encoder() {
|
||||
int32_t status = 0;
|
||||
HAL_FreeEncoder(m_encoder, &status);
|
||||
FRC_ReportError(status, "{}", "FreeEncoder");
|
||||
FRC_ReportError(status, "FreeEncoder");
|
||||
}
|
||||
|
||||
int Encoder::Get() const {
|
||||
int32_t status = 0;
|
||||
int value = HAL_GetEncoder(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Get");
|
||||
FRC_CheckErrorStatus(status, "Get");
|
||||
return value;
|
||||
}
|
||||
|
||||
void Encoder::Reset() {
|
||||
int32_t status = 0;
|
||||
HAL_ResetEncoder(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Reset");
|
||||
FRC_CheckErrorStatus(status, "Reset");
|
||||
}
|
||||
|
||||
units::second_t Encoder::GetPeriod() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetEncoderPeriod(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetPeriod");
|
||||
FRC_CheckErrorStatus(status, "GetPeriod");
|
||||
return units::second_t{value};
|
||||
}
|
||||
|
||||
void Encoder::SetMaxPeriod(units::second_t maxPeriod) {
|
||||
int32_t status = 0;
|
||||
HAL_SetEncoderMaxPeriod(m_encoder, maxPeriod.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetMaxPeriod");
|
||||
FRC_CheckErrorStatus(status, "SetMaxPeriod");
|
||||
}
|
||||
|
||||
bool Encoder::GetStopped() const {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetEncoderStopped(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetStopped");
|
||||
FRC_CheckErrorStatus(status, "GetStopped");
|
||||
return value;
|
||||
}
|
||||
|
||||
bool Encoder::GetDirection() const {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetEncoderDirection(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetDirection");
|
||||
FRC_CheckErrorStatus(status, "GetDirection");
|
||||
return value;
|
||||
}
|
||||
|
||||
int Encoder::GetRaw() const {
|
||||
int32_t status = 0;
|
||||
int value = HAL_GetEncoderRaw(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetRaw");
|
||||
FRC_CheckErrorStatus(status, "GetRaw");
|
||||
return value;
|
||||
}
|
||||
|
||||
int Encoder::GetEncodingScale() const {
|
||||
int32_t status = 0;
|
||||
int val = HAL_GetEncoderEncodingScale(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetEncodingScale");
|
||||
FRC_CheckErrorStatus(status, "GetEncodingScale");
|
||||
return val;
|
||||
}
|
||||
|
||||
double Encoder::GetDistance() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetEncoderDistance(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetDistance");
|
||||
FRC_CheckErrorStatus(status, "GetDistance");
|
||||
return value;
|
||||
}
|
||||
|
||||
double Encoder::GetRate() const {
|
||||
int32_t status = 0;
|
||||
double value = HAL_GetEncoderRate(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetRate");
|
||||
FRC_CheckErrorStatus(status, "GetRate");
|
||||
return value;
|
||||
}
|
||||
|
||||
void Encoder::SetMinRate(double minRate) {
|
||||
int32_t status = 0;
|
||||
HAL_SetEncoderMinRate(m_encoder, minRate, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetMinRate");
|
||||
FRC_CheckErrorStatus(status, "SetMinRate");
|
||||
}
|
||||
|
||||
void Encoder::SetDistancePerPulse(double distancePerPulse) {
|
||||
int32_t status = 0;
|
||||
HAL_SetEncoderDistancePerPulse(m_encoder, distancePerPulse, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetDistancePerPulse");
|
||||
FRC_CheckErrorStatus(status, "SetDistancePerPulse");
|
||||
}
|
||||
|
||||
double Encoder::GetDistancePerPulse() const {
|
||||
int32_t status = 0;
|
||||
double distancePerPulse = HAL_GetEncoderDistancePerPulse(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetDistancePerPulse");
|
||||
FRC_CheckErrorStatus(status, "GetDistancePerPulse");
|
||||
return distancePerPulse;
|
||||
}
|
||||
|
||||
void Encoder::SetReverseDirection(bool reverseDirection) {
|
||||
int32_t status = 0;
|
||||
HAL_SetEncoderReverseDirection(m_encoder, reverseDirection, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetReverseDirection");
|
||||
FRC_CheckErrorStatus(status, "SetReverseDirection");
|
||||
}
|
||||
|
||||
void Encoder::SetSamplesToAverage(int samplesToAverage) {
|
||||
@@ -167,13 +167,13 @@ void Encoder::SetSamplesToAverage(int samplesToAverage) {
|
||||
}
|
||||
int32_t status = 0;
|
||||
HAL_SetEncoderSamplesToAverage(m_encoder, samplesToAverage, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetSamplesToAverage");
|
||||
FRC_CheckErrorStatus(status, "SetSamplesToAverage");
|
||||
}
|
||||
|
||||
int Encoder::GetSamplesToAverage() const {
|
||||
int32_t status = 0;
|
||||
int result = HAL_GetEncoderSamplesToAverage(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetSamplesToAverage");
|
||||
FRC_CheckErrorStatus(status, "GetSamplesToAverage");
|
||||
return result;
|
||||
}
|
||||
|
||||
@@ -192,7 +192,7 @@ void Encoder::SetIndexSource(const DigitalSource& source,
|
||||
source.GetAnalogTriggerTypeForRouting()),
|
||||
static_cast<HAL_EncoderIndexingType>(type),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetIndexSource");
|
||||
FRC_CheckErrorStatus(status, "SetIndexSource");
|
||||
}
|
||||
|
||||
void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
|
||||
@@ -202,14 +202,14 @@ void Encoder::SetSimDevice(HAL_SimDeviceHandle device) {
|
||||
int Encoder::GetFPGAIndex() const {
|
||||
int32_t status = 0;
|
||||
int val = HAL_GetEncoderFPGAIndex(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFPGAIndex");
|
||||
FRC_CheckErrorStatus(status, "GetFPGAIndex");
|
||||
return val;
|
||||
}
|
||||
|
||||
void Encoder::InitSendable(wpi::SendableBuilder& builder) {
|
||||
int32_t status = 0;
|
||||
HAL_EncoderEncodingType type = HAL_GetEncoderEncodingType(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetEncodingType");
|
||||
FRC_CheckErrorStatus(status, "GetEncodingType");
|
||||
if (type == HAL_EncoderEncodingType::HAL_Encoder_k4X) {
|
||||
builder.SetSmartDashboardType("Quadrature Encoder");
|
||||
} else {
|
||||
@@ -236,7 +236,7 @@ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
|
||||
m_bSource->GetAnalogTriggerTypeForRouting()),
|
||||
reverseDirection, static_cast<HAL_EncoderEncodingType>(encodingType),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitEncoder");
|
||||
FRC_CheckErrorStatus(status, "InitEncoder");
|
||||
|
||||
HAL_Report(HALUsageReporting::kResourceType_Encoder, GetFPGAIndex() + 1,
|
||||
encodingType);
|
||||
@@ -246,6 +246,6 @@ void Encoder::InitEncoder(bool reverseDirection, EncodingType encodingType) {
|
||||
double Encoder::DecodingScaleFactor() const {
|
||||
int32_t status = 0;
|
||||
double val = HAL_GetEncoderDecodingScaleFactor(m_encoder, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "DecodingScaleFactor");
|
||||
FRC_CheckErrorStatus(status, "DecodingScaleFactor");
|
||||
return val;
|
||||
}
|
||||
|
||||
@@ -18,7 +18,7 @@ I2C::I2C(Port port, int deviceAddress)
|
||||
int32_t status = 0;
|
||||
|
||||
if (port == I2C::Port::kOnboard) {
|
||||
FRC_ReportError(warn::Warning, "{}",
|
||||
FRC_ReportError(warn::Warning,
|
||||
"Onboard I2C port is subject to system lockups. See Known "
|
||||
"Issues page for "
|
||||
"details");
|
||||
@@ -74,7 +74,7 @@ bool I2C::Read(int registerAddress, int count, uint8_t* buffer) {
|
||||
throw FRC_MakeError(err::ParameterOutOfRange, "count {}", count);
|
||||
}
|
||||
if (!buffer) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "buffer");
|
||||
throw FRC_MakeError(err::NullParameter, "buffer");
|
||||
}
|
||||
uint8_t regAddr = registerAddress;
|
||||
return Transaction(®Addr, sizeof(regAddr), buffer, count);
|
||||
@@ -85,7 +85,7 @@ bool I2C::ReadOnly(int count, uint8_t* buffer) {
|
||||
throw FRC_MakeError(err::ParameterOutOfRange, "count {}", count);
|
||||
}
|
||||
if (!buffer) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "buffer");
|
||||
throw FRC_MakeError(err::NullParameter, "buffer");
|
||||
}
|
||||
return HAL_ReadI2C(m_port, m_deviceAddress, buffer, count) < 0;
|
||||
}
|
||||
|
||||
@@ -18,12 +18,12 @@ using namespace frc;
|
||||
|
||||
Notifier::Notifier(std::function<void()> handler) {
|
||||
if (!handler) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "handler");
|
||||
throw FRC_MakeError(err::NullParameter, "handler");
|
||||
}
|
||||
m_handler = handler;
|
||||
int32_t status = 0;
|
||||
m_notifier = HAL_InitializeNotifier(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
|
||||
FRC_CheckErrorStatus(status, "InitializeNotifier");
|
||||
|
||||
m_thread = std::thread([=, this] {
|
||||
for (;;) {
|
||||
@@ -60,12 +60,12 @@ Notifier::Notifier(std::function<void()> handler) {
|
||||
|
||||
Notifier::Notifier(int priority, std::function<void()> handler) {
|
||||
if (!handler) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "handler");
|
||||
throw FRC_MakeError(err::NullParameter, "handler");
|
||||
}
|
||||
m_handler = handler;
|
||||
int32_t status = 0;
|
||||
m_notifier = HAL_InitializeNotifier(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
|
||||
FRC_CheckErrorStatus(status, "InitializeNotifier");
|
||||
|
||||
m_thread = std::thread([=, this] {
|
||||
int32_t status = 0;
|
||||
@@ -106,7 +106,7 @@ Notifier::~Notifier() {
|
||||
// atomically set handle to 0, then clean
|
||||
HAL_NotifierHandle handle = m_notifier.exchange(0);
|
||||
HAL_StopNotifier(handle, &status);
|
||||
FRC_ReportError(status, "{}", "StopNotifier");
|
||||
FRC_ReportError(status, "StopNotifier");
|
||||
|
||||
// Join the thread to ensure the handler has exited.
|
||||
if (m_thread.joinable()) {
|
||||
@@ -172,7 +172,7 @@ void Notifier::Stop() {
|
||||
m_periodic = false;
|
||||
int32_t status = 0;
|
||||
HAL_CancelNotifierAlarm(m_notifier, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "CancelNotifierAlarm");
|
||||
FRC_CheckErrorStatus(status, "CancelNotifierAlarm");
|
||||
}
|
||||
|
||||
void Notifier::UpdateAlarm(uint64_t triggerTime) {
|
||||
@@ -183,7 +183,7 @@ void Notifier::UpdateAlarm(uint64_t triggerTime) {
|
||||
return;
|
||||
}
|
||||
HAL_UpdateNotifierAlarm(notifier, triggerTime, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "UpdateNotifierAlarm");
|
||||
FRC_CheckErrorStatus(status, "UpdateNotifierAlarm");
|
||||
}
|
||||
|
||||
void Notifier::UpdateAlarm() {
|
||||
|
||||
@@ -148,7 +148,7 @@ void PneumaticHub::EnableCompressorAnalog(
|
||||
units::pounds_per_square_inch_t minPressure,
|
||||
units::pounds_per_square_inch_t maxPressure) {
|
||||
if (minPressure >= maxPressure) {
|
||||
throw FRC_MakeError(err::InvalidParameter, "{}",
|
||||
throw FRC_MakeError(err::InvalidParameter,
|
||||
"maxPressure must be greater than minPresure");
|
||||
}
|
||||
if (minPressure < 0_psi || minPressure > 120_psi) {
|
||||
@@ -173,7 +173,7 @@ void PneumaticHub::EnableCompressorHybrid(
|
||||
units::pounds_per_square_inch_t minPressure,
|
||||
units::pounds_per_square_inch_t maxPressure) {
|
||||
if (minPressure >= maxPressure) {
|
||||
throw FRC_MakeError(err::InvalidParameter, "{}",
|
||||
throw FRC_MakeError(err::InvalidParameter,
|
||||
"maxPressure must be greater than minPresure");
|
||||
}
|
||||
if (minPressure < 0_psi || minPressure > 120_psi) {
|
||||
|
||||
@@ -15,161 +15,161 @@ using namespace frc;
|
||||
int RobotController::GetFPGAVersion() {
|
||||
int32_t status = 0;
|
||||
int version = HAL_GetFPGAVersion(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFPGAVersion");
|
||||
FRC_CheckErrorStatus(status, "GetFPGAVersion");
|
||||
return version;
|
||||
}
|
||||
|
||||
int64_t RobotController::GetFPGARevision() {
|
||||
int32_t status = 0;
|
||||
int64_t revision = HAL_GetFPGARevision(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFPGARevision");
|
||||
FRC_CheckErrorStatus(status, "GetFPGARevision");
|
||||
return revision;
|
||||
}
|
||||
|
||||
uint64_t RobotController::GetFPGATime() {
|
||||
int32_t status = 0;
|
||||
uint64_t time = HAL_GetFPGATime(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFPGATime");
|
||||
FRC_CheckErrorStatus(status, "GetFPGATime");
|
||||
return time;
|
||||
}
|
||||
|
||||
bool RobotController::GetUserButton() {
|
||||
int32_t status = 0;
|
||||
bool value = HAL_GetFPGAButton(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetUserButton");
|
||||
FRC_CheckErrorStatus(status, "GetUserButton");
|
||||
return value;
|
||||
}
|
||||
|
||||
units::volt_t RobotController::GetBatteryVoltage() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetVinVoltage(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetBatteryVoltage");
|
||||
FRC_CheckErrorStatus(status, "GetBatteryVoltage");
|
||||
return units::volt_t{retVal};
|
||||
}
|
||||
|
||||
bool RobotController::IsSysActive() {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetSystemActive(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "IsSysActive");
|
||||
FRC_CheckErrorStatus(status, "IsSysActive");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
bool RobotController::IsBrownedOut() {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetBrownedOut(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "IsBrownedOut");
|
||||
FRC_CheckErrorStatus(status, "IsBrownedOut");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetInputVoltage() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetVinVoltage(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetInputVoltage");
|
||||
FRC_CheckErrorStatus(status, "GetInputVoltage");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetInputCurrent() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetVinCurrent(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetInputCurrent");
|
||||
FRC_CheckErrorStatus(status, "GetInputCurrent");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetVoltage3V3() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetUserVoltage3V3(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetVoltage3V3");
|
||||
FRC_CheckErrorStatus(status, "GetVoltage3V3");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetCurrent3V3() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetUserCurrent3V3(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetCurrent3V3");
|
||||
FRC_CheckErrorStatus(status, "GetCurrent3V3");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
bool RobotController::GetEnabled3V3() {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetUserActive3V3(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetEnabled3V3");
|
||||
FRC_CheckErrorStatus(status, "GetEnabled3V3");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
int RobotController::GetFaultCount3V3() {
|
||||
int32_t status = 0;
|
||||
int retVal = HAL_GetUserCurrentFaults3V3(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFaultCount3V3");
|
||||
FRC_CheckErrorStatus(status, "GetFaultCount3V3");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetVoltage5V() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetUserVoltage5V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetVoltage5V");
|
||||
FRC_CheckErrorStatus(status, "GetVoltage5V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetCurrent5V() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetUserCurrent5V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetCurrent5V");
|
||||
FRC_CheckErrorStatus(status, "GetCurrent5V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
bool RobotController::GetEnabled5V() {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetUserActive5V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetEnabled5V");
|
||||
FRC_CheckErrorStatus(status, "GetEnabled5V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
int RobotController::GetFaultCount5V() {
|
||||
int32_t status = 0;
|
||||
int retVal = HAL_GetUserCurrentFaults5V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFaultCount5V");
|
||||
FRC_CheckErrorStatus(status, "GetFaultCount5V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetVoltage6V() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetUserVoltage6V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetVoltage6V");
|
||||
FRC_CheckErrorStatus(status, "GetVoltage6V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
double RobotController::GetCurrent6V() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetUserCurrent6V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetCurrent6V");
|
||||
FRC_CheckErrorStatus(status, "GetCurrent6V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
bool RobotController::GetEnabled6V() {
|
||||
int32_t status = 0;
|
||||
bool retVal = HAL_GetUserActive6V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetEnabled6V");
|
||||
FRC_CheckErrorStatus(status, "GetEnabled6V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
int RobotController::GetFaultCount6V() {
|
||||
int32_t status = 0;
|
||||
int retVal = HAL_GetUserCurrentFaults6V(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetFaultCount6V");
|
||||
FRC_CheckErrorStatus(status, "GetFaultCount6V");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
units::volt_t RobotController::GetBrownoutVoltage() {
|
||||
int32_t status = 0;
|
||||
double retVal = HAL_GetBrownoutVoltage(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetBrownoutVoltage");
|
||||
FRC_CheckErrorStatus(status, "GetBrownoutVoltage");
|
||||
return units::volt_t{retVal};
|
||||
}
|
||||
|
||||
void RobotController::SetBrownoutVoltage(units::volt_t brownoutVoltage) {
|
||||
int32_t status = 0;
|
||||
HAL_SetBrownoutVoltage(brownoutVoltage.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetBrownoutVoltage");
|
||||
FRC_CheckErrorStatus(status, "SetBrownoutVoltage");
|
||||
}
|
||||
|
||||
CANStatus RobotController::GetCANStatus() {
|
||||
@@ -181,7 +181,7 @@ CANStatus RobotController::GetCANStatus() {
|
||||
uint32_t transmitErrorCount = 0;
|
||||
HAL_CAN_GetCANStatus(&percentBusUtilization, &busOffCount, &txFullCount,
|
||||
&receiveErrorCount, &transmitErrorCount, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetCANStatus");
|
||||
FRC_CheckErrorStatus(status, "GetCANStatus");
|
||||
return {percentBusUtilization, static_cast<int>(busOffCount),
|
||||
static_cast<int>(txFullCount), static_cast<int>(receiveErrorCount),
|
||||
static_cast<int>(transmitErrorCount)};
|
||||
|
||||
@@ -77,7 +77,7 @@ SerialPort::SerialPort(int baudRate, std::string_view portName, Port port,
|
||||
SerialPort::~SerialPort() {
|
||||
int32_t status = 0;
|
||||
HAL_CloseSerial(m_portHandle, &status);
|
||||
FRC_ReportError(status, "{}", "CloseSerial");
|
||||
FRC_ReportError(status, "CloseSerial");
|
||||
}
|
||||
|
||||
void SerialPort::SetFlowControl(SerialPort::FlowControl flowControl) {
|
||||
@@ -96,20 +96,20 @@ void SerialPort::EnableTermination(char terminator) {
|
||||
void SerialPort::DisableTermination() {
|
||||
int32_t status = 0;
|
||||
HAL_DisableSerialTermination(m_portHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "DisableTermination");
|
||||
FRC_CheckErrorStatus(status, "DisableTermination");
|
||||
}
|
||||
|
||||
int SerialPort::GetBytesReceived() {
|
||||
int32_t status = 0;
|
||||
int retVal = HAL_GetSerialBytesReceived(m_portHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetBytesReceived");
|
||||
FRC_CheckErrorStatus(status, "GetBytesReceived");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
int SerialPort::Read(char* buffer, int count) {
|
||||
int32_t status = 0;
|
||||
int retVal = HAL_ReadSerial(m_portHandle, buffer, count, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Read");
|
||||
FRC_CheckErrorStatus(status, "Read");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
@@ -121,14 +121,14 @@ int SerialPort::Write(std::string_view buffer) {
|
||||
int32_t status = 0;
|
||||
int retVal =
|
||||
HAL_WriteSerial(m_portHandle, buffer.data(), buffer.size(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Write");
|
||||
FRC_CheckErrorStatus(status, "Write");
|
||||
return retVal;
|
||||
}
|
||||
|
||||
void SerialPort::SetTimeout(units::second_t timeout) {
|
||||
int32_t status = 0;
|
||||
HAL_SetSerialTimeout(m_portHandle, timeout.value(), &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetTimeout");
|
||||
FRC_CheckErrorStatus(status, "SetTimeout");
|
||||
}
|
||||
|
||||
void SerialPort::SetReadBufferSize(int size) {
|
||||
@@ -152,11 +152,11 @@ void SerialPort::SetWriteBufferMode(SerialPort::WriteBufferMode mode) {
|
||||
void SerialPort::Flush() {
|
||||
int32_t status = 0;
|
||||
HAL_FlushSerial(m_portHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Flush");
|
||||
FRC_CheckErrorStatus(status, "Flush");
|
||||
}
|
||||
|
||||
void SerialPort::Reset() {
|
||||
int32_t status = 0;
|
||||
HAL_ClearSerial(m_portHandle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Reset");
|
||||
FRC_CheckErrorStatus(status, "Reset");
|
||||
}
|
||||
|
||||
@@ -21,7 +21,7 @@ SynchronousInterrupt::SynchronousInterrupt(DigitalSource& source)
|
||||
SynchronousInterrupt::SynchronousInterrupt(DigitalSource* source)
|
||||
: m_source{source, wpi::NullDeleter<DigitalSource>()} {
|
||||
if (m_source == nullptr) {
|
||||
FRC_CheckErrorStatus(frc::err::NullParameter, "{}", "Source is null");
|
||||
FRC_CheckErrorStatus(frc::err::NullParameter, "Source is null");
|
||||
} else {
|
||||
InitSynchronousInterrupt();
|
||||
}
|
||||
@@ -30,7 +30,7 @@ SynchronousInterrupt::SynchronousInterrupt(
|
||||
std::shared_ptr<DigitalSource> source)
|
||||
: m_source{std::move(source)} {
|
||||
if (m_source == nullptr) {
|
||||
FRC_CheckErrorStatus(frc::err::NullParameter, "{}", "Source is null");
|
||||
FRC_CheckErrorStatus(frc::err::NullParameter, "Source is null");
|
||||
} else {
|
||||
InitSynchronousInterrupt();
|
||||
}
|
||||
@@ -39,14 +39,14 @@ SynchronousInterrupt::SynchronousInterrupt(
|
||||
void SynchronousInterrupt::InitSynchronousInterrupt() {
|
||||
int32_t status = 0;
|
||||
m_handle = HAL_InitializeInterrupts(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt failed to initialize");
|
||||
FRC_CheckErrorStatus(status, "Interrupt failed to initialize");
|
||||
HAL_RequestInterrupts(m_handle, m_source->GetPortHandleForRouting(),
|
||||
static_cast<HAL_AnalogTriggerType>(
|
||||
m_source->GetAnalogTriggerTypeForRouting()),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt request failed");
|
||||
FRC_CheckErrorStatus(status, "Interrupt request failed");
|
||||
HAL_SetInterruptUpSourceEdge(m_handle, true, false, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt setting up source edge failed");
|
||||
FRC_CheckErrorStatus(status, "Interrupt setting up source edge failed");
|
||||
}
|
||||
|
||||
SynchronousInterrupt::~SynchronousInterrupt() {
|
||||
@@ -79,19 +79,19 @@ void SynchronousInterrupt::SetInterruptEdges(bool risingEdge,
|
||||
bool fallingEdge) {
|
||||
int32_t status = 0;
|
||||
HAL_SetInterruptUpSourceEdge(m_handle, risingEdge, fallingEdge, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt setting edges failed");
|
||||
FRC_CheckErrorStatus(status, "Interrupt setting edges failed");
|
||||
}
|
||||
|
||||
void SynchronousInterrupt::WakeupWaitingInterrupt() {
|
||||
int32_t status = 0;
|
||||
HAL_ReleaseWaitingInterrupt(m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt wakeup failed");
|
||||
FRC_CheckErrorStatus(status, "Interrupt wakeup failed");
|
||||
}
|
||||
|
||||
units::second_t SynchronousInterrupt::GetRisingTimestamp() {
|
||||
int32_t status = 0;
|
||||
auto ts = HAL_ReadInterruptRisingTimestamp(m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt rising timestamp failed");
|
||||
FRC_CheckErrorStatus(status, "Interrupt rising timestamp failed");
|
||||
units::microsecond_t ms{static_cast<double>(ts)};
|
||||
return ms;
|
||||
}
|
||||
@@ -99,7 +99,7 @@ units::second_t SynchronousInterrupt::GetRisingTimestamp() {
|
||||
units::second_t SynchronousInterrupt::GetFallingTimestamp() {
|
||||
int32_t status = 0;
|
||||
auto ts = HAL_ReadInterruptFallingTimestamp(m_handle, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "Interrupt falling timestamp failed");
|
||||
FRC_CheckErrorStatus(status, "Interrupt falling timestamp failed");
|
||||
units::microsecond_t ms{static_cast<double>(ts)};
|
||||
return ms;
|
||||
}
|
||||
|
||||
@@ -16,7 +16,7 @@ int GetThreadPriority(std::thread& thread, bool* isRealTime) {
|
||||
HAL_Bool rt = false;
|
||||
auto native = thread.native_handle();
|
||||
auto ret = HAL_GetThreadPriority(&native, &rt, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetThreadPriority");
|
||||
FRC_CheckErrorStatus(status, "GetThreadPriority");
|
||||
*isRealTime = rt;
|
||||
return ret;
|
||||
}
|
||||
@@ -25,7 +25,7 @@ int GetCurrentThreadPriority(bool* isRealTime) {
|
||||
int32_t status = 0;
|
||||
HAL_Bool rt = false;
|
||||
auto ret = HAL_GetCurrentThreadPriority(&rt, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "GetCurrentThreadPriority");
|
||||
FRC_CheckErrorStatus(status, "GetCurrentThreadPriority");
|
||||
*isRealTime = rt;
|
||||
return ret;
|
||||
}
|
||||
@@ -34,14 +34,14 @@ bool SetThreadPriority(std::thread& thread, bool realTime, int priority) {
|
||||
int32_t status = 0;
|
||||
auto native = thread.native_handle();
|
||||
auto ret = HAL_SetThreadPriority(&native, realTime, priority, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetThreadPriority");
|
||||
FRC_CheckErrorStatus(status, "SetThreadPriority");
|
||||
return ret;
|
||||
}
|
||||
|
||||
bool SetCurrentThreadPriority(bool realTime, int priority) {
|
||||
int32_t status = 0;
|
||||
auto ret = HAL_SetCurrentThreadPriority(realTime, priority, &status);
|
||||
FRC_CheckErrorStatus(status, "{}", "SetCurrentThreadPriority");
|
||||
FRC_CheckErrorStatus(status, "SetCurrentThreadPriority");
|
||||
return ret;
|
||||
}
|
||||
|
||||
|
||||
@@ -40,7 +40,7 @@ void TimedRobot::StartCompetition() {
|
||||
HAL_UpdateNotifierAlarm(
|
||||
m_notifier, static_cast<uint64_t>(callback.expirationTime * 1e6),
|
||||
&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "UpdateNotifierAlarm");
|
||||
FRC_CheckErrorStatus(status, "UpdateNotifierAlarm");
|
||||
|
||||
uint64_t curTime = HAL_WaitForNotifierAlarm(m_notifier, &status);
|
||||
if (curTime == 0 || status != 0) {
|
||||
@@ -76,7 +76,7 @@ TimedRobot::TimedRobot(units::second_t period) : IterativeRobotBase(period) {
|
||||
|
||||
int32_t status = 0;
|
||||
m_notifier = HAL_InitializeNotifier(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "InitializeNotifier");
|
||||
FRC_CheckErrorStatus(status, "InitializeNotifier");
|
||||
HAL_SetNotifierName(m_notifier, "TimedRobot", &status);
|
||||
|
||||
HAL_Report(HALUsageReporting::kResourceType_Framework,
|
||||
@@ -87,7 +87,7 @@ TimedRobot::~TimedRobot() {
|
||||
int32_t status = 0;
|
||||
|
||||
HAL_StopNotifier(m_notifier, &status);
|
||||
FRC_ReportError(status, "{}", "StopNotifier");
|
||||
FRC_ReportError(status, "StopNotifier");
|
||||
|
||||
HAL_CleanNotifier(m_notifier, &status);
|
||||
}
|
||||
|
||||
@@ -39,10 +39,10 @@ Ultrasonic::Ultrasonic(DigitalOutput* pingChannel, DigitalInput* echoChannel)
|
||||
m_echoChannel(echoChannel, wpi::NullDeleter<DigitalInput>()),
|
||||
m_counter(m_echoChannel) {
|
||||
if (!pingChannel) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "pingChannel");
|
||||
throw FRC_MakeError(err::NullParameter, "pingChannel");
|
||||
}
|
||||
if (!echoChannel) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "echoChannel");
|
||||
throw FRC_MakeError(err::NullParameter, "echoChannel");
|
||||
}
|
||||
Initialize();
|
||||
}
|
||||
@@ -86,7 +86,7 @@ int Ultrasonic::GetEchoChannel() const {
|
||||
|
||||
void Ultrasonic::Ping() {
|
||||
if (m_automaticEnabled) {
|
||||
throw FRC_MakeError(err::IncompatibleMode, "{}",
|
||||
throw FRC_MakeError(err::IncompatibleMode,
|
||||
"cannot call Ping() in automatic mode");
|
||||
}
|
||||
|
||||
|
||||
@@ -47,7 +47,7 @@ class Watchdog::Impl {
|
||||
Watchdog::Impl::Impl() {
|
||||
int32_t status = 0;
|
||||
m_notifier = HAL_InitializeNotifier(&status);
|
||||
FRC_CheckErrorStatus(status, "{}", "starting watchdog notifier");
|
||||
FRC_CheckErrorStatus(status, "starting watchdog notifier");
|
||||
HAL_SetNotifierName(m_notifier, "Watchdog", &status);
|
||||
|
||||
m_thread = std::thread([=, this] { Main(); });
|
||||
@@ -58,7 +58,7 @@ Watchdog::Impl::~Impl() {
|
||||
// atomically set handle to 0, then clean
|
||||
HAL_NotifierHandle handle = m_notifier.exchange(0);
|
||||
HAL_StopNotifier(handle, &status);
|
||||
FRC_ReportError(status, "{}", "stopping watchdog notifier");
|
||||
FRC_ReportError(status, "stopping watchdog notifier");
|
||||
|
||||
// Join the thread to ensure the handler has exited.
|
||||
if (m_thread.joinable()) {
|
||||
@@ -84,7 +84,7 @@ void Watchdog::Impl::UpdateAlarm() {
|
||||
1e6),
|
||||
&status);
|
||||
}
|
||||
FRC_CheckErrorStatus(status, "{}", "updating watchdog notifier alarm");
|
||||
FRC_CheckErrorStatus(status, "updating watchdog notifier alarm");
|
||||
}
|
||||
|
||||
void Watchdog::Impl::Main() {
|
||||
|
||||
@@ -24,10 +24,10 @@ ExternalDirectionCounter::ExternalDirectionCounter(
|
||||
std::shared_ptr<DigitalSource> countSource,
|
||||
std::shared_ptr<DigitalSource> directionSource) {
|
||||
if (countSource == nullptr) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "countSource");
|
||||
throw FRC_MakeError(err::NullParameter, "countSource");
|
||||
}
|
||||
if (directionSource == nullptr) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "directionSource");
|
||||
throw FRC_MakeError(err::NullParameter, "directionSource");
|
||||
}
|
||||
|
||||
m_countSource = countSource;
|
||||
|
||||
@@ -19,7 +19,7 @@ Tachometer::Tachometer(DigitalSource& source)
|
||||
: Tachometer({&source, wpi::NullDeleter<DigitalSource>()}) {}
|
||||
Tachometer::Tachometer(std::shared_ptr<DigitalSource> source) {
|
||||
if (source == nullptr) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "source");
|
||||
throw FRC_MakeError(err::NullParameter, "source");
|
||||
}
|
||||
|
||||
m_source = source;
|
||||
|
||||
@@ -39,8 +39,7 @@ void RecordingController::AddEventMarker(
|
||||
std::string_view name, std::string_view description,
|
||||
ShuffleboardEventImportance importance) {
|
||||
if (name.empty()) {
|
||||
FRC_ReportError(err::Error, "{}",
|
||||
"Shuffleboard event name was not specified");
|
||||
FRC_ReportError(err::Error, "Shuffleboard event name was not specified");
|
||||
return;
|
||||
}
|
||||
m_eventsTable->GetSubTable(name)->GetEntry("Info").SetStringArray(
|
||||
|
||||
@@ -70,7 +70,7 @@ ComplexWidget& ShuffleboardContainer::Add(std::string_view title,
|
||||
ComplexWidget& ShuffleboardContainer::Add(wpi::Sendable& sendable) {
|
||||
auto name = wpi::SendableRegistry::GetName(&sendable);
|
||||
if (name.empty()) {
|
||||
FRC_ReportError(err::Error, "{}", "Sendable must have a name");
|
||||
FRC_ReportError(err::Error, "Sendable must have a name");
|
||||
}
|
||||
return Add(name, sendable);
|
||||
}
|
||||
|
||||
@@ -76,7 +76,7 @@ nt::NetworkTableEntry SmartDashboard::GetEntry(std::string_view key) {
|
||||
|
||||
void SmartDashboard::PutData(std::string_view key, wpi::Sendable* data) {
|
||||
if (!data) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "value");
|
||||
throw FRC_MakeError(err::NullParameter, "value");
|
||||
}
|
||||
auto& inst = GetInstance();
|
||||
std::scoped_lock lock(inst.tablesToDataMutex);
|
||||
@@ -96,7 +96,7 @@ void SmartDashboard::PutData(std::string_view key, wpi::Sendable* data) {
|
||||
|
||||
void SmartDashboard::PutData(wpi::Sendable* value) {
|
||||
if (!value) {
|
||||
throw FRC_MakeError(err::NullParameter, "{}", "value");
|
||||
throw FRC_MakeError(err::NullParameter, "value");
|
||||
}
|
||||
auto name = wpi::SendableRegistry::GetName(value);
|
||||
if (!name.empty()) {
|
||||
|
||||
@@ -45,7 +45,7 @@ int frc::RunHALInitialization() {
|
||||
HALUsageReporting::kLanguage_CPlusPlus, 0, GetWPILibVersion());
|
||||
|
||||
if (!frc::Notifier::SetHALThreadPriority(true, 40)) {
|
||||
FRC_ReportError(warn::Warning, "{}",
|
||||
FRC_ReportError(warn::Warning,
|
||||
"Setting HAL Notifier RT priority to 40 failed\n");
|
||||
}
|
||||
|
||||
|
||||
@@ -74,9 +74,10 @@ void ReportErrorV(int32_t status, const char* fileName, int lineNumber,
|
||||
* @param[in] format error message format
|
||||
* @param[in] args error message format args
|
||||
*/
|
||||
template <typename S, typename... Args>
|
||||
template <typename... Args>
|
||||
inline void ReportError(int32_t status, const char* fileName, int lineNumber,
|
||||
const char* funcName, const S& format, Args&&... args) {
|
||||
const char* funcName, fmt::string_view format,
|
||||
Args&&... args) {
|
||||
ReportErrorV(status, fileName, lineNumber, funcName, format,
|
||||
fmt::make_format_args(args...));
|
||||
}
|
||||
@@ -99,12 +100,10 @@ inline void ReportError(int32_t status, const char* fileName, int lineNumber,
|
||||
fmt::string_view format,
|
||||
fmt::format_args args);
|
||||
|
||||
template <typename S, typename... Args>
|
||||
[[nodiscard]] inline RuntimeError MakeError(int32_t status,
|
||||
const char* fileName,
|
||||
int lineNumber,
|
||||
const char* funcName,
|
||||
const S& format, Args&&... args) {
|
||||
template <typename... Args>
|
||||
[[nodiscard]] inline RuntimeError MakeError(
|
||||
int32_t status, const char* fileName, int lineNumber, const char* funcName,
|
||||
fmt::string_view format, Args&&... args) {
|
||||
return MakeErrorV(status, fileName, lineNumber, funcName, format,
|
||||
fmt::make_format_args(args...));
|
||||
}
|
||||
@@ -122,18 +121,24 @@ namespace warn {
|
||||
} // namespace warn
|
||||
} // namespace frc
|
||||
|
||||
// C++20 relaxed the number of arguments to variadics, but Apple Clang's
|
||||
// warnings haven't caught up yet: https://stackoverflow.com/a/67996331
|
||||
#ifdef __clang__
|
||||
#pragma clang diagnostic ignored "-Wgnu-zero-variadic-macro-arguments"
|
||||
#endif
|
||||
|
||||
/**
|
||||
* Reports an error to the driver station (using HAL_SendError).
|
||||
*
|
||||
* @param[out] status error code
|
||||
* @param[in] format error message format
|
||||
*/
|
||||
#define FRC_ReportError(status, format, ...) \
|
||||
do { \
|
||||
if ((status) != 0) { \
|
||||
::frc::ReportError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format), __VA_ARGS__); \
|
||||
} \
|
||||
#define FRC_ReportError(status, format, ...) \
|
||||
do { \
|
||||
if ((status) != 0) { \
|
||||
::frc::ReportError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
/**
|
||||
@@ -146,7 +151,7 @@ namespace warn {
|
||||
*/
|
||||
#define FRC_MakeError(status, format, ...) \
|
||||
::frc::MakeError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format), __VA_ARGS__)
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
/**
|
||||
* Checks a status code and depending on its value, either throws a
|
||||
@@ -155,23 +160,24 @@ namespace warn {
|
||||
* @param[out] status error code
|
||||
* @param[in] format error message format
|
||||
*/
|
||||
#define FRC_CheckErrorStatus(status, format, ...) \
|
||||
do { \
|
||||
if ((status) < 0) { \
|
||||
throw ::frc::MakeError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format), __VA_ARGS__); \
|
||||
} else if ((status) > 0) { \
|
||||
::frc::ReportError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format), __VA_ARGS__); \
|
||||
} \
|
||||
#define FRC_CheckErrorStatus(status, format, ...) \
|
||||
do { \
|
||||
if ((status) < 0) { \
|
||||
throw ::frc::MakeError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__); \
|
||||
} else if ((status) > 0) { \
|
||||
::frc::ReportError(status, __FILE__, __LINE__, __FUNCTION__, \
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define FRC_AssertMessage(condition, format, ...) \
|
||||
do { \
|
||||
if (!(condition)) { \
|
||||
throw ::frc::MakeError(err::AssertionFailure, __FILE__, __LINE__, \
|
||||
__FUNCTION__, FMT_STRING(format), __VA_ARGS__); \
|
||||
__FUNCTION__, \
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__); \
|
||||
} \
|
||||
} while (0)
|
||||
|
||||
#define FRC_Assert(condition) FRC_AssertMessage(condition, "{}", #condition)
|
||||
#define FRC_Assert(condition) FRC_AssertMessage(condition, #condition)
|
||||
|
||||
@@ -34,7 +34,7 @@ void RunRobot(wpi::mutex& m, Robot** robot) {
|
||||
} catch (const frc::RuntimeError& e) {
|
||||
e.Report();
|
||||
FRC_ReportError(
|
||||
err::Error, "{}",
|
||||
err::Error,
|
||||
"The robot program quit unexpectedly."
|
||||
" This is usually due to a code error.\n"
|
||||
" The above stacktrace can help determine where the error occurred.\n"
|
||||
|
||||
@@ -24,7 +24,7 @@ PIDController::PIDController(double Kp, double Ki, double Kd,
|
||||
period.value());
|
||||
m_period = 20_ms;
|
||||
wpi::math::MathSharedStore::ReportWarning(
|
||||
"{}", "Controller period defaulted to 20ms.");
|
||||
"Controller period defaulted to 20ms.");
|
||||
}
|
||||
static int instances = 0;
|
||||
instances++;
|
||||
|
||||
@@ -22,7 +22,7 @@ DsClient::DsClient(wpi::uv::Loop& loop, wpi::Logger& logger,
|
||||
m_tcp{uv::Tcp::Create(loop)},
|
||||
m_timer{uv::Timer::Create(loop)} {
|
||||
m_tcp->end.connect([this] {
|
||||
WPI_DEBUG4(m_logger, "{}", "DS connection closed");
|
||||
WPI_DEBUG4(m_logger, "DS connection closed");
|
||||
clearIp();
|
||||
// try to connect again
|
||||
m_tcp->Reuse([this] { m_timer->Start(kReconnectTime); });
|
||||
@@ -56,7 +56,7 @@ void DsClient::Connect() {
|
||||
m_tcp->Reuse([this] { m_timer->Start(kReconnectTime); });
|
||||
};
|
||||
|
||||
WPI_DEBUG4(m_logger, "{}", "Starting DS connection attempt");
|
||||
WPI_DEBUG4(m_logger, "Starting DS connection attempt");
|
||||
m_tcp->Connect("127.0.0.1", 1742, connreq);
|
||||
}
|
||||
|
||||
|
||||
@@ -26,7 +26,7 @@ ParallelTcpConnector::ParallelTcpConnector(
|
||||
m_reconnectTimer{uv::Timer::Create(loop)} {
|
||||
m_reconnectTimer->timeout.connect([this] {
|
||||
if (!IsConnected()) {
|
||||
WPI_DEBUG1(m_logger, "{}", "timed out, reconnecting");
|
||||
WPI_DEBUG1(m_logger, "timed out, reconnecting");
|
||||
Connect();
|
||||
}
|
||||
});
|
||||
@@ -70,7 +70,7 @@ void ParallelTcpConnector::Connect() {
|
||||
CancelAll();
|
||||
m_reconnectTimer->Start(m_reconnectRate);
|
||||
|
||||
WPI_DEBUG3(m_logger, "{}", "starting new connection attempts");
|
||||
WPI_DEBUG3(m_logger, "starting new connection attempts");
|
||||
|
||||
// kick off parallel lookups
|
||||
for (auto&& server : m_servers) {
|
||||
@@ -154,7 +154,7 @@ void ParallelTcpConnector::Connect() {
|
||||
}
|
||||
|
||||
void ParallelTcpConnector::CancelAll(wpi::uv::Tcp* except) {
|
||||
WPI_DEBUG4(m_logger, "{}", "canceling previous attempts");
|
||||
WPI_DEBUG4(m_logger, "canceling previous attempts");
|
||||
for (auto&& resolverWeak : m_resolvers) {
|
||||
if (auto resolver = resolverWeak.lock()) {
|
||||
WPI_DEBUG4(m_logger, "canceling GetAddrInfo({})",
|
||||
|
||||
@@ -74,7 +74,7 @@ int UDPClient::start(int port) {
|
||||
m_lsd = socket(AF_INET, SOCK_DGRAM, 0);
|
||||
|
||||
if (m_lsd < 0) {
|
||||
WPI_ERROR(m_logger, "{}", "could not create socket");
|
||||
WPI_ERROR(m_logger, "could not create socket");
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -142,7 +142,7 @@ int UDPClient::send(std::span<const uint8_t> data, std::string_view server,
|
||||
addr.sin_family = AF_INET;
|
||||
SmallString<128> remoteAddr{server};
|
||||
if (remoteAddr.empty()) {
|
||||
WPI_ERROR(m_logger, "{}", "server must be passed");
|
||||
WPI_ERROR(m_logger, "server must be passed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -171,7 +171,7 @@ int UDPClient::send(std::string_view data, std::string_view server, int port) {
|
||||
addr.sin_family = AF_INET;
|
||||
SmallString<128> remoteAddr{server};
|
||||
if (remoteAddr.empty()) {
|
||||
WPI_ERROR(m_logger, "{}", "server must be passed");
|
||||
WPI_ERROR(m_logger, "server must be passed");
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -243,7 +243,7 @@ int UDPClient::set_timeout(double timeout) {
|
||||
int ret = setsockopt(m_lsd, SOL_SOCKET, SO_RCVTIMEO,
|
||||
reinterpret_cast<char*>(&tv), sizeof(tv));
|
||||
if (ret < 0) {
|
||||
WPI_ERROR(m_logger, "{}", "set timeout failed");
|
||||
WPI_ERROR(m_logger, "set timeout failed");
|
||||
}
|
||||
return ret;
|
||||
}
|
||||
|
||||
@@ -80,7 +80,7 @@ int TCPAcceptor::start() {
|
||||
|
||||
m_lsd = socket(PF_INET, SOCK_STREAM, 0);
|
||||
if (m_lsd < 0) {
|
||||
WPI_ERROR(m_logger, "{}", "could not create socket");
|
||||
WPI_ERROR(m_logger, "could not create socket");
|
||||
return -1;
|
||||
}
|
||||
struct sockaddr_in address;
|
||||
|
||||
@@ -106,7 +106,7 @@ std::unique_ptr<NetworkStream> TCPConnector::connect(const char* server,
|
||||
if (timeout == 0) {
|
||||
int sd = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sd < 0) {
|
||||
WPI_ERROR(logger, "{}", "could not create socket");
|
||||
WPI_ERROR(logger, "could not create socket");
|
||||
return nullptr;
|
||||
}
|
||||
if (::connect(sd, reinterpret_cast<struct sockaddr*>(&address),
|
||||
@@ -128,7 +128,7 @@ std::unique_ptr<NetworkStream> TCPConnector::connect(const char* server,
|
||||
socklen_t len;
|
||||
int result = -1, valopt, sd = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (sd < 0) {
|
||||
WPI_ERROR(logger, "{}", "could not create socket");
|
||||
WPI_ERROR(logger, "could not create socket");
|
||||
return nullptr;
|
||||
}
|
||||
|
||||
|
||||
@@ -276,7 +276,7 @@ void DataLog::WriterThreadMain(std::string_view dir) {
|
||||
}
|
||||
|
||||
if (f == fs::kInvalidFile) {
|
||||
WPI_ERROR(m_msglog, "{}", "Could not open log file, no log being saved");
|
||||
WPI_ERROR(m_msglog, "Could not open log file, no log being saved");
|
||||
} else {
|
||||
WPI_INFO(m_msglog, "Logging to '{}'", (dirPath / filename).string());
|
||||
}
|
||||
|
||||
@@ -45,9 +45,9 @@ class Logger {
|
||||
void LogV(unsigned int level, const char* file, unsigned int line,
|
||||
fmt::string_view format, fmt::format_args args);
|
||||
|
||||
template <typename S, typename... Args>
|
||||
template <typename... Args>
|
||||
void Log(unsigned int level, const char* file, unsigned int line,
|
||||
const S& format, Args&&... args) {
|
||||
fmt::string_view format, Args&&... args) {
|
||||
if (m_func && level >= m_min_level) {
|
||||
LogV(level, file, line, format, fmt::make_format_args(args...));
|
||||
}
|
||||
@@ -60,18 +60,25 @@ class Logger {
|
||||
unsigned int m_min_level = 20;
|
||||
};
|
||||
|
||||
#define WPI_LOG(logger_inst, level, format, ...) \
|
||||
if ((logger_inst).HasLogger() && level >= (logger_inst).min_level()) { \
|
||||
(logger_inst) \
|
||||
.Log(level, __FILE__, __LINE__, FMT_STRING(format), __VA_ARGS__); \
|
||||
// C++20 relaxed the number of arguments to variadics, but Apple Clang's
|
||||
// warnings haven't caught up yet: https://stackoverflow.com/a/67996331
|
||||
#ifdef __clang__
|
||||
#pragma clang diagnostic ignored "-Wgnu-zero-variadic-macro-arguments"
|
||||
#endif
|
||||
|
||||
#define WPI_LOG(logger_inst, level, format, ...) \
|
||||
if ((logger_inst).HasLogger() && level >= (logger_inst).min_level()) { \
|
||||
(logger_inst) \
|
||||
.Log(level, __FILE__, __LINE__, \
|
||||
FMT_STRING(format) __VA_OPT__(, ) __VA_ARGS__); \
|
||||
}
|
||||
|
||||
#define WPI_ERROR(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_ERROR, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_ERROR, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WPI_WARNING(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_WARNING, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_WARNING, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WPI_INFO(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_INFO, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_INFO, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
|
||||
#ifdef NDEBUG
|
||||
#define WPI_DEBUG(inst, format, ...) \
|
||||
@@ -91,15 +98,15 @@ class Logger {
|
||||
} while (0)
|
||||
#else
|
||||
#define WPI_DEBUG(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WPI_DEBUG1(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG1, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG1, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WPI_DEBUG2(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG2, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG2, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WPI_DEBUG3(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG3, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG3, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#define WPI_DEBUG4(inst, format, ...) \
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG4, format, __VA_ARGS__)
|
||||
WPI_LOG(inst, ::wpi::WPI_LOG_DEBUG4, format __VA_OPT__(, ) __VA_ARGS__)
|
||||
#endif
|
||||
|
||||
} // namespace wpi
|
||||
|
||||
@@ -163,7 +163,7 @@ void wpi::wpi_unreachable_internal(const char *msg, const char *file,
|
||||
std::fputs("UNREACHABLE executed", stderr);
|
||||
if (file)
|
||||
fmt::print(stderr, " at {}:{}", file, line);
|
||||
fmt::print(stderr, "{}", "!\n");
|
||||
fmt::print(stderr, "!\n");
|
||||
abort();
|
||||
#ifdef LLVM_BUILTIN_UNREACHABLE
|
||||
// Windows systems and possibly others don't declare abort() to be noreturn,
|
||||
|
||||
Reference in New Issue
Block a user