Prefix LOG and CHECK with AOS_
This prepares us for introducing glog more widely and transitioning
things over where they make sense.
Change-Id: Ic6c208882407bc2153fe875ffc736d66f5c8ade5
diff --git a/y2019/actors/autonomous_actor.cc b/y2019/actors/autonomous_actor.cc
index 8408789..a5a71aa 100644
--- a/y2019/actors/autonomous_actor.cc
+++ b/y2019/actors/autonomous_actor.cc
@@ -35,7 +35,7 @@
".y2019.control_loops.superstructure.superstructure_queue.status")) {}
bool AutonomousActor::WaitForDriveXGreater(double x) {
- LOG(INFO, "Waiting until x > %f\n", x);
+ AOS_LOG(INFO, "Waiting until x > %f\n", x);
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
event_loop()->monotonic_now(),
::std::chrono::milliseconds(5) / 2);
@@ -47,14 +47,14 @@
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
if (drivetrain_status_fetcher_->x > x) {
- LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
+ AOS_LOG(INFO, "X at %f\n", drivetrain_status_fetcher_->x);
return true;
}
}
}
bool AutonomousActor::WaitForDriveYCloseToZero(double y) {
- LOG(INFO, "Waiting until |y| < %f\n", y);
+ AOS_LOG(INFO, "Waiting until |y| < %f\n", y);
::aos::time::PhasedLoop phased_loop(::std::chrono::milliseconds(5),
event_loop()->monotonic_now(),
::std::chrono::milliseconds(5) / 2);
@@ -66,7 +66,7 @@
phased_loop.SleepUntilNext();
drivetrain_status_fetcher_.Fetch();
if (::std::abs(drivetrain_status_fetcher_->y) < y) {
- LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
+ AOS_LOG(INFO, "Y at %f\n", drivetrain_status_fetcher_->y);
return true;
}
}
@@ -97,18 +97,20 @@
localizer_resetter->theta = M_PI;
localizer_resetter->theta_uncertainty = 0.00001;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
// Wait for the drivetrain to run so it has time to reset the heading.
// Otherwise our drivetrain reset will do a 180 right at the start.
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
- LOG(INFO, "Heading is %f\n", drivetrain_status_fetcher_->estimated_heading);
+ AOS_LOG(INFO, "Heading is %f\n",
+ drivetrain_status_fetcher_->estimated_heading);
InitializeEncoders();
ResetDrivetrain();
WaitUntil([this]() { return drivetrain_status_fetcher_.Fetch(); });
- LOG(INFO, "Heading is %f\n", drivetrain_status_fetcher_->estimated_heading);
+ AOS_LOG(INFO, "Heading is %f\n",
+ drivetrain_status_fetcher_->estimated_heading);
ResetDrivetrain();
InitializeEncoders();
@@ -134,8 +136,8 @@
const bool is_left = params.mode == 0;
{
- LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
- params.mode, is_left ? "left" : "right");
+ AOS_LOG(INFO, "Starting autonomous action with mode %" PRId32 " %s\n",
+ params.mode, is_left ? "left" : "right");
}
const double turn_scalar = is_left ? 1.0 : -1.0;
@@ -157,12 +159,12 @@
// if planned start the spline and plan the next
if (!spline1.WaitForPlan()) return true;
- LOG(INFO, "Planned\n");
+ AOS_LOG(INFO, "Planned\n");
spline1.Start();
// If suction, move the superstructure to score
if (!WaitForGamePiece()) return true;
- LOG(INFO, "Has game piece\n");
+ AOS_LOG(INFO, "Has game piece\n");
if (!spline1.WaitForSplineDistanceRemaining(3.5)) return true;
set_elevator_wrist_goal(kPanelBackwardMiddlePos);
SendSuperstructureGoal();
@@ -186,7 +188,7 @@
if (!WaitForMilliseconds(::std::chrono::milliseconds(150))) return true;
if (!spline2.WaitForPlan()) return true;
- LOG(INFO, "Planned\n");
+ AOS_LOG(INFO, "Planned\n");
// Drive back to hp and set the superstructure accordingly
spline2.Start();
if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
@@ -207,7 +209,7 @@
if (!WaitForDriveXGreater(0.50)) return true;
if (!spline3.WaitForPlan()) return true;
spline3.Start();
- LOG(INFO, "Has game piece\n");
+ AOS_LOG(INFO, "Has game piece\n");
if (!WaitForMilliseconds(::std::chrono::milliseconds(1000))) return true;
set_elevator_wrist_goal(kPanelBackwardMiddlePos);
SendSuperstructureGoal();
@@ -235,12 +237,12 @@
// if planned start the spline and plan the next
if (!spline1.WaitForPlan()) return true;
- LOG(INFO, "Planned\n");
+ AOS_LOG(INFO, "Planned\n");
spline1.Start();
// If suction, move the superstructure to score
if (!WaitForGamePiece()) return true;
- LOG(INFO, "Has game piece\n");
+ AOS_LOG(INFO, "Has game piece\n");
// unstick the hatch panel
if (!WaitForMilliseconds(::std::chrono::milliseconds(500))) return true;
set_elevator_goal(0.5);
@@ -257,8 +259,8 @@
set_suction_goal(false, 1);
SendSuperstructureGoal();
- LOG(INFO, "Dropping disc 1 %f\n",
- ::aos::time::DurationInSeconds(monotonic_now() - start_time));
+ AOS_LOG(INFO, "Dropping disc 1 %f\n",
+ ::aos::time::DurationInSeconds(monotonic_now() - start_time));
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(300))) return true;
@@ -269,7 +271,7 @@
SplineDirection::kForward);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
if (!spline2.WaitForPlan()) return true;
- LOG(INFO, "Planned\n");
+ AOS_LOG(INFO, "Planned\n");
// Drive back to hp and set the superstructure accordingly
spline2.Start();
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
@@ -283,8 +285,8 @@
LineFollowAtVelocity(1.5);
// As soon as we pick up Panel 2 go score on the rocket
if (!WaitForGamePiece()) return true;
- LOG(INFO, "Got gamepiece %f\n",
- ::aos::time::DurationInSeconds(monotonic_now() - start_time));
+ AOS_LOG(INFO, "Got gamepiece %f\n",
+ ::aos::time::DurationInSeconds(monotonic_now() - start_time));
LineFollowAtVelocity(-4.0);
SplineHandle spline3 =
PlanSpline(AutonomousSplines::HPToThirdCargoShipBay(is_left),
@@ -300,19 +302,19 @@
if (!spline3.WaitForSplineDistanceRemaining(0.7)) return true;
// Line follow in to the second disc.
LineFollowAtVelocity(-0.7, 3);
- LOG(INFO, "Drawing in disc 2 %f\n",
- ::aos::time::DurationInSeconds(monotonic_now() - start_time));
+ AOS_LOG(INFO, "Drawing in disc 2 %f\n",
+ ::aos::time::DurationInSeconds(monotonic_now() - start_time));
if (!WaitForDriveYCloseToZero(1.2)) return true;
set_suction_goal(false, 1);
SendSuperstructureGoal();
- LOG(INFO, "Dropping disc 2 %f\n",
- ::aos::time::DurationInSeconds(monotonic_now() - start_time));
+ AOS_LOG(INFO, "Dropping disc 2 %f\n",
+ ::aos::time::DurationInSeconds(monotonic_now() - start_time));
if (!WaitForDriveYCloseToZero(1.13)) return true;
if (!WaitForMilliseconds(::std::chrono::milliseconds(200))) return true;
- LOG(INFO, "Backing up %f\n",
- ::aos::time::DurationInSeconds(monotonic_now() - start_time));
+ AOS_LOG(INFO, "Backing up %f\n",
+ ::aos::time::DurationInSeconds(monotonic_now() - start_time));
LineFollowAtVelocity(0.9, 3);
if (!WaitForMilliseconds(::std::chrono::milliseconds(400))) return true;
} else {
@@ -324,37 +326,37 @@
SendSuperstructureGoal();
if (!WaitForGamePiece()) return true;
- LOG(INFO, "Has game piece\n");
+ AOS_LOG(INFO, "Has game piece\n");
StartDrive(-4.0, 0.0, kJumpDrive, kTurn);
if (!WaitForDriveNear(3.3, 10.0)) return true;
- LOG(INFO, "Lifting\n");
+ AOS_LOG(INFO, "Lifting\n");
set_elevator_goal(0.30);
SendSuperstructureGoal();
if (!WaitForDriveNear(2.8, 10.0)) return true;
- LOG(INFO, "Off the platform\n");
+ AOS_LOG(INFO, "Off the platform\n");
StartDrive(0.0, 1.00 * turn_scalar, kDrive, kTurn);
- LOG(INFO, "Turn started\n");
+ AOS_LOG(INFO, "Turn started\n");
if (!WaitForSuperstructureDone()) return true;
- LOG(INFO, "Superstructure done\n");
+ AOS_LOG(INFO, "Superstructure done\n");
if (!WaitForDriveNear(0.7, 10.0)) return true;
StartDrive(0.0, -0.35 * turn_scalar, kDrive, kTurn);
- LOG(INFO, "Elevator up\n");
+ AOS_LOG(INFO, "Elevator up\n");
set_elevator_goal(0.78);
SendSuperstructureGoal();
if (!WaitForDriveDone()) return true;
- LOG(INFO, "Done driving\n");
+ AOS_LOG(INFO, "Done driving\n");
if (!WaitForSuperstructureDone()) return true;
}
- LOG(INFO, "Done %f\n",
- ::aos::time::DurationInSeconds(monotonic_now() - start_time));
+ AOS_LOG(INFO, "Done %f\n",
+ ::aos::time::DurationInSeconds(monotonic_now() - start_time));
return true;
}
diff --git a/y2019/actors/autonomous_actor.h b/y2019/actors/autonomous_actor.h
index edb2272..8c61596 100644
--- a/y2019/actors/autonomous_actor.h
+++ b/y2019/actors/autonomous_actor.h
@@ -94,7 +94,7 @@
wrist_max_acceleration_;
if (!new_superstructure_goal.Send()) {
- LOG(ERROR, "Sending superstructure goal failed.\n");
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
}
diff --git a/y2019/constants.cc b/y2019/constants.cc
index f9811a4..043624d 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -224,7 +224,7 @@
break;
default:
- LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
+ AOS_LOG(FATAL, "unknown team #%" PRIu16 "\n", team);
}
return r;
@@ -232,7 +232,7 @@
const Values *DoGetValues() {
uint16_t team = ::aos::network::GetTeamNumber();
- LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
+ AOS_LOG(INFO, "creating a Constants for team %" PRIu16 "\n", team);
return DoGetValuesForTeam(team);
}
diff --git a/y2019/control_loops/drivetrain/event_loop_localizer.cc b/y2019/control_loops/drivetrain/event_loop_localizer.cc
index 94ab9ea..67a708a 100644
--- a/y2019/control_loops/drivetrain/event_loop_localizer.cc
+++ b/y2019/control_loops/drivetrain/event_loop_localizer.cc
@@ -64,10 +64,10 @@
::aos::monotonic_clock::time_point now, double left_encoder,
double right_encoder, double gyro_rate,
double /*longitudinal_accelerometer*/) {
- CHECK(U.allFinite());
- CHECK(::std::isfinite(left_encoder));
- CHECK(::std::isfinite(right_encoder));
- CHECK(::std::isfinite(gyro_rate));
+ AOS_CHECK(U.allFinite());
+ AOS_CHECK(::std::isfinite(left_encoder));
+ AOS_CHECK(::std::isfinite(right_encoder));
+ AOS_CHECK(::std::isfinite(gyro_rate));
localizer_.UpdateEncodersAndGyro(left_encoder, right_encoder, gyro_rate, U,
now);
while (frame_fetcher_.FetchNext()) {
@@ -81,11 +81,11 @@
// Note: num_targets and camera are unsigned integers and so don't need to be
// checked for < 0.
if (frame.num_targets > kMaxTargetsPerFrame) {
- LOG(ERROR, "Got bad num_targets %d\n", frame.num_targets);
+ AOS_LOG(ERROR, "Got bad num_targets %d\n", frame.num_targets);
return;
}
if (frame.camera > cameras_.size()) {
- LOG(ERROR, "Got bad camera number %d\n", frame.camera);
+ AOS_LOG(ERROR, "Got bad camera number %d\n", frame.camera);
return;
}
for (int ii = 0; ii < frame.num_targets; ++ii) {
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index 1571fec..d716535 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -54,14 +54,14 @@
}
if (!SanitizeTargets(targets)) {
- LOG(ERROR, "Throwing out targets due to in insane values.\n");
+ AOS_LOG(ERROR, "Throwing out targets due to in insane values.\n");
return;
}
if (t > HybridEkf::latest_t()) {
- LOG(ERROR,
- "target observations must be older than most recent encoder/gyro "
- "update.\n");
+ AOS_LOG(ERROR,
+ "target observations must be older than most recent encoder/gyro "
+ "update.\n");
return;
}
@@ -111,15 +111,15 @@
if (!(::std::isfinite(reading.heading) &&
::std::isfinite(reading.distance) &&
::std::isfinite(reading.skew) && ::std::isfinite(reading.height))) {
- LOG(ERROR, "Got non-finite values in target.\n");
+ AOS_LOG(ERROR, "Got non-finite values in target.\n");
return false;
}
if (reading.distance < 0) {
- LOG(ERROR, "Got negative distance.\n");
+ AOS_LOG(ERROR, "Got negative distance.\n");
return false;
}
if (::std::abs(::aos::math::NormalizeAngle(reading.skew)) > M_PI_2) {
- LOG(ERROR, "Got skew > pi / 2.\n");
+ AOS_LOG(ERROR, "Got skew > pi / 2.\n");
return false;
}
}
@@ -284,7 +284,7 @@
}
if (camera_views.size() == 0) {
- LOG(DEBUG, "Unable to identify potential target matches.\n");
+ AOS_LOG(DEBUG, "Unable to identify potential target matches.\n");
// If we can't get a match, provide H = zero, which will make this
// correction step a nop.
*h = [](const State &, const Input &) { return Output::Zero(); };
@@ -305,7 +305,7 @@
for (size_t ii = 0; ii < target_views.size(); ++ii) {
size_t view_idx = best_frames[ii];
if (view_idx < 0 || view_idx >= camera_views.size()) {
- LOG(ERROR, "Somehow, the view scorer failed.\n");
+ AOS_LOG(ERROR, "Somehow, the view scorer failed.\n");
h_functions->push_back(
[](const State &, const Input &) { return Output::Zero(); });
dhdx_functions->push_back([](const State &) {
@@ -319,10 +319,10 @@
const TargetView target_view = target_views[ii];
const Scalar match_score = scores(ii, view_idx);
if (match_score > kRejectionScore) {
- LOG(DEBUG,
- "Rejecting target at (%f, %f, %f, %f) due to high score.\n",
- target_view.reading.heading, target_view.reading.distance,
- target_view.reading.skew, target_view.reading.height);
+ AOS_LOG(DEBUG,
+ "Rejecting target at (%f, %f, %f, %f) due to high score.\n",
+ target_view.reading.heading, target_view.reading.distance,
+ target_view.reading.skew, target_view.reading.height);
h_functions->push_back(
[](const State &, const Input &) { return Output::Zero(); });
dhdx_functions->push_back([](const State &) {
@@ -456,7 +456,7 @@
// If we reach here and best_match = -1, that means that no potential
// targets were generated by the camera, and we should never have gotten
// here.
- CHECK(best_match != -1);
+ AOS_CHECK(best_match != -1);
::aos::SizedArray<bool, max_targets_per_frame> sub_views = used_views;
sub_views[ii] = true;
::std::array<bool, num_targets> sub_targets = used_targets;
diff --git a/y2019/control_loops/drivetrain/replay_localizer.cc b/y2019/control_loops/drivetrain/replay_localizer.cc
index ce0ac09..93282c8 100644
--- a/y2019/control_loops/drivetrain/replay_localizer.cc
+++ b/y2019/control_loops/drivetrain/replay_localizer.cc
@@ -139,7 +139,7 @@
replayer_.AddHandler<frc971::control_loops::drivetrain::LocalizerControl>(
drivetrain, "localizer_control",
[this](const frc971::control_loops::drivetrain::LocalizerControl &msg) {
- LOG_STRUCT(DEBUG, "localizer_control", msg);
+ AOS_LOG_STRUCT(DEBUG, "localizer_control", msg);
localizer_.ResetPosition(msg.sent_time, msg.x, msg.y, msg.theta,
msg.theta_uncertainty,
!msg.keep_current_theta);
@@ -186,7 +186,7 @@
fd = open(filename, O_RDONLY);
}
if (fd == -1) {
- PLOG(FATAL, "couldn't open file '%s' for reading", filename);
+ AOS_PLOG(FATAL, "couldn't open file '%s' for reading", filename);
}
replayer_.OpenFile(fd);
@@ -204,7 +204,7 @@
}
replayer_.CloseCurrentFile();
- PCHECK(close(fd));
+ AOS_PCHECK(close(fd));
Plot();
}
diff --git a/y2019/control_loops/drivetrain/target_selector.cc b/y2019/control_loops/drivetrain/target_selector.cc
index a338181..3e43db9 100644
--- a/y2019/control_loops/drivetrain/target_selector.cc
+++ b/y2019/control_loops/drivetrain/target_selector.cc
@@ -25,13 +25,13 @@
ball_mode_ = superstructure_goal_fetcher_->suction.gamepiece_mode == 0;
}
if (hint_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "selector_hint", *hint_fetcher_);
+ AOS_LOG_STRUCT(DEBUG, "selector_hint", *hint_fetcher_);
// suggested_target is unsigned so we don't check for >= 0.
if (hint_fetcher_->suggested_target < 4) {
target_hint_ =
static_cast<SelectionHint>(hint_fetcher_->suggested_target);
} else {
- LOG(ERROR, "Got invalid suggested target.\n");
+ AOS_LOG(ERROR, "Got invalid suggested target.\n");
}
}
*robot_pose_.mutable_pos() << state.x(), state.y(), 0.0;
diff --git a/y2019/control_loops/superstructure/superstructure.cc b/y2019/control_loops/superstructure/superstructure.cc
index ec9e2e5..ff9bb13 100644
--- a/y2019/control_loops/superstructure/superstructure.cc
+++ b/y2019/control_loops/superstructure/superstructure.cc
@@ -30,7 +30,7 @@
SuperstructureQueue::Output *output,
SuperstructureQueue::Status *status) {
if (WasReset()) {
- LOG(ERROR, "WPILib reset, restarting\n");
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
elevator_.Reset();
wrist_.Reset();
intake_.Reset();
@@ -155,7 +155,7 @@
new_status_light->blue = blue;
if (!new_status_light.Send()) {
- LOG(ERROR, "Failed to send lights.\n");
+ AOS_LOG(ERROR, "Failed to send lights.\n");
}
}
diff --git a/y2019/image_streamer/image_streamer.cc b/y2019/image_streamer/image_streamer.cc
index ebcd0b2..30bea8f 100644
--- a/y2019/image_streamer/image_streamer.cc
+++ b/y2019/image_streamer/image_streamer.cc
@@ -92,7 +92,7 @@
}
size_t Recv(void *data, int size) {
- return PCHECK(recv(fd(), static_cast<char *>(data), size, 0));
+ return AOS_PCHECK(recv(fd(), static_cast<char *>(data), size, 0));
}
};
@@ -197,7 +197,7 @@
fprintf(stderr, "wrong sized buffer\n");
exit(-1);
}
- LOG(INFO, "Frame size in bytes: data.size() = %zu\n", data.size());
+ AOS_LOG(INFO, "Frame size in bytes: data.size() = %zu\n", data.size());
output_buffer_.push_back(aos::vision::DataRef(data_header_tmp_, n_written));
output_buffer_.push_back(data);
output_buffer_.push_back("\r\n\r\n");
@@ -322,11 +322,11 @@
params1.set_exposure(FLAGS_camera1_exposure);
::y2019::VisionStatus vision_status;
- LOG(INFO,
- "The UDP socket should be on port 5001 to 10.9.71.2 for "
- "the competition robot.\n");
- LOG(INFO, "Starting UDP socket on port 5001 to %s\n",
- FLAGS_roborio_ip.c_str());
+ AOS_LOG(INFO,
+ "The UDP socket should be on port 5001 to 10.9.71.2 for "
+ "the competition robot.\n");
+ AOS_LOG(INFO, "Starting UDP socket on port 5001 to %s\n",
+ FLAGS_roborio_ip.c_str());
::aos::events::ProtoTXUdpSocket<::y2019::VisionStatus> status_socket(
FLAGS_roborio_ip.c_str(), 5001);
@@ -335,7 +335,7 @@
params0, "/dev/video0", &tcp_server_, !FLAGS_log.empty(),
[&camera0, &status_socket, &vision_status]() {
vision_status.set_low_frame_count(vision_status.low_frame_count() + 1);
- LOG(INFO, "Got a frame cam0\n");
+ AOS_LOG(INFO, "Got a frame cam0\n");
if (camera0->active()) {
status_socket.Send(vision_status);
}
@@ -346,7 +346,7 @@
[&camera1, &status_socket, &vision_status]() {
vision_status.set_high_frame_count(vision_status.high_frame_count() +
1);
- LOG(INFO, "Got a frame cam1\n");
+ AOS_LOG(INFO, "Got a frame cam1\n");
if (camera1->active()) {
status_socket.Send(vision_status);
}
@@ -366,7 +366,8 @@
cam0_active = true;
camera0->set_active(true);
}
- LOG(INFO, "Got control packet, cam%d active\n", cam0_active ? 0 : 1);
+ AOS_LOG(INFO, "Got control packet, cam%d active\n",
+ cam0_active ? 0 : 1);
});
// Default to camera0
diff --git a/y2019/jevois/camera/image_stream.cc b/y2019/jevois/camera/image_stream.cc
index 29a1513..44fd8e5 100644
--- a/y2019/jevois/camera/image_stream.cc
+++ b/y2019/jevois/camera/image_stream.cc
@@ -8,7 +8,7 @@
void ImageStreamEvent::ProcessHelper(
aos::vision::DataRef data, aos::monotonic_clock::time_point timestamp) {
if (data.size() < 300) {
- LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
+ AOS_LOG(INFO, "got bad img of size(%d)\n", static_cast<int>(data.size()));
return;
}
ProcessImage(data, timestamp);
diff --git a/y2019/jevois/serial.cc b/y2019/jevois/serial.cc
index 7c49b4f..5febbb4 100644
--- a/y2019/jevois/serial.cc
+++ b/y2019/jevois/serial.cc
@@ -14,11 +14,12 @@
int open_via_terminos(const char *tty_name) {
int itsDev = ::open(tty_name, O_RDWR | O_NOCTTY | O_NONBLOCK);
if (itsDev == -1) {
- LOG(FATAL, "problem opening: %s\n", tty_name);
+ AOS_LOG(FATAL, "problem opening: %s\n", tty_name);
}
termios options = {};
- if (tcgetattr(itsDev, &options) == -1) LOG(FATAL, "Failed to get options");
+ if (tcgetattr(itsDev, &options) == -1)
+ AOS_LOG(FATAL, "Failed to get options");
// get raw input from the port
options.c_cflag |= (CLOCAL // ignore modem control lines
@@ -70,7 +71,7 @@
// options.c_cflag |= CRTSCTS;
if (tcsetattr(itsDev, TCSANOW, &options) == -1)
- LOG(FATAL, "Failed to set port options");
+ AOS_LOG(FATAL, "Failed to set port options");
return itsDev;
}
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index c695c89..b6a281e 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -5,8 +5,8 @@
#ifdef __linux__
#include "aos/logging/logging.h"
#else
-#define CHECK(...)
-#define CHECK_GE(...)
+#define AOS_CHECK(...)
+#define AOS_CHECK_GE(...)
#endif
// SPI transfer format (6x 8 bit frames):
@@ -175,11 +175,11 @@
crc = jevois_crc_update(crc, transfer.data(),
transfer.size() - remaining_space.size());
crc = jevois_crc_finalize(crc);
- CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
memcpy(&remaining_space[0], &crc, sizeof(crc));
remaining_space = remaining_space.subspan(sizeof(crc));
}
- CHECK(remaining_space.empty());
+ AOS_CHECK(remaining_space.empty());
return transfer;
}
@@ -226,10 +226,11 @@
transfer.size() - remaining_input.size());
calculated_crc = jevois_crc_finalize(calculated_crc);
uint16_t received_crc;
- CHECK_GE(static_cast<size_t>(remaining_input.size()), sizeof(received_crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_input.size()),
+ sizeof(received_crc));
memcpy(&received_crc, &remaining_input[0], sizeof(received_crc));
remaining_input = remaining_input.subspan(sizeof(received_crc));
- CHECK(remaining_input.empty());
+ AOS_CHECK(remaining_input.empty());
if (calculated_crc != received_crc) {
return tl::nullopt;
}
@@ -259,7 +260,7 @@
crc = jevois_crc_update(crc, transfer.data(),
transfer.size() - remaining_space.size());
crc = jevois_crc_finalize(crc);
- CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
memcpy(&remaining_space[0], &crc, sizeof(crc));
remaining_space = remaining_space.subspan(sizeof(crc));
}
@@ -292,7 +293,8 @@
transfer.size() - remaining_input.size());
calculated_crc = jevois_crc_finalize(calculated_crc);
uint16_t received_crc;
- CHECK_GE(static_cast<size_t>(remaining_input.size()), sizeof(received_crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_input.size()),
+ sizeof(received_crc));
memcpy(&received_crc, &remaining_input[0], sizeof(received_crc));
remaining_input = remaining_input.subspan(sizeof(received_crc));
if (calculated_crc != received_crc) {
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index 63138a8..6ebfe30 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -8,8 +8,8 @@
#ifdef __linux__
#include "aos/logging/logging.h"
#else
-#define CHECK(...)
-#define CHECK_GE(...)
+#define AOS_CHECK(...)
+#define AOS_CHECK_GE(...)
#endif
namespace frc971 {
@@ -43,11 +43,11 @@
crc = jevois_crc_update(crc, buffer.data(),
buffer.size() - remaining_space.size());
crc = jevois_crc_finalize(crc);
- CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
memcpy(&remaining_space[0], &crc, sizeof(crc));
remaining_space = remaining_space.subspan(sizeof(crc));
}
- CHECK(remaining_space.empty());
+ AOS_CHECK(remaining_space.empty());
UartToTeensyBuffer result;
result.set_size(
CobsEncode<uart_to_teensy_size()>(buffer, result.mutable_backing_array())
@@ -91,10 +91,11 @@
buffer.size() - remaining_input.size());
calculated_crc = jevois_crc_finalize(calculated_crc);
uint16_t received_crc;
- CHECK_GE(static_cast<size_t>(remaining_input.size()), sizeof(received_crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_input.size()),
+ sizeof(received_crc));
memcpy(&received_crc, &remaining_input[0], sizeof(received_crc));
remaining_input = remaining_input.subspan(sizeof(received_crc));
- CHECK(remaining_input.empty());
+ AOS_CHECK(remaining_input.empty());
if (calculated_crc != received_crc) {
return tl::nullopt;
}
@@ -129,11 +130,11 @@
crc = jevois_crc_update(crc, buffer.data(),
buffer.size() - remaining_space.size());
crc = jevois_crc_finalize(crc);
- CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_space.size()), sizeof(crc));
memcpy(&remaining_space[0], &crc, sizeof(crc));
remaining_space = remaining_space.subspan(sizeof(crc));
}
- CHECK(remaining_space.empty());
+ AOS_CHECK(remaining_space.empty());
UartToCameraBuffer result;
result.set_size(
CobsEncode<uart_to_camera_size()>(buffer, result.mutable_backing_array())
@@ -180,10 +181,11 @@
buffer.size() - remaining_input.size());
calculated_crc = jevois_crc_finalize(calculated_crc);
uint16_t received_crc;
- CHECK_GE(static_cast<size_t>(remaining_input.size()), sizeof(received_crc));
+ AOS_CHECK_GE(static_cast<size_t>(remaining_input.size()),
+ sizeof(received_crc));
memcpy(&received_crc, &remaining_input[0], sizeof(received_crc));
remaining_input = remaining_input.subspan(sizeof(received_crc));
- CHECK(remaining_input.empty());
+ AOS_CHECK(remaining_input.empty());
if (calculated_crc != received_crc) {
return tl::nullopt;
}
diff --git a/y2019/joystick_reader.cc b/y2019/joystick_reader.cc
index 5f5f06c..9450b13 100644
--- a/y2019/joystick_reader.cc
+++ b/y2019/joystick_reader.cc
@@ -179,7 +179,7 @@
}
void AutoEnded() override {
- LOG(INFO, "Auto ended, assuming disc and have piece\n");
+ AOS_LOG(INFO, "Auto ended, assuming disc and have piece\n");
grab_piece_ = true;
switch_ball_ = false;
}
@@ -191,7 +191,7 @@
superstructure_status_fetcher_.Fetch();
if (!superstructure_status_fetcher_.get() ||
!superstructure_position_fetcher_.get()) {
- LOG(ERROR, "Got no superstructure status or position packet.\n");
+ AOS_LOG(ERROR, "Got no superstructure status or position packet.\n");
return;
}
@@ -223,7 +223,7 @@
}
}
if (!target_hint.Send()) {
- LOG(ERROR, "Failed to send target selector hint.\n");
+ AOS_LOG(ERROR, "Failed to send target selector hint.\n");
}
}
@@ -235,7 +235,7 @@
localizer_resetter->keep_current_theta = true;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -247,7 +247,7 @@
localizer_resetter->keep_current_theta = true;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -259,7 +259,7 @@
localizer_resetter->theta = 0.0;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -271,7 +271,7 @@
localizer_resetter->theta = M_PI;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -283,7 +283,7 @@
localizer_resetter->theta = 0.0;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -295,7 +295,7 @@
localizer_resetter->theta = M_PI;
if (!localizer_resetter.Send()) {
- LOG(ERROR, "Failed to reset localizer.\n");
+ AOS_LOG(ERROR, "Failed to reset localizer.\n");
}
}
@@ -313,7 +313,7 @@
last_release_button_press_ = monotonic_now;
}
- LOG(INFO, "has_piece: %d\n", superstructure_status_fetcher_->has_piece);
+ AOS_LOG(INFO, "has_piece: %d\n", superstructure_status_fetcher_->has_piece);
if (data.IsPressed(kSuctionBall)) {
grab_piece_ = true;
} else if (data.IsPressed(kSuctionHatch)) {
@@ -323,7 +323,7 @@
data.IsPressed(kReleaseButtonBoard) ||
!superstructure_status_fetcher_->has_piece) {
grab_piece_ = false;
- LOG(INFO, "releasing due to other thing\n");
+ AOS_LOG(INFO, "releasing due to other thing\n");
}
if (data.IsPressed(kRocketBackwardUnpressed)) {
@@ -477,7 +477,7 @@
data.IsPressed(kRelease)) ||
data.IsPressed(kReleaseButtonBoard)) {
grab_piece_ = false;
- LOG(INFO, "Releasing due to button\n");
+ AOS_LOG(INFO, "Releasing due to button\n");
}
if (switch_ball_) {
@@ -494,9 +494,9 @@
elevator_wrist_pos_.elevator;
new_superstructure_goal->wrist.unsafe_goal = elevator_wrist_pos_.wrist;
- LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
+ AOS_LOG_STRUCT(DEBUG, "sending goal", *new_superstructure_goal);
if (!new_superstructure_goal.Send()) {
- LOG(ERROR, "Sending superstructure goal failed.\n");
+ AOS_LOG(ERROR, "Sending superstructure goal failed.\n");
}
if (monotonic_now >
@@ -508,7 +508,7 @@
{
auto camera_log_message = camera_log_sender_.MakeMessage();
camera_log_message->log = data.IsPressed(kCameraLog);
- LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
+ AOS_LOG_STRUCT(DEBUG, "camera_log", *camera_log_message);
camera_log_message.Send();
}
}
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index baab2c2..e66a70b 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -46,19 +46,19 @@
void WebsocketHandler::onConnect(seasocks::WebSocket *connection) {
connections_.insert(connection);
- LOG(INFO, "Connected: %s : %s\n", connection->getRequestUri().c_str(),
- seasocks::formatAddress(connection->getRemoteAddress()).c_str());
+ AOS_LOG(INFO, "Connected: %s : %s\n", connection->getRequestUri().c_str(),
+ seasocks::formatAddress(connection->getRemoteAddress()).c_str());
}
void WebsocketHandler::onData(seasocks::WebSocket * /*connection*/,
const char *data) {
- LOG(INFO, "Got data: %s\n", data);
+ AOS_LOG(INFO, "Got data: %s\n", data);
}
void WebsocketHandler::onDisconnect(seasocks::WebSocket *connection) {
connections_.erase(connection);
- LOG(INFO, "Disconnected: %s : %s\n", connection->getRequestUri().c_str(),
- seasocks::formatAddress(connection->getRemoteAddress()).c_str());
+ AOS_LOG(INFO, "Disconnected: %s : %s\n", connection->getRequestUri().c_str(),
+ seasocks::formatAddress(connection->getRemoteAddress()).c_str());
}
void WebsocketHandler::SendData(const std::string &data) {
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 9ca37d8..4f09da1 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -297,7 +297,7 @@
auto_mode_message->mode |= 1 << i;
}
}
- LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
+ AOS_LOG_STRUCT(DEBUG, "auto mode", *auto_mode_message);
auto_mode_message.Send();
}
}
@@ -384,7 +384,7 @@
const auto unpacked = SpiUnpackToRoborio(
gsl::make_span(to_receive).last(spi_transfer_size()));
if (!unpacked) {
- LOG(INFO, "Decoding SPI data failed\n");
+ AOS_LOG(INFO, "Decoding SPI data failed\n");
return;
}
@@ -405,7 +405,7 @@
to_send->targets[i].skew = received.targets[i].skew;
}
to_send->camera = received.camera_index;
- LOG_STRUCT(DEBUG, "camera_frames", *to_send);
+ AOS_LOG_STRUCT(DEBUG, "camera_frames", *to_send);
to_send.Send();
}
@@ -416,7 +416,7 @@
}
void DoTransaction(gsl::span<char> to_send, gsl::span<char> to_receive) {
- CHECK_EQ(to_send.size(), to_receive.size());
+ AOS_CHECK_EQ(to_send.size(), to_receive.size());
const auto result = spi_->Transaction(
reinterpret_cast<uint8_t *>(to_send.data()),
reinterpret_cast<uint8_t *>(to_receive.data()), to_send.size());
@@ -424,10 +424,10 @@
return;
}
if (result == -1) {
- LOG(INFO, "SPI::Transaction of %zd bytes failed\n", to_send.size());
+ AOS_LOG(INFO, "SPI::Transaction of %zd bytes failed\n", to_send.size());
return;
}
- LOG(FATAL, "SPI::Transaction returned something weird\n");
+ AOS_LOG(FATAL, "SPI::Transaction returned something weird\n");
}
void SetDummySPI(frc::SPI::Port port) {
@@ -489,7 +489,7 @@
private:
void Write(const SuperstructureQueue::Output &output) override {
- LOG_STRUCT(DEBUG, "will output", output);
+ AOS_LOG_STRUCT(DEBUG, "will output", output);
elevator_victor_->SetSpeed(::aos::Clip(output.elevator_voltage,
-kMaxBringupPower,
kMaxBringupPower) /
@@ -522,7 +522,7 @@
}
void Stop() override {
- LOG(WARNING, "Superstructure output too old.\n");
+ AOS_LOG(WARNING, "Superstructure output too old.\n");
elevator_victor_->SetDisabled();
intake_victor_->SetDisabled();
@@ -574,13 +574,13 @@
void Loop(const int iterations) {
if (iterations != 1) {
- LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
+ AOS_LOG(DEBUG, "Solenoids skipped %d iterations\n", iterations - 1);
}
{
superstructure_fetcher_.Fetch();
if (superstructure_fetcher_.get()) {
- LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
+ AOS_LOG_STRUCT(DEBUG, "solenoids", *superstructure_fetcher_);
big_suction_cup0_->Set(!superstructure_fetcher_->intake_suction_bottom);
big_suction_cup1_->Set(!superstructure_fetcher_->intake_suction_bottom);
@@ -600,7 +600,7 @@
pcm_.Flush();
to_log.read_solenoids = pcm_.GetAll();
- LOG_STRUCT(DEBUG, "pneumatics info", to_log);
+ AOS_LOG_STRUCT(DEBUG, "pneumatics info", to_log);
}
status_light_fetcher_.Fetch();
@@ -623,10 +623,10 @@
light_flash_ = 0;
}
- LOG_STRUCT(DEBUG, "color", color);
+ AOS_LOG_STRUCT(DEBUG, "color", color);
SetColor(color);
} else {
- LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "color", *status_light_fetcher_.get());
SetColor(*status_light_fetcher_.get());
}
}