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/frc971/autonomous/base_autonomous_actor.cc b/frc971/autonomous/base_autonomous_actor.cc
index c78a90d..367e849 100644
--- a/frc971/autonomous/base_autonomous_actor.cc
+++ b/frc971/autonomous/base_autonomous_actor.cc
@@ -44,7 +44,7 @@
".frc971.control_loops.drivetrain_queue.goal")) {}
void BaseAutonomousActor::ResetDrivetrain() {
- LOG(INFO, "resetting the drivetrain\n");
+ AOS_LOG(INFO, "resetting the drivetrain\n");
max_drivetrain_voltage_ = 12.0;
goal_spline_handle_ = 0;
@@ -72,7 +72,7 @@
void BaseAutonomousActor::StartDrive(double distance, double angle,
ProfileParameters linear,
ProfileParameters angular) {
- LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
+ AOS_LOG(INFO, "Driving distance %f, angle %f\n", distance, angle);
{
const double dangle = angle * dt_config_.robot_radius;
initial_drivetrain_.left += distance - dangle;
@@ -90,7 +90,7 @@
drivetrain_message->linear = linear;
drivetrain_message->angular = angular;
- LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+ AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
drivetrain_message.Send();
}
@@ -98,7 +98,7 @@
void BaseAutonomousActor::WaitUntilDoneOrCanceled(
::std::unique_ptr<aos::common::actions::Action> action) {
if (!action) {
- LOG(ERROR, "No action, not waiting\n");
+ AOS_LOG(ERROR, "No action, not waiting\n");
return;
}
@@ -149,7 +149,7 @@
kVelocityTolerance &&
::std::abs(drivetrain_status_fetcher_->estimated_right_velocity) <
kVelocityTolerance) {
- LOG(INFO, "Finished drive\n");
+ AOS_LOG(INFO, "Finished drive\n");
return true;
}
}
@@ -275,17 +275,17 @@
drive_has_been_close |= drive_close;
turn_has_been_close |= turn_close;
if (drive_has_been_close && !turn_has_been_close && !printed_first) {
- LOG(INFO, "Drive finished first\n");
+ AOS_LOG(INFO, "Drive finished first\n");
printed_first = true;
} else if (!drive_has_been_close && turn_has_been_close &&
!printed_first) {
- LOG(INFO, "Turn finished first\n");
+ AOS_LOG(INFO, "Turn finished first\n");
printed_first = true;
}
if (drive_close && turn_close) {
- LOG(INFO, "Closer than %f < %f distance, %f < %f angle\n",
- distance_to_go, distance, angle_to_go, angle);
+ AOS_LOG(INFO, "Closer than %f < %f distance, %f < %f angle\n",
+ distance_to_go, distance, angle_to_go, angle);
return true;
}
}
@@ -316,7 +316,7 @@
if (drivetrain_status_fetcher_.get()) {
if (::std::abs(linear_error(0)) < tolerance) {
- LOG(INFO, "Finished drive\n");
+ AOS_LOG(INFO, "Finished drive\n");
return true;
}
}
@@ -352,7 +352,7 @@
if (drivetrain_status_fetcher_.get()) {
if (::std::abs(angular_error(0)) < tolerance) {
- LOG(INFO, "Finished turn\n");
+ AOS_LOG(INFO, "Finished turn\n");
return true;
}
}
@@ -423,7 +423,7 @@
BaseAutonomousActor::SplineHandle BaseAutonomousActor::PlanSpline(
const ::frc971::MultiSpline &spline, SplineDirection direction) {
- LOG(INFO, "Planning spline\n");
+ AOS_LOG(INFO, "Planning spline\n");
int32_t spline_handle = (++spline_handle_) | ((getpid() & 0xFFFF) << 15);
@@ -445,7 +445,7 @@
drivetrain_message->spline.drive_spline_backwards =
direction == SplineDirection::kBackward;
- LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+ AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
drivetrain_message.Send();
@@ -454,8 +454,8 @@
bool BaseAutonomousActor::SplineHandle::IsPlanned() {
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
- LOG_STRUCT(DEBUG, "dts",
- *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
+ AOS_LOG_STRUCT(DEBUG, "dts",
+ *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
if (base_autonomous_actor_->drivetrain_status_fetcher_.get() &&
((base_autonomous_actor_->drivetrain_status_fetcher_->trajectory_logging
.planning_spline_idx == spline_handle_ &&
@@ -489,20 +489,20 @@
base_autonomous_actor_->drivetrain_goal_sender_.MakeMessage();
drivetrain_message->controller_type = 2;
- LOG(INFO, "Starting spline\n");
+ AOS_LOG(INFO, "Starting spline\n");
drivetrain_message->spline_handle = spline_handle_;
base_autonomous_actor_->goal_spline_handle_ = spline_handle_;
- LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
+ AOS_LOG_STRUCT(DEBUG, "dtg", *drivetrain_message);
drivetrain_message.Send();
}
bool BaseAutonomousActor::SplineHandle::IsDone() {
base_autonomous_actor_->drivetrain_status_fetcher_.Fetch();
- LOG_STRUCT(INFO, "dts",
- *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
+ AOS_LOG_STRUCT(INFO, "dts",
+ *(base_autonomous_actor_->drivetrain_status_fetcher_.get()));
// We check that the spline we are waiting on is neither currently planning
// nor executing (we check is_executed because it is possible to receive
diff --git a/frc971/control_loops/coerce_goal.h b/frc971/control_loops/coerce_goal.h
index 801135a..6c1f504 100644
--- a/frc971/control_loops/coerce_goal.h
+++ b/frc971/control_loops/coerce_goal.h
@@ -76,7 +76,7 @@
} else {
Eigen::Matrix<Scalar, 2, 4> region_vertices = region.StaticVertices();
#ifdef __linux__
- CHECK_GT(region_vertices.outerSize(), 0);
+ AOS_CHECK_GT(region_vertices.outerSize(), 0);
#else
assert(region_vertices.outerSize() > 0);
#endif
diff --git a/frc971/control_loops/drivetrain/distance_spline.cc b/frc971/control_loops/drivetrain/distance_spline.cc
index 1572526..cdb7fb9 100644
--- a/frc971/control_loops/drivetrain/distance_spline.cc
+++ b/frc971/control_loops/drivetrain/distance_spline.cc
@@ -23,16 +23,18 @@
const ::Eigen::Matrix<double, 2, 1> start1 = spline1.Point(0.0);
if (!end0.isApprox(start1, 1e-6)) {
- LOG(ERROR, "Splines %d and %d don't line up. [%f, %f] != [%f, %f]\n",
- static_cast<int>(i - 1), static_cast<int>(i), end0(0, 0),
- end0(1, 0), start1(0, 0), start1(1, 0));
+ AOS_LOG(ERROR,
+ "Splines %d and %d don't line up. [%f, %f] != [%f, %f]\n",
+ static_cast<int>(i - 1), static_cast<int>(i), end0(0, 0),
+ end0(1, 0), start1(0, 0), start1(1, 0));
}
const ::Eigen::Matrix<double, 2, 1> dend0 = spline0.DPoint(1.0);
const ::Eigen::Matrix<double, 2, 1> dstart1 = spline1.DPoint(0.0);
if (!dend0.isApprox(dstart1, 1e-6)) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Splines %d and %d don't line up in the first derivitive. [%f, "
"%f] != [%f, %f]\n",
static_cast<int>(i - 1), static_cast<int>(i), dend0(0, 0),
@@ -43,7 +45,8 @@
const ::Eigen::Matrix<double, 2, 1> ddstart1 = spline1.DDPoint(0.0);
if (!ddend0.isApprox(ddstart1, 1e-6)) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Splines %d and %d don't line up in the second derivitive. [%f, "
"%f] != [%f, %f]\n",
static_cast<int>(i - 1), static_cast<int>(i), ddend0(0, 0),
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index b9d2bc4..4685ed6 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -162,7 +162,7 @@
}
if (accel_squared > 1.03 || accel_squared < 0.97) {
- LOG(DEBUG, "New IMU value, rejecting reading\n");
+ AOS_LOG(DEBUG, "New IMU value, rejecting reading\n");
} else {
// -y is our gyro.
// z accel is down
@@ -172,10 +172,10 @@
down_estimator_.Correct(Y);
}
- LOG(DEBUG,
- "New IMU value, rate is %f, angle %f, fused %f, bias "
- "%f\n",
- rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
+ AOS_LOG(DEBUG,
+ "New IMU value, rate is %f, angle %f, fused %f, bias "
+ "%f\n",
+ rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
down_U_(0, 0) = rate;
}
down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
@@ -185,48 +185,48 @@
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = imu_values_fetcher_->gyro_x;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Y_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = imu_values_fetcher_->gyro_y;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Z_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = imu_values_fetcher_->gyro_z;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
last_gyro_rate_ = -imu_values_fetcher_->gyro_z;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
last_gyro_rate_ = gyro_reading_fetcher_->velocity;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_SPARTAN_GYRO:
if (gyro_reading_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
+ AOS_LOG_STRUCT(DEBUG, "using", *gyro_reading_fetcher_.get());
last_gyro_rate_ = -gyro_reading_fetcher_->velocity;
last_gyro_time_ = monotonic_now;
}
break;
default:
- LOG(FATAL, "invalid gyro configured");
+ AOS_LOG(FATAL, "invalid gyro configured");
break;
}
@@ -244,7 +244,7 @@
// TODO(james): Use a watcher (instead of a fetcher) once we support it in
// simulation.
if (localizer_control_fetcher_.Fetch()) {
- LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
+ AOS_LOG_STRUCT(DEBUG, "localizer_control", *localizer_control_fetcher_);
localizer_->ResetPosition(
monotonic_now, localizer_control_fetcher_->x,
localizer_control_fetcher_->y, localizer_control_fetcher_->theta,
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index 16b0b61..3f3a7f7 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -157,7 +157,7 @@
// Because the check below for have_zeroed_encoders_ will add an
// Observation, do a check here to ensure that initialization has been
// performed and so there is at least one observation.
- CHECK(!observations_.empty());
+ AOS_CHECK(!observations_.empty());
if (!have_zeroed_encoders_) {
// This logic handles ensuring that on the first encoder reading, we
// update the internal state for the encoders to match the reading.
@@ -310,7 +310,7 @@
PredictImpl(obs->U, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
- CHECK(obs->make_h);
+ AOS_CHECK(obs->make_h);
obs->make_h(*state, *P, &obs->h, &obs->dhdx);
}
CorrectImpl(obs->R, obs->z, obs->h(*state, obs->U), obs->dhdx(*state),
@@ -351,18 +351,19 @@
dhdx,
const Eigen::Matrix<Scalar, kNOutputs, kNOutputs> &R,
aos::monotonic_clock::time_point t) {
- CHECK(!observations_.empty());
+ AOS_CHECK(!observations_.empty());
if (!observations_.full() && t < observations_.begin()->t) {
- LOG(ERROR,
- "Dropped an observation that was received before we "
- "initialized.\n");
+ AOS_LOG(ERROR,
+ "Dropped an observation that was received before we "
+ "initialized.\n");
return;
}
auto cur_it =
observations_.PushFromBottom({t, t, State::Zero(), StateSquare::Zero(),
Input::Zero(), z, make_h, h, dhdx, R});
if (cur_it == observations_.end()) {
- LOG(DEBUG,
+ AOS_LOG(
+ DEBUG,
"Camera dropped off of end with time of %fs; earliest observation in "
"queue has time of %fs.\n",
::aos::time::DurationInSeconds(t.time_since_epoch()),
@@ -395,7 +396,7 @@
--prev_it;
cur_it->prev_t = prev_it->t;
// TODO(james): Figure out a saner way of handling this.
- CHECK(U != nullptr);
+ AOS_CHECK(U != nullptr);
cur_it->U = *U;
} else {
cur_it->X_hat = next_it->X_hat;
@@ -411,7 +412,7 @@
// We use X_hat_ and P_ to store the intermediate states, and then
// once we reach the end they will all be up-to-date.
ProcessObservation(&*cur_it, cur_it->t - cur_it->prev_t, &X_hat_, &P_);
- CHECK(X_hat_.allFinite());
+ AOS_CHECK(X_hat_.allFinite());
if (next_it != observations_.end()) {
next_it->X_hat = X_hat_;
next_it->P = P_;
diff --git a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
index 4c51569..8234b1c 100644
--- a/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
+++ b/frc971/control_loops/drivetrain/line_follow_drivetrain.cc
@@ -75,8 +75,8 @@
if (info != 0) {
// We allow a FATAL error here since this should only be called during
// initialization.
- LOG(FATAL, "Failed to solve %d, controllability: %d\n", info,
- controls::Controllability(A, B));
+ AOS_LOG(FATAL, "Failed to solve %d, controllability: %d\n", info,
+ controls::Controllability(A, B));
}
return K;
}
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 1f7e2b0..de59355 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -31,7 +31,7 @@
::aos::MutexLocker locker(&mutex_);
while (run_) {
while (goal_.spline.spline_idx == future_spline_idx_) {
- CHECK(!new_goal_.Wait());
+ AOS_CHECK(!new_goal_.Wait());
if (!run_) {
return;
}
diff --git a/frc971/control_loops/drivetrain/ssdrivetrain.cc b/frc971/control_loops/drivetrain/ssdrivetrain.cc
index 5330414..5924eb1 100644
--- a/frc971/control_loops/drivetrain/ssdrivetrain.cc
+++ b/frc971/control_loops/drivetrain/ssdrivetrain.cc
@@ -60,7 +60,7 @@
const Eigen::Matrix<double, 7, 1> error = kf_->R() - kf_->X_hat();
- LOG_MATRIX(DEBUG, "U_uncapped", *U);
+ AOS_LOG_MATRIX(DEBUG, "U_uncapped", *U);
Eigen::Matrix<double, 2, 2> position_K;
position_K << kf_->controller().K(0, 0), kf_->controller().K(0, 2),
@@ -75,8 +75,7 @@
const auto drive_error = T_inverse_ * position_error;
Eigen::Matrix<double, 2, 1> velocity_error;
velocity_error << error(1, 0), error(3, 0);
- LOG_MATRIX(DEBUG, "error", error);
-
+ AOS_LOG_MATRIX(DEBUG, "error", error);
Eigen::Matrix<double, 2, 1> U_integral;
U_integral << kf_->X_hat(4, 0), kf_->X_hat(5, 0);
@@ -139,7 +138,7 @@
if (!output_was_capped_) {
if ((*U - kf_->U_uncapped()).norm() > 0.0001) {
- LOG(FATAL, "U unnecessarily capped\n");
+ AOS_LOG(FATAL, "U unnecessarily capped\n");
}
}
}
@@ -228,7 +227,7 @@
// coordinates. Convert back...
linear_profile_.MoveCurrentState(dt_config_.LeftRightToLinear(kf_->R()));
- LOG(DEBUG, "Saturated while moving\n");
+ AOS_LOG(DEBUG, "Saturated while moving\n");
Eigen::Matrix<double, 2, 1> absolute_angular =
dt_config_.LeftRightToAngular(kf_->R());
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index ae51cb1..9d35a5e 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -86,8 +86,8 @@
// If we would end up with an imaginary number, cap us at 0 acceleration.
// TODO(austin): Investigate when this happens, why, and fix it.
if (squared < 0.0) {
- LOG(ERROR, "Imaginary %f, maxa: %f, fa(%f, %f) -> 0.0\n", squared, maxa, x,
- v);
+ AOS_LOG(ERROR, "Imaginary %f, maxa: %f, fa(%f, %f) -> 0.0\n", squared, maxa,
+ x, v);
return 0.0;
}
const double longitudal_acceleration =
@@ -148,8 +148,8 @@
// If we would end up with an imaginary number, cap us at 0 acceleration.
// TODO(austin): Investigate when this happens, why, and fix it.
if (squared < 0.0) {
- LOG(ERROR, "Imaginary %f, mina: %f, fa(%f, %f) -> 0.0\n", squared, mina, x,
- v);
+ AOS_LOG(ERROR, "Imaginary %f, mina: %f, fa(%f, %f) -> 0.0\n", squared, mina,
+ x, v);
return 0.0;
}
const double longitudal_acceleration =
@@ -221,7 +221,8 @@
acceleration = BackwardAcceleration(distance, velocity);
break;
default:
- LOG(FATAL, "Unknown segment type %d\n",
+ AOS_LOG(
+ FATAL, "Unknown segment type %d\n",
static_cast<int>(plan_segment_type_[DistanceToSegment(distance)]));
break;
}
@@ -229,7 +230,7 @@
if (vcurvature < velocity) {
velocity = vcurvature;
acceleration = 0.0;
- LOG(ERROR, "Curvature won\n");
+ AOS_LOG(ERROR, "Curvature won\n");
}
return (::Eigen::Matrix<double, 3, 1>() << distance, velocity, acceleration)
.finished();
@@ -327,23 +328,23 @@
int info = ::frc971::controls::dlqr<5, 2>(A, B, Q, R, &K, &S);
if (info == 0) {
- LOG_MATRIX(INFO, "K", K);
+ AOS_LOG_MATRIX(INFO, "K", K);
} else {
- LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
- controls::Controllability(A, B));
+ AOS_LOG(ERROR, "Failed to solve %d, controllability: %d\n", info,
+ controls::Controllability(A, B));
// TODO(austin): Can we be more clever here? Use the last one? We should
// collect more info about when this breaks down from logs.
K = ::Eigen::Matrix<double, 2, 5>::Zero();
}
::Eigen::EigenSolver<::Eigen::Matrix<double, 5, 5>> eigensolver(A - B * K);
const auto eigenvalues = eigensolver.eigenvalues();
- LOG(DEBUG,
- "Eigenvalues: (%f + %fj), (%f + %fj), (%f + %fj), (%f + %fj), (%f + "
- "%fj)\n",
- eigenvalues(0).real(), eigenvalues(0).imag(), eigenvalues(1).real(),
- eigenvalues(1).imag(), eigenvalues(2).real(), eigenvalues(2).imag(),
- eigenvalues(3).real(), eigenvalues(3).imag(), eigenvalues(4).real(),
- eigenvalues(4).imag());
+ AOS_LOG(DEBUG,
+ "Eigenvalues: (%f + %fj), (%f + %fj), (%f + %fj), (%f + %fj), (%f + "
+ "%fj)\n",
+ eigenvalues(0).real(), eigenvalues(0).imag(), eigenvalues(1).real(),
+ eigenvalues(1).imag(), eigenvalues(2).real(), eigenvalues(2).imag(),
+ eigenvalues(3).real(), eigenvalues(3).imag(), eigenvalues(4).real(),
+ eigenvalues(4).imag());
return K;
}
@@ -397,8 +398,8 @@
const double min_length = length() / static_cast<double>(plan_.size() - 1);
if (starting_distance > ending_distance) {
- LOG(FATAL, "End before start: %f > %f\n", starting_distance,
- ending_distance);
+ AOS_LOG(FATAL, "End before start: %f > %f\n", starting_distance,
+ ending_distance);
}
starting_distance = ::std::min(length(), ::std::max(0.0, starting_distance));
ending_distance = ::std::min(length(), ::std::max(0.0, ending_distance));
diff --git a/frc971/control_loops/drivetrain/trajectory_plot.cc b/frc971/control_loops/drivetrain/trajectory_plot.cc
index d2dd2c8..0b35ee8 100644
--- a/frc971/control_loops/drivetrain/trajectory_plot.cc
+++ b/frc971/control_loops/drivetrain/trajectory_plot.cc
@@ -97,10 +97,10 @@
::std::vector<double> backward_plan = trajectory.plan();
- LOG(INFO, "Took %fms to plan\n",
- chrono::duration_cast<chrono::duration<double, ::std::milli>>(end_time -
- start_time)
- .count());
+ AOS_LOG(INFO, "Took %fms to plan\n",
+ chrono::duration_cast<chrono::duration<double, ::std::milli>>(
+ end_time - start_time)
+ .count());
// Now, compute the xva as a function of time.
::std::vector<double> length_plan_t;
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index d69f7db..a6160e7 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -147,7 +147,7 @@
for (int i = 0; i < kNumInputs; ++i) {
if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
- LOG(FATAL, "U out of range\n");
+ AOS_LOG(FATAL, "U out of range\n");
}
}
}
diff --git a/frc971/control_loops/profiled_subsystem.h b/frc971/control_loops/profiled_subsystem.h
index 53c729d..58058c7 100644
--- a/frc971/control_loops/profiled_subsystem.h
+++ b/frc971/control_loops/profiled_subsystem.h
@@ -234,7 +234,7 @@
template <class ZeroingEstimator>
void SingleDOFProfiledSubsystem<ZeroingEstimator>::UpdateOffset(double offset) {
const double doffset = offset - offset_(0, 0);
- LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
+ AOS_LOG(INFO, "Adjusting offset from %f to %f\n", offset_(0, 0), offset);
this->loop_->mutable_X_hat()(0, 0) += doffset;
this->Y_(0, 0) += doffset;
@@ -282,7 +282,7 @@
this->estimators_[0].UpdateEstimate(new_position);
if (this->estimators_[0].error()) {
- LOG(ERROR, "zeroing error\n");
+ AOS_LOG(ERROR, "zeroing error\n");
return;
}
@@ -309,12 +309,12 @@
const char *name, Eigen::Matrix<double, 3, 1> *goal) {
// Limit the goal to min/max allowable positions.
if ((*goal)(0, 0) > range_.upper) {
- LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
- range_.upper);
+ AOS_LOG(WARNING, "Goal %s above limit, %f > %f\n", name, (*goal)(0, 0),
+ range_.upper);
(*goal)(0, 0) = range_.upper;
}
if ((*goal)(0, 0) < range_.lower) {
- LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
+ AOS_LOG(WARNING, "Goal %s below limit, %f < %f\n", name, (*goal)(0, 0),
range_.lower);
(*goal)(0, 0) = range_.lower;
}
@@ -373,7 +373,8 @@
// Returns whether hard limits have been exceeded.
if (position() > range_.upper_hard || position() < range_.lower_hard) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"SingleDOFProfiledSubsystem at %f out of bounds [%f, %f], ESTOPing\n",
position(), range_.lower_hard, range_.upper_hard);
return true;
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index d5861b7..81783cc 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -147,7 +147,7 @@
if (U(i, 0) > U_max(i, 0) + static_cast<Scalar>(0.00001) ||
U(i, 0) < U_min(i, 0) - static_cast<Scalar>(0.00001)) {
#if defined(__linux__)
- LOG(FATAL, "U out of range\n");
+ AOS_LOG(FATAL, "U out of range\n");
#else
abort();
#endif
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
index 7e8f4a7..748086f 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem.h
@@ -185,11 +185,11 @@
double safe_goal = goal->unsafe_goal;
if (safe_goal < min_position_) {
- LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
+ AOS_LOG(DEBUG, "Limiting to %f from %f\n", min_position_, safe_goal);
safe_goal = min_position_;
}
if (safe_goal > max_position_) {
- LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
+ AOS_LOG(DEBUG, "Limiting to %f from %f\n", max_position_, safe_goal);
safe_goal = max_position_;
}
profiled_subsystem_.set_unprofiled_goal(safe_goal);
@@ -197,7 +197,7 @@
} break;
case State::ESTOP:
- LOG(ERROR, "Estop\n");
+ AOS_LOG(ERROR, "Estop\n");
disabled = true;
break;
}
diff --git a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
index 58096f3..96cb782 100644
--- a/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
+++ b/frc971/control_loops/static_zeroing_single_dof_profiled_subsystem_test.cc
@@ -263,7 +263,7 @@
void RunIteration(const GoalType *unsafe_goal, const PositionType *position,
OutputType *output, StatusType *status) {
if (this->WasReset()) {
- LOG(ERROR, "WPILib reset, restarting\n");
+ AOS_LOG(ERROR, "WPILib reset, restarting\n");
subsystem_.Reset();
}
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index 38d194d..4fc2c7b 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -25,7 +25,7 @@
rx_clearer_.ClearRxFifo();
switch (spi_->Transaction(to_send, to_receive, size)) {
case -1:
- LOG(INFO, "SPI::Transaction of %zd bytes failed\n", size);
+ AOS_LOG(INFO, "SPI::Transaction of %zd bytes failed\n", size);
return false;
case size:
if (dummy_spi_) {
@@ -34,7 +34,7 @@
}
return true;
default:
- LOG(FATAL, "SPI::Transaction returned something weird\n");
+ AOS_LOG(FATAL, "SPI::Transaction returned something weird\n");
}
}
@@ -142,7 +142,7 @@
// NI's SPI driver defaults to SCHED_OTHER. Find it's PID with ps, and change
// it to a RT priority of 33.
- PCHECK(
+ AOS_PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
@@ -180,7 +180,7 @@
::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
}
}
- LOG(INFO, "IMU initialized successfully\n");
+ AOS_LOG(INFO, "IMU initialized successfully\n");
}
void ADIS16448::DoRun() {
@@ -201,7 +201,7 @@
const frc::InterruptableSensorBase::WaitResult result =
dio1_->WaitForInterrupt(0.1, !got_an_interrupt);
if (result == frc::InterruptableSensorBase::kTimeout) {
- LOG(WARNING, "IMU read timed out\n");
+ AOS_LOG(WARNING, "IMU read timed out\n");
InitializeUntilSuccessful();
continue;
}
@@ -221,7 +221,7 @@
// scenario.
if (!dio1_->Get() || dio1_->WaitForInterrupt(0, false) !=
frc::InterruptableSensorBase::kTimeout) {
- LOG(ERROR, "IMU read took too long\n");
+ AOS_LOG(ERROR, "IMU read took too long\n");
continue;
}
@@ -230,8 +230,9 @@
uint16_t received_crc =
to_receive[13 * 2 + 1] | (to_receive[13 * 2] << 8);
if (received_crc != calculated_crc) {
- LOG(WARNING, "received CRC %" PRIx16 " but calculated %" PRIx16 "\n",
- received_crc, calculated_crc);
+ AOS_LOG(WARNING,
+ "received CRC %" PRIx16 " but calculated %" PRIx16 "\n",
+ received_crc, calculated_crc);
InitializeUntilSuccessful();
continue;
}
@@ -274,9 +275,9 @@
gyro_x_zeroed_offset_ = -average_gyro_x.GetAverage();
gyro_y_zeroed_offset_ = -average_gyro_y.GetAverage();
gyro_z_zeroed_offset_ = -average_gyro_z.GetAverage();
- LOG(INFO, "total gyro zero offset X:%f, Y:%f, Z:%f\n",
- gyro_x_zeroed_offset_, gyro_y_zeroed_offset_,
- gyro_z_zeroed_offset_);
+ AOS_LOG(INFO, "total gyro zero offset X:%f, Y:%f, Z:%f\n",
+ gyro_x_zeroed_offset_, gyro_y_zeroed_offset_,
+ gyro_z_zeroed_offset_);
gyros_are_zeroed_ = true;
}
}
@@ -306,9 +307,9 @@
message->temperature =
ConvertValue(&to_receive[24], kTemperatureLsbDegree) + kTemperatureZero;
- LOG_STRUCT(DEBUG, "sending", *message);
+ AOS_LOG_STRUCT(DEBUG, "sending", *message);
if (!message.Send()) {
- LOG(WARNING, "sending queue message failed\n");
+ AOS_LOG(WARNING, "sending queue message failed\n");
}
spi_idle_callback_();
@@ -355,51 +356,51 @@
bool ADIS16448::CheckDiagStatValue(uint16_t value) const {
bool r = true;
if (value & (1 << 2)) {
- LOG(WARNING, "IMU gave flash update failure\n");
+ AOS_LOG(WARNING, "IMU gave flash update failure\n");
}
if (value & (1 << 3)) {
- LOG(WARNING, "IMU gave SPI communication failure\n");
+ AOS_LOG(WARNING, "IMU gave SPI communication failure\n");
}
if (value & (1 << 4)) {
- LOG(WARNING, "IMU gave sensor overrange\n");
+ AOS_LOG(WARNING, "IMU gave sensor overrange\n");
}
if (value & (1 << 5)) {
- LOG(WARNING, "IMU gave self-test failure\n");
+ AOS_LOG(WARNING, "IMU gave self-test failure\n");
r = false;
if (value & (1 << 10)) {
- LOG(WARNING, "IMU gave X-axis gyro self-test failure\n");
+ AOS_LOG(WARNING, "IMU gave X-axis gyro self-test failure\n");
}
if (value & (1 << 11)) {
- LOG(WARNING, "IMU gave Y-axis gyro self-test failure\n");
+ AOS_LOG(WARNING, "IMU gave Y-axis gyro self-test failure\n");
}
if (value & (1 << 12)) {
- LOG(WARNING, "IMU gave Z-axis gyro self-test failure\n");
+ AOS_LOG(WARNING, "IMU gave Z-axis gyro self-test failure\n");
}
if (value & (1 << 13)) {
- LOG(WARNING, "IMU gave X-axis accelerometer self-test failure\n");
+ AOS_LOG(WARNING, "IMU gave X-axis accelerometer self-test failure\n");
}
if (value & (1 << 14)) {
- LOG(WARNING, "IMU gave Y-axis accelerometer self-test failure\n");
+ AOS_LOG(WARNING, "IMU gave Y-axis accelerometer self-test failure\n");
}
if (value & (1 << 15)) {
- LOG(WARNING, "IMU gave Z-axis accelerometer self-test failure, %x\n",
- value);
+ AOS_LOG(WARNING, "IMU gave Z-axis accelerometer self-test failure, %x\n",
+ value);
}
if (value & (1 << 0)) {
- LOG(WARNING, "IMU gave magnetometer functional test failure\n");
+ AOS_LOG(WARNING, "IMU gave magnetometer functional test failure\n");
}
if (value & (1 << 1)) {
- LOG(WARNING, "IMU gave barometer functional test failure\n");
+ AOS_LOG(WARNING, "IMU gave barometer functional test failure\n");
}
}
if (value & (1 << 6)) {
- LOG(WARNING, "IMU gave flash test checksum failure\n");
+ AOS_LOG(WARNING, "IMU gave flash test checksum failure\n");
}
if (value & (1 << 8)) {
- LOG(WARNING, "IMU says alarm 1 is active\n");
+ AOS_LOG(WARNING, "IMU says alarm 1 is active\n");
}
if (value & (1 << 9)) {
- LOG(WARNING, "IMU says alarm 2 is active\n");
+ AOS_LOG(WARNING, "IMU says alarm 2 is active\n");
}
return r;
}
@@ -409,7 +410,7 @@
uint16_t product_id;
if (!ReadRegister(kLotId1Address, &product_id)) return false;
if (product_id != 0x4040) {
- LOG(ERROR, "product ID is %" PRIx16 " instead of 0x4040\n", product_id);
+ AOS_LOG(ERROR, "product ID is %" PRIx16 " instead of 0x4040\n", product_id);
return false;
}
@@ -417,8 +418,8 @@
if (!ReadRegister(kLotId2Address, &lot_id1)) return false;
if (!ReadRegister(kSerialNumberAddress, &lot_id2)) return false;
if (!ReadRegister(0, &serial_number)) return false;
- LOG(INFO, "have IMU %" PRIx16 "%" PRIx16 ": %" PRIx16 "\n", lot_id1, lot_id2,
- serial_number);
+ AOS_LOG(INFO, "have IMU %" PRIx16 "%" PRIx16 ": %" PRIx16 "\n", lot_id1,
+ lot_id2, serial_number);
// Divide the sampling by 2^2 = 4 to get 819.2 / 4 = 204.8 Hz.
if (!WriteRegister(kSmplPrdAddress, (2 << 8) | 1)) return false;
diff --git a/frc971/wpilib/buffered_pcm.cc b/frc971/wpilib/buffered_pcm.cc
index de8c547..3123add 100644
--- a/frc971/wpilib/buffered_pcm.cc
+++ b/frc971/wpilib/buffered_pcm.cc
@@ -16,7 +16,7 @@
solenoid_handles_[i] =
HAL_InitializeSolenoidPort(HAL_GetPortWithModule(module_, i), &status);
if (status != 0) {
- LOG(FATAL, "Status was %d\n", status);
+ AOS_LOG(FATAL, "Status was %d\n", status);
}
}
}
@@ -38,18 +38,18 @@
int32_t status = 0;
int32_t result = HAL_GetAllSolenoids(module_, &status);
if (status != 0) {
- LOG(ERROR, "Failed to flush, %d\n", status);
+ AOS_LOG(ERROR, "Failed to flush, %d\n", status);
return 0;
}
return result;
}
void BufferedPcm::Flush() {
- LOG(DEBUG, "sending solenoids 0x%" PRIx8 "\n", values_);
+ AOS_LOG(DEBUG, "sending solenoids 0x%" PRIx8 "\n", values_);
int32_t status = 0;
HAL_SetAllSolenoids(module_, static_cast<int>(values_), &status);
if (status != 0) {
- LOG(ERROR, "Failed to flush, %d\n", status);
+ AOS_LOG(ERROR, "Failed to flush, %d\n", status);
}
}
diff --git a/frc971/wpilib/dma_edge_counting.cc b/frc971/wpilib/dma_edge_counting.cc
index 9fffebe..1b7bee9 100644
--- a/frc971/wpilib/dma_edge_counting.cc
+++ b/frc971/wpilib/dma_edge_counting.cc
@@ -57,7 +57,7 @@
case DMA::STATUS_TIMEOUT:
return;
case DMA::STATUS_ERROR:
- LOG(WARNING, "DMA read failed\n");
+ AOS_LOG(WARNING, "DMA read failed\n");
break;
}
}
diff --git a/frc971/wpilib/drivetrain_writer.cc b/frc971/wpilib/drivetrain_writer.cc
index fdbc028..5c6feb8 100644
--- a/frc971/wpilib/drivetrain_writer.cc
+++ b/frc971/wpilib/drivetrain_writer.cc
@@ -12,7 +12,7 @@
void DrivetrainWriter::Write(
const ::frc971::control_loops::DrivetrainQueue::Output &output) {
- LOG_STRUCT(DEBUG, "will output", output);
+ AOS_LOG_STRUCT(DEBUG, "will output", output);
left_controller0_->SetSpeed(SafeSpeed(reversed_left0_, output.left_voltage));
right_controller0_->SetSpeed(
SafeSpeed(reversed_right0_, output.right_voltage));
@@ -28,7 +28,7 @@
}
void DrivetrainWriter::Stop() {
- LOG(WARNING, "drivetrain output too old\n");
+ AOS_LOG(WARNING, "drivetrain output too old\n");
left_controller0_->SetDisabled();
right_controller0_->SetDisabled();
diff --git a/frc971/wpilib/encoder_and_potentiometer.cc b/frc971/wpilib/encoder_and_potentiometer.cc
index 8fe8c25..ac1a4a0 100644
--- a/frc971/wpilib/encoder_and_potentiometer.cc
+++ b/frc971/wpilib/encoder_and_potentiometer.cc
@@ -21,10 +21,10 @@
}
void InterruptEncoderAndPotentiometer::Start() {
- CHECK_NE(nullptr, encoder_);
- CHECK_NE(nullptr, index_);
- CHECK_NE(nullptr, potentiometer_);
- CHECK_NE(0, priority_);
+ AOS_CHECK_NE(nullptr, encoder_);
+ AOS_CHECK_NE(nullptr, index_);
+ AOS_CHECK_NE(nullptr, potentiometer_);
+ AOS_CHECK_NE(0, priority_);
thread_ = ::std::thread(::std::ref(*this));
}
diff --git a/frc971/wpilib/gyro_interface.cc b/frc971/wpilib/gyro_interface.cc
index da0c01b..eeacfce 100644
--- a/frc971/wpilib/gyro_interface.cc
+++ b/frc971/wpilib/gyro_interface.cc
@@ -26,45 +26,47 @@
bool GyroInterface::InitializeGyro() {
uint32_t result;
if (!DoTransaction(0x20000003, &result)) {
- LOG(WARNING, "failed to start a self-check\n");
+ AOS_LOG(WARNING, "failed to start a self-check\n");
return false;
}
if (result != 1) {
// We might have hit a parity error or something and are now retrying, so
// this isn't a very big deal.
- LOG(INFO, "gyro unexpected initial response 0x%" PRIx32 "\n", result);
+ AOS_LOG(INFO, "gyro unexpected initial response 0x%" PRIx32 "\n", result);
}
// Wait for it to assert the fault conditions before reading them.
::std::this_thread::sleep_for(::std::chrono::milliseconds(50));
if (!DoTransaction(0x20000000, &result)) {
- LOG(WARNING, "failed to clear latched non-fault data\n");
+ AOS_LOG(WARNING, "failed to clear latched non-fault data\n");
return false;
}
- LOG(DEBUG, "gyro dummy response is 0x%" PRIx32 "\n", result);
+ AOS_LOG(DEBUG, "gyro dummy response is 0x%" PRIx32 "\n", result);
if (!DoTransaction(0x20000000, &result)) {
- LOG(WARNING, "failed to read self-test data\n");
+ AOS_LOG(WARNING, "failed to read self-test data\n");
return false;
}
if (ExtractStatus(result) != 2) {
- LOG(WARNING, "gyro first value 0x%" PRIx32 " not self-test data\n", result);
+ AOS_LOG(WARNING, "gyro first value 0x%" PRIx32 " not self-test data\n",
+ result);
return false;
}
if (ExtractErrors(result) != 0x7F) {
- LOG(WARNING, "gyro first value 0x%" PRIx32 " does not have all errors\n",
- result);
+ AOS_LOG(WARNING,
+ "gyro first value 0x%" PRIx32 " does not have all errors\n",
+ result);
return false;
}
if (!DoTransaction(0x20000000, &result)) {
- LOG(WARNING, "failed to clear latched self-test data\n");
+ AOS_LOG(WARNING, "failed to clear latched self-test data\n");
return false;
}
if (ExtractStatus(result) != 2) {
- LOG(WARNING, "gyro second value 0x%" PRIx32 " not self-test data\n",
- result);
+ AOS_LOG(WARNING, "gyro second value 0x%" PRIx32 " not self-test data\n",
+ result);
return false;
}
@@ -88,21 +90,21 @@
switch (gyro_->Transaction(to_send, to_receive, kBytes)) {
case -1:
- LOG(INFO, "SPI::Transaction failed\n");
+ AOS_LOG(INFO, "SPI::Transaction failed\n");
return false;
case kBytes:
break;
default:
- LOG(FATAL, "SPI::Transaction returned something weird\n");
+ AOS_LOG(FATAL, "SPI::Transaction returned something weird\n");
}
memcpy(result, to_receive, kBytes);
if (__builtin_parity(*result & 0xFFFF) != 1) {
- LOG(INFO, "high byte parity failure\n");
+ AOS_LOG(INFO, "high byte parity failure\n");
return false;
}
if (__builtin_parity(*result) != 1) {
- LOG(INFO, "whole value parity failure\n");
+ AOS_LOG(INFO, "whole value parity failure\n");
return false;
}
@@ -115,13 +117,14 @@
uint32_t response;
while (true) {
if (!DoTransaction(command, &response)) {
- LOG(WARNING, "reading 0x%" PRIx8 " failed\n", address);
+ AOS_LOG(WARNING, "reading 0x%" PRIx8 " failed\n", address);
continue;
}
if ((response & 0xEFE00000) != 0x4E000000) {
- LOG(WARNING, "gyro read from 0x%" PRIx8
- " gave unexpected response 0x%" PRIx32 "\n",
- address, response);
+ AOS_LOG(WARNING,
+ "gyro read from 0x%" PRIx8 " gave unexpected response 0x%" PRIx32
+ "\n",
+ address, response);
continue;
}
return (response >> 5) & 0xFFFF;
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index b1edbd2..3a9f220 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -32,7 +32,7 @@
gyro_reading_sender_(
event_loop_->MakeSender<::frc971::sensors::GyroReading>(
".frc971.sensors.gyro_reading")) {
- PCHECK(
+ AOS_PCHECK(
system("ps -ef | grep '\\[spi0\\]' | awk '{print $1}' | xargs chrt -f -p "
"33") == 0);
event_loop_->set_name("Gyro");
@@ -53,12 +53,12 @@
if (last_initialize_time_ + chrono::milliseconds(50) < monotonic_now) {
if (gyro_.InitializeGyro()) {
state_ = State::RUNNING;
- LOG(INFO, "gyro initialized successfully\n");
+ AOS_LOG(INFO, "gyro initialized successfully\n");
{
auto message = uid_sender_.MakeMessage();
message->uid = gyro_.ReadPartID();
- LOG_STRUCT(INFO, "gyro ID", *message);
+ AOS_LOG_STRUCT(INFO, "gyro ID", *message);
message.Send();
}
}
@@ -68,42 +68,42 @@
case State::RUNNING: {
const uint32_t result = gyro_.GetReading();
if (result == 0) {
- LOG(WARNING, "normal gyro read failed\n");
+ AOS_LOG(WARNING, "normal gyro read failed\n");
return;
}
switch (gyro_.ExtractStatus(result)) {
case 0:
- LOG(WARNING, "gyro says data is bad\n");
+ AOS_LOG(WARNING, "gyro says data is bad\n");
return;
case 1:
break;
default:
- LOG(WARNING, "gyro gave weird status 0x%" PRIx8 "\n",
- gyro_.ExtractStatus(result));
+ AOS_LOG(WARNING, "gyro gave weird status 0x%" PRIx8 "\n",
+ gyro_.ExtractStatus(result));
return;
}
if (gyro_.ExtractErrors(result) != 0) {
const uint8_t errors = gyro_.ExtractErrors(result);
if (errors & (1 << 6)) {
- LOG(WARNING, "gyro gave PLL error\n");
+ AOS_LOG(WARNING, "gyro gave PLL error\n");
}
if (errors & (1 << 5)) {
- LOG(WARNING, "gyro gave quadrature error\n");
+ AOS_LOG(WARNING, "gyro gave quadrature error\n");
}
if (errors & (1 << 4)) {
- LOG(WARNING, "gyro gave non-volatile memory error\n");
+ AOS_LOG(WARNING, "gyro gave non-volatile memory error\n");
}
if (errors & (1 << 3)) {
- LOG(WARNING, "gyro gave volatile memory error\n");
+ AOS_LOG(WARNING, "gyro gave volatile memory error\n");
}
if (errors & (1 << 2)) {
- LOG(WARNING, "gyro gave power error\n");
+ AOS_LOG(WARNING, "gyro gave power error\n");
}
if (errors & (1 << 1)) {
- LOG(WARNING, "gyro gave continuous self-test error\n");
+ AOS_LOG(WARNING, "gyro gave continuous self-test error\n");
}
if (errors & 1) {
- LOG(WARNING, "gyro gave unexpected self-test mode\n");
+ AOS_LOG(WARNING, "gyro gave unexpected self-test mode\n");
}
return;
}
@@ -120,7 +120,7 @@
angle_ += (new_angle + zero_offset_) * iterations;
message->angle = angle_;
message->velocity = angle_rate + zero_offset_ * kReadingRate;
- LOG_STRUCT(DEBUG, "sending", *message);
+ AOS_LOG_STRUCT(DEBUG, "sending", *message);
message.Send();
} else {
// TODO(brian): Don't break without 6 seconds of standing still before
@@ -130,7 +130,7 @@
{
message->angle = new_angle;
message->velocity = angle_rate;
- LOG_STRUCT(DEBUG, "collected while zeroing", *message);
+ AOS_LOG_STRUCT(DEBUG, "collected while zeroing", *message);
message->angle = 0.0;
message->velocity = 0.0;
message.Send();
@@ -141,7 +141,7 @@
if (joystick_state_fetcher_.get() && joystick_state_fetcher_->enabled &&
zeroing_data_.full()) {
zero_offset_ = -zeroing_data_.GetAverage();
- LOG(INFO, "total zero offset %f\n", zero_offset_);
+ AOS_LOG(INFO, "total zero offset %f\n", zero_offset_);
zeroed_ = true;
}
}
diff --git a/frc971/wpilib/interrupt_edge_counting.cc b/frc971/wpilib/interrupt_edge_counting.cc
index a06df8f..546e64b 100644
--- a/frc971/wpilib/interrupt_edge_counting.cc
+++ b/frc971/wpilib/interrupt_edge_counting.cc
@@ -55,8 +55,8 @@
}
current_value_ = hall_value;
} else {
- LOG(WARNING, "Detected spurious edge on %d. Dropping it.\n",
- input_->GetChannel());
+ AOS_LOG(WARNING, "Detected spurious edge on %d. Dropping it.\n",
+ input_->GetChannel());
}
}
}
@@ -93,7 +93,7 @@
::std::unique_lock<::aos::stl_mutex> mutex_guard(mutex_);
for (auto &c : handlers_) {
if (c->interrupt_count_changed()) {
- LOG(WARNING, "got an interrupt while sampling. retrying\n");
+ AOS_LOG(WARNING, "got an interrupt while sampling. retrying\n");
return false;
}
}
diff --git a/frc971/wpilib/interrupt_edge_counting.h b/frc971/wpilib/interrupt_edge_counting.h
index 50dc94d..160682c 100644
--- a/frc971/wpilib/interrupt_edge_counting.h
+++ b/frc971/wpilib/interrupt_edge_counting.h
@@ -46,8 +46,8 @@
// Starts the thread running.
// set_priority and set_mutex must be called first.
void Start() {
- CHECK_NE(nullptr, mutex_);
- CHECK_NE(0, priority_);
+ AOS_CHECK_NE(nullptr, mutex_);
+ AOS_CHECK_NE(0, priority_);
thread_ = ::std::thread(::std::ref(*this));
}
diff --git a/frc971/wpilib/joystick_sender.cc b/frc971/wpilib/joystick_sender.cc
index a073066..31ecc6c 100644
--- a/frc971/wpilib/joystick_sender.cc
+++ b/frc971/wpilib/joystick_sender.cc
@@ -54,10 +54,10 @@
if (ds->GetStickPOVCount(i) > 0) {
new_state->joysticks[i].pov = ds->GetStickPOV(i, 0);
}
- LOG_STRUCT(DEBUG, "joystick_state", *new_state);
+ AOS_LOG_STRUCT(DEBUG, "joystick_state", *new_state);
}
if (!new_state.Send()) {
- LOG(WARNING, "sending joystick_state failed\n");
+ AOS_LOG(WARNING, "sending joystick_state failed\n");
}
});
}
diff --git a/frc971/wpilib/loop_output_handler_test.cc b/frc971/wpilib/loop_output_handler_test.cc
index 8a7e903..12c3f96 100644
--- a/frc971/wpilib/loop_output_handler_test.cc
+++ b/frc971/wpilib/loop_output_handler_test.cc
@@ -49,14 +49,14 @@
protected:
void Write(const LoopOutputHandlerTestOutput &output) override {
- LOG_STRUCT(DEBUG, "output", output);
+ AOS_LOG_STRUCT(DEBUG, "output", output);
++count_;
last_time_ = event_loop()->monotonic_now();
}
void Stop() override {
stop_time_ = event_loop()->monotonic_now();
- LOG(DEBUG, "Stopping\n");
+ AOS_LOG(DEBUG, "Stopping\n");
}
private:
@@ -92,7 +92,7 @@
++count;
}
- LOG(INFO, "Ping\n");
+ AOS_LOG(INFO, "Ping\n");
});
// Kick off the ping timer handler.
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index 98da761..aa85184 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -29,7 +29,7 @@
void PDPFetcher::Loop(int iterations) {
if (iterations != 1) {
- LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
+ AOS_LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
}
auto message = pdp_values_sender_.MakeMessage();
message->voltage = pdp_->GetVoltage();
@@ -38,9 +38,9 @@
for (int i = 0; i < 16; ++i) {
message->currents[i] = pdp_->GetCurrent(i);
}
- LOG_STRUCT(DEBUG, "got", *message);
+ AOS_LOG_STRUCT(DEBUG, "got", *message);
if (!message.Send()) {
- LOG(WARNING, "sending pdp values failed\n");
+ AOS_LOG(WARNING, "sending pdp values failed\n");
}
}
diff --git a/frc971/wpilib/sensor_reader.cc b/frc971/wpilib/sensor_reader.cc
index 209054b..63d3992 100644
--- a/frc971/wpilib/sensor_reader.cc
+++ b/frc971/wpilib/sensor_reader.cc
@@ -102,9 +102,9 @@
period_ =
pwm_trigger_ ? chrono::microseconds(5050) : chrono::microseconds(5000);
if (pwm_trigger_) {
- LOG(INFO, "Using PWM trigger and a 5.05 ms period\n");
+ AOS_LOG(INFO, "Using PWM trigger and a 5.05 ms period\n");
} else {
- LOG(INFO, "Defaulting to open loop pwm synchronization\n");
+ AOS_LOG(INFO, "Defaulting to open loop pwm synchronization\n");
}
// Now that we are configured, actually fill in the defaults.
@@ -117,7 +117,7 @@
void SensorReader::Loop(const int iterations) {
if (iterations != 1) {
- LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
+ AOS_LOG(WARNING, "SensorReader skipped %d iterations\n", iterations - 1);
}
const monotonic_clock::time_point monotonic_now =
@@ -126,7 +126,7 @@
{
auto new_state = robot_state_sender_.MakeMessage();
::frc971::wpilib::PopulateRobotState(new_state.get(), my_pid_);
- LOG_STRUCT(DEBUG, "robot_state", *new_state);
+ AOS_LOG_STRUCT(DEBUG, "robot_state", *new_state);
new_state.Send();
}
RunIteration();
@@ -136,8 +136,8 @@
}
if (pwm_trigger_) {
- LOG(DEBUG, "PWM wakeup delta: %lld\n",
- (monotonic_now - last_monotonic_now_).count());
+ AOS_LOG(DEBUG, "PWM wakeup delta: %lld\n",
+ (monotonic_now - last_monotonic_now_).count());
last_monotonic_now_ = monotonic_now;
monotonic_clock::time_point last_tick_timepoint = GetPWMStartTime();
diff --git a/frc971/wpilib/spi_rx_clearer.cc b/frc971/wpilib/spi_rx_clearer.cc
index dfe7e8b..245f073 100644
--- a/frc971/wpilib/spi_rx_clearer.cc
+++ b/frc971/wpilib/spi_rx_clearer.cc
@@ -11,18 +11,18 @@
namespace wpilib {
SpiRxClearer::SpiRxClearer() {
- const int fd = PCHECK(open("/dev/mem", O_RDWR));
+ const int fd = AOS_PCHECK(open("/dev/mem", O_RDWR));
void *const mmap_result = mmap(nullptr, kMappingSize, PROT_READ | PROT_WRITE,
MAP_SHARED, fd, spi_peripheral_base_);
if (mmap_result == MAP_FAILED) {
- PLOG(FATAL, "mmap the SPI peripheral from /dev/mem failed\n");
+ AOS_PLOG(FATAL, "mmap the SPI peripheral from /dev/mem failed\n");
}
- PCHECK(close(fd));
+ AOS_PCHECK(close(fd));
mapping_ = static_cast<volatile uint32_t *>(mmap_result);
}
SpiRxClearer::~SpiRxClearer() {
- PCHECK(munmap(const_cast<uint32_t *>(mapping_), kMappingSize));
+ AOS_PCHECK(munmap(const_cast<uint32_t *>(mapping_), kMappingSize));
}
void SpiRxClearer::ClearRxFifo() {
@@ -34,9 +34,9 @@
return;
}
// Read the next byte.
- LOG(DEBUG, "Read from RX FIFO: %" PRIx32 "\n", ReadRegister(0x20));
+ AOS_LOG(DEBUG, "Read from RX FIFO: %" PRIx32 "\n", ReadRegister(0x20));
}
- LOG(FATAL, "Failed to clear the RX FIFO\n");
+ AOS_LOG(FATAL, "Failed to clear the RX FIFO\n");
}
} // namespace wpilib
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 851d809..4216e3b 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -23,7 +23,7 @@
robot_state->voltage_battery = HAL_GetVinVoltage(&status);
if (status != 0) {
- LOG(FATAL, "Failed to get robot state: %d\n", status);
+ AOS_LOG(FATAL, "Failed to get robot state: %d\n", status);
}
}
diff --git a/frc971/wpilib/wpilib_robot_base.h b/frc971/wpilib/wpilib_robot_base.h
index ef3ec31..86672c4 100644
--- a/frc971/wpilib/wpilib_robot_base.h
+++ b/frc971/wpilib/wpilib_robot_base.h
@@ -28,7 +28,7 @@
thread.join();
}
- LOG(ERROR, "Exiting WPILibRobot\n");
+ AOS_LOG(ERROR, "Exiting WPILibRobot\n");
::aos::Cleanup();
}
diff --git a/frc971/zeroing/zeroing.cc b/frc971/zeroing/zeroing.cc
index 1ed98a3..1399efd 100644
--- a/frc971/zeroing/zeroing.cc
+++ b/frc971/zeroing/zeroing.cc
@@ -30,7 +30,7 @@
void PotAndIndexPulseZeroingEstimator::TriggerError() {
if (!error_) {
- LOG(ERROR, "Manually triggered zeroing error.\n");
+ AOS_LOG(ERROR, "Manually triggered zeroing error.\n");
error_ = true;
}
}
@@ -98,7 +98,7 @@
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = offset_;
- LOG(INFO, "latching start position %f\n", first_start_pos_);
+ AOS_LOG(INFO, "latching start position %f\n", first_start_pos_);
}
// Now that we have an accurate starting position we can consider ourselves
@@ -109,7 +109,8 @@
if (::std::abs(first_start_pos_ - offset_) >
constants_.allowable_encoder_error * constants_.index_difference) {
if (!error_) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Encoder ticks out of range since last index pulse. first start "
"position: %f recent starting position: %f, allowable error: %f\n",
first_start_pos_, offset_,
@@ -156,7 +157,7 @@
void HallEffectAndPositionZeroingEstimator::TriggerError() {
if (!error_) {
- LOG(ERROR, "Manually triggered zeroing error.\n");
+ AOS_LOG(ERROR, "Manually triggered zeroing error.\n");
error_ = true;
}
}
@@ -224,7 +225,7 @@
// Save the first starting position.
if (!zeroed_) {
first_start_pos_ = offset_;
- LOG(INFO, "latching start position %f\n", first_start_pos_);
+ AOS_LOG(INFO, "latching start position %f\n", first_start_pos_);
}
// Now that we have an accurate starting position we can consider ourselves
@@ -292,12 +293,12 @@
// code below. NaN values are given when the Absolute Encoder is disconnected.
if (::std::isnan(info.absolute_encoder)) {
if (zeroed_) {
- LOG(ERROR, "NAN on absolute encoder\n");
+ AOS_LOG(ERROR, "NAN on absolute encoder\n");
error_ = true;
} else {
++nan_samples_;
- LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
- static_cast<int>(nan_samples_));
+ AOS_LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
+ static_cast<int>(nan_samples_));
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
@@ -401,11 +402,13 @@
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Offset moved too far. Initial: %f, current %f, allowable change: "
"%f\n",
- first_offset_, offset_, constants_.allowable_encoder_error *
- constants_.one_revolution_distance);
+ first_offset_, offset_,
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance);
error_ = true;
}
@@ -471,8 +474,9 @@
const int index_pulse_count = IndexPulseCount();
if (index_pulse_count > constants_.index_pulse_count) {
if (!error_) {
- LOG(ERROR, "Got more index pulses than expected. Got %d expected %d.\n",
- index_pulse_count, constants_.index_pulse_count);
+ AOS_LOG(ERROR,
+ "Got more index pulses than expected. Got %d expected %d.\n",
+ index_pulse_count, constants_.index_pulse_count);
error_ = true;
}
}
@@ -502,15 +506,15 @@
// This lets us check if the index pulse is within an acceptable error
// margin of where we expected it to be.
if (::std::abs(error) > constants_.allowable_encoder_error) {
- LOG(ERROR,
- "Encoder ticks out of range since last index pulse. known index "
- "pulse: %f, expected index pulse: %f, actual index pulse: %f, "
- "allowable error: %f\n",
- constants_.measured_index_position,
- round(relative_distance) * constants_.index_difference +
+ AOS_LOG(ERROR,
+ "Encoder ticks out of range since last index pulse. known index "
+ "pulse: %f, expected index pulse: %f, actual index pulse: %f, "
+ "allowable error: %f\n",
constants_.measured_index_position,
- info.latched_encoder + offset_,
- constants_.allowable_encoder_error * constants_.index_difference);
+ round(relative_distance) * constants_.index_difference +
+ constants_.measured_index_position,
+ info.latched_encoder + offset_,
+ constants_.allowable_encoder_error * constants_.index_difference);
error_ = true;
}
}
@@ -568,12 +572,12 @@
// code below. NaN values are given when the Absolute Encoder is disconnected.
if (::std::isnan(info.absolute_encoder)) {
if (zeroed_) {
- LOG(ERROR, "NAN on absolute encoder\n");
+ AOS_LOG(ERROR, "NAN on absolute encoder\n");
error_ = true;
} else {
++nan_samples_;
- LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
- static_cast<int>(nan_samples_));
+ AOS_LOG(ERROR, "NAN on absolute encoder while zeroing %d\n",
+ static_cast<int>(nan_samples_));
if (nan_samples_ >= constants_.average_filter_size) {
error_ = true;
zeroed_ = true;
@@ -669,11 +673,13 @@
if (::std::abs(first_offset_ - offset_) >
constants_.allowable_encoder_error *
constants_.one_revolution_distance) {
- LOG(ERROR,
+ AOS_LOG(
+ ERROR,
"Offset moved too far. Initial: %f, current %f, allowable change: "
"%f\n",
- first_offset_, offset_, constants_.allowable_encoder_error *
- constants_.one_revolution_distance);
+ first_offset_, offset_,
+ constants_.allowable_encoder_error *
+ constants_.one_revolution_distance);
error_ = true;
}