Remove unused 2022 channels
IMUValuesBatch on the roborio is unused, I erroneously added a drivetrain
Position message on the imu node earlier, and the drivetrain test
library needed to be updated to support the encoders in the IMUValues
message (as well as the GyroReadings message).
Also, begin forwarding superstructure status message to the IMU pi, and
set the LocalizerControl message to be reliable.
Change-Id: I18f1b4131710e0539018002e91fce99f7f1215b4
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 62b77c4..767fb61 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -35,7 +35,7 @@
localizer_control_fetcher_(
event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
- event_loop->MakeFetcher<::frc971::IMUValuesBatch>("/drivetrain")),
+ event_loop->TryMakeFetcher<::frc971::IMUValuesBatch>("/drivetrain")),
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
"/drivetrain")),
@@ -52,7 +52,9 @@
event_loop->OnRun([this]() {
// On the first fetch, make sure that we are caught all the way up to the
// present.
- imu_values_fetcher_.Fetch();
+ if (imu_values_fetcher_.valid()) {
+ imu_values_fetcher_.Fetch();
+ }
});
if (dt_config.is_simulated) {
down_estimator_.assume_perfect_gravity();
@@ -117,7 +119,7 @@
break;
}
- while (imu_values_fetcher_.FetchNext()) {
+ while (imu_values_fetcher_.valid() && imu_values_fetcher_.FetchNext()) {
CHECK(imu_values_fetcher_->has_readings());
last_gyro_time_ = monotonic_now;
for (const IMUValues *value : *imu_values_fetcher_->readings()) {
@@ -138,7 +140,7 @@
}
bool got_imu_reading = false;
- if (imu_values_fetcher_.get() != nullptr) {
+ if (imu_values_fetcher_.valid() && imu_values_fetcher_.get() != nullptr) {
imu_zeroer_.ProcessMeasurements();
got_imu_reading = true;
CHECK(imu_values_fetcher_->has_readings());
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 27f1552..a973552 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -114,7 +114,7 @@
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
imu_sender_(
- event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
+ event_loop->TryMakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -126,13 +126,13 @@
HybridKalman<2, 2, 2>>(
dt_config_.make_hybrid_drivetrain_velocity_loop()))) {
if (imu_event_loop_ != nullptr) {
- localizer_position_sender_ =
- imu_event_loop_
- ->MakeSender<::frc971::control_loops::drivetrain::Position>(
- "/localizer");
- localizer_imu_sender_ =
+ CHECK(!imu_sender_);
+ imu_sender_ =
imu_event_loop_->MakeSender<::frc971::IMUValuesBatch>("/localizer");
+ gyro_sender_ =
+ event_loop_->MakeSender<::frc971::sensors::GyroReading>("/drivetrain");
}
+ CHECK(imu_sender_);
Reinitialize();
last_U_.setZero();
event_loop_->AddPhasedLoop(
@@ -202,10 +202,6 @@
position(fbb.Release());
CHECK_EQ(drivetrain_position_sender_.Send(position),
aos::RawSender::Error::kOk);
- if (localizer_position_sender_) {
- CHECK_EQ(localizer_position_sender_.Send(position),
- aos::RawSender::Error::kOk);
- }
}
}
@@ -231,8 +227,10 @@
std::chrono::duration_cast<std::chrono::nanoseconds>(
event_loop_->monotonic_now().time_since_epoch())
.count();
+ last_yaw_rate_ = gyro.z();
imu_readings_.push({.gyro = gyro,
.accel = accel,
+ .encoders = {GetLeftPosition(), GetRightPosition()},
.timestamp = timestamp,
.faulted = imu_faulted_});
}
@@ -273,6 +271,14 @@
imu_builder.add_accelerometer_z(imu_reading.accel.z());
imu_builder.add_monotonic_timestamp_ns(imu_reading.timestamp);
+ if (imu_event_loop_ != nullptr) {
+ imu_builder.add_pico_timestamp_us(imu_reading.timestamp / 1000);
+ imu_builder.add_data_counter(imu_data_counter_++);
+ imu_builder.add_checksum_failed(false);
+ imu_builder.add_left_encoder(imu_reading.encoders(0));
+ imu_builder.add_right_encoder(imu_reading.encoders(1));
+ }
+
imu_values.push_back(imu_builder.Finish());
}
@@ -284,8 +290,14 @@
fbb.Finish(imu_values_batch_builder.Finish());
aos::FlatbufferDetachedBuffer<frc971::IMUValuesBatch> message = fbb.Release();
CHECK_EQ(imu_sender_.Send(message), aos::RawSender::Error::kOk);
- if (localizer_imu_sender_) {
- CHECK_EQ(localizer_imu_sender_.Send(message), aos::RawSender::Error::kOk);
+ if (gyro_sender_) {
+ auto builder = gyro_sender_.MakeBuilder();
+ sensors::GyroReading::Builder reading_builder =
+ builder.MakeBuilder<sensors::GyroReading>();
+ reading_builder.add_angle(state_(2));
+ reading_builder.add_velocity(last_yaw_rate_);
+ CHECK_EQ(builder.Send(reading_builder.Finish()),
+ aos::RawSender::Error::kOk);
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index d7c0e53..356eadf 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
#include "frc971/wpilib/imu_batch_generated.h"
+#include "frc971/queues/gyro_generated.h"
namespace frc971 {
namespace control_loops {
@@ -101,6 +102,9 @@
struct ImuReading {
Eigen::Vector3d gyro;
Eigen::Vector3d accel;
+ // On the 2022 robot, encoders are read as part of the same procedure that
+ // reads the IMU.
+ Eigen::Vector2d encoders;
int64_t timestamp;
bool faulted;
};
@@ -123,27 +127,19 @@
::aos::Sender<::frc971::control_loops::drivetrain::Position>
drivetrain_position_sender_;
- // Duplicate Position sender to be sent from the imu pi, for robots that
- // support it.
- // TODO(james): Update this to match what Ravago did for the IMU--either
- // update that library, or update this one.
- ::aos::Sender<::frc971::control_loops::drivetrain::Position>
- localizer_position_sender_;
::aos::Sender<::frc971::control_loops::drivetrain::Status>
drivetrain_truth_sender_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Output>
drivetrain_output_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- // TODO(james): Disable this on non-IMU roborios.
::aos::Sender<::frc971::IMUValuesBatch> imu_sender_;
- // Duplicate IMUValues sender to be sent from the imu pi, for robots that
- // support it.
- // TODO(james): Also, add a roborio-based GyroReading sender.
- ::aos::Sender<::frc971::IMUValuesBatch> localizer_imu_sender_;
+ ::aos::Sender<::frc971::sensors::GyroReading> gyro_sender_;
bool imu_faulted_ = false;
+ double last_yaw_rate_ = 0.0;
+ int imu_data_counter_ = 0;
std::queue<ImuReading> imu_readings_;
DrivetrainConfig<double> dt_config_;