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_;
diff --git a/y2022/control_loops/drivetrain/drivetrain_base.cc b/y2022/control_loops/drivetrain/drivetrain_base.cc
index cbc9c42..93233d2 100644
--- a/y2022/control_loops/drivetrain/drivetrain_base.cc
+++ b/y2022/control_loops/drivetrain/drivetrain_base.cc
@@ -25,7 +25,7 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::SIMPLE_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
::frc971::control_loops::drivetrain::IMUType::IMU_FLIPPED_X,
drivetrain::MakeDrivetrainLoop,
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
index bbf0324..3332c1e 100644
--- a/y2022/control_loops/localizer/BUILD
+++ b/y2022/control_loops/localizer/BUILD
@@ -60,7 +60,6 @@
"//frc971/control_loops:c2d",
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops/drivetrain:drivetrain_output_fbs",
- "//frc971/control_loops/drivetrain:drivetrain_position_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_fbs",
"//frc971/control_loops/drivetrain:improved_down_estimator",
"//frc971/control_loops/drivetrain:localizer_fbs",
diff --git a/y2022/control_loops/localizer/localizer.cc b/y2022/control_loops/localizer/localizer.cc
index a145ad4..d6afd56 100644
--- a/y2022/control_loops/localizer/localizer.cc
+++ b/y2022/control_loops/localizer/localizer.cc
@@ -507,9 +507,6 @@
model_based_(dt_config),
status_sender_(event_loop_->MakeSender<LocalizerStatus>("/localizer")),
output_sender_(event_loop_->MakeSender<LocalizerOutput>("/localizer")),
- position_fetcher_(
- event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Position>(
- "/localizer")),
output_fetcher_(
event_loop_->MakeFetcher<frc971::control_loops::drivetrain::Output>(
"/drivetrain")) {
@@ -526,26 +523,24 @@
event_loop_->MakeWatcher(
"/localizer", [this](const frc971::IMUValuesBatch &values) {
CHECK(values.has_readings());
- position_fetcher_.Fetch();
output_fetcher_.Fetch();
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
if (zeroer_.Zeroed()) {
- CHECK(position_fetcher_.get() != nullptr);
- CHECK(output_fetcher_.get() != nullptr);
- const bool disabled =
- output_fetcher_.context().monotonic_event_time +
- std::chrono::milliseconds(10) <
- event_loop_->context().monotonic_event_time;
- model_based_.HandleImu(
- aos::monotonic_clock::time_point(
- std::chrono::nanoseconds(value->monotonic_timestamp_ns())),
- zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
- {position_fetcher_->left_encoder(),
- position_fetcher_->right_encoder()},
- disabled ? Eigen::Vector2d::Zero()
- : Eigen::Vector2d{output_fetcher_->left_voltage(),
- output_fetcher_->right_voltage()});
+ if (output_fetcher_.get() != nullptr) {
+ const bool disabled =
+ output_fetcher_.context().monotonic_event_time +
+ std::chrono::milliseconds(10) <
+ event_loop_->context().monotonic_event_time;
+ model_based_.HandleImu(
+ aos::monotonic_clock::time_point(std::chrono::nanoseconds(
+ value->monotonic_timestamp_ns())),
+ zeroer_.ZeroedGyro(), zeroer_.ZeroedAccel(),
+ {value->left_encoder(), value->right_encoder()},
+ disabled ? Eigen::Vector2d::Zero()
+ : Eigen::Vector2d{output_fetcher_->left_voltage(),
+ output_fetcher_->right_voltage()});
+ }
}
{
auto builder = status_sender_.MakeBuilder();
diff --git a/y2022/control_loops/localizer/localizer.h b/y2022/control_loops/localizer/localizer.h
index f2d76ef..3a09b28 100644
--- a/y2022/control_loops/localizer/localizer.h
+++ b/y2022/control_loops/localizer/localizer.h
@@ -11,7 +11,6 @@
#include "y2022/control_loops/localizer/localizer_status_generated.h"
#include "y2022/control_loops/localizer/localizer_output_generated.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
-#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_output_generated.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
@@ -208,7 +207,6 @@
ModelBasedLocalizer model_based_;
aos::Sender<LocalizerStatus> status_sender_;
aos::Sender<LocalizerOutput> output_sender_;
- aos::Fetcher<frc971::control_loops::drivetrain::Position> position_fetcher_;
aos::Fetcher<frc971::control_loops::drivetrain::Output> output_fetcher_;
zeroing::ImuZeroer zeroer_;
aos::monotonic_clock::time_point last_output_send_ =
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index db7ec8f..eb76e00 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -18,7 +18,7 @@
Imu::Imu(aos::ShmEventLoop *event_loop)
: event_loop_(event_loop),
imu_sender_(
- event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain")) {
+ event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")) {
event_loop->SetRuntimeRealtimePriority(30);
imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
index da0e4cb..793d31a 100644
--- a/y2022/y2022_imu.json
+++ b/y2022/y2022_imu.json
@@ -199,6 +199,9 @@
"frequency": 2200,
"max_size": 2000,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
"destination_nodes": [
{
"name": "logger",
@@ -227,6 +230,9 @@
"frequency": 200,
"max_size": 200,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio"
+ ],
"destination_nodes": [
{
"name": "roborio",
@@ -271,9 +277,12 @@
"type": "frc971.IMUValuesBatch",
"source_node": "imu",
"frequency": 2200,
- "max_size": 1000,
+ "max_size": 1600,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "logger"
+ ],
"destination_nodes": [
{
"name": "logger",
@@ -294,35 +303,6 @@
"frequency": 2200,
"num_senders": 2,
"max_size": 200
- },
- {
- "name": "/localizer",
- "type": "frc971.control_loops.drivetrain.Position",
- "source_node": "imu",
- "frequency": 200,
- "max_size": 200,
- "num_senders": 2,
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 5,
- "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_nodes": [
- "imu"
- ],
- "time_to_live": 5000000
- }
- ]
- },
- {
- "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-control_loops-drivetrain-Position",
- "type": "aos.message_bridge.RemoteMessage",
- "source_node": "imu",
- "logger": "NOT_LOGGED",
- "frequency": 200,
- "num_senders": 2,
- "max_size": 200
}
],
"applications": [
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 67f2822..592109e 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -20,22 +20,6 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValuesBatch",
- "source_node": "roborio",
- "logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_nodes": [
- "logger"
- ],
- "destination_nodes": [
- {
- "name": "logger",
- "priority": 2,
- "time_to_live": 500000000
- }
- ]
- },
- {
- "name": "/drivetrain",
"type": "frc971.control_loops.drivetrain.Position",
"source_node": "roborio",
"logger": "LOCAL_AND_REMOTE_LOGGER",
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 41f5baa..cb1b2d8 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -220,7 +220,18 @@
"type": "y2022.control_loops.superstructure.Status",
"source_node": "roborio",
"frequency": 200,
- "num_senders": 2
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
},
{
"name": "/superstructure",
@@ -240,14 +251,6 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValuesBatch",
- "source_node": "roborio",
- "frequency": 250,
- "max_size": 2000,
- "num_senders": 2
- },
- {
- "name": "/drivetrain",
"type": "frc971.sensors.GyroReading",
"source_node": "roborio",
"frequency": 200,
@@ -300,6 +303,9 @@
"max_size": 80,
"num_senders": 2,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
"destination_nodes": [
{
"name": "imu",
@@ -336,6 +342,9 @@
"frequency": 200,
"max_size": 96,
"logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "imu"
+ ],
"destination_nodes": [
{
"name": "imu",
@@ -344,7 +353,7 @@
"timestamp_logger_nodes": [
"imu"
],
- "time_to_live": 5000000
+ "time_to_live": 0
}
]
},