Switch to a batch IMU message
We only send in batches. We would also like to forward these messages
across the network to the logger. The resulting 2000 hz wakeups are
swamping message_gateway, and it isn't worth adding a batch forwarding
mode yet.
Change-Id: I2c2f5bc021a660e932e086965e10bc92f27e2ec1
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 8c81440..ac2a2e8 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -134,6 +134,7 @@
"//frc971/queues:gyro_fbs",
"//frc971/queues:gyro_uid_fbs",
"//frc971/wpilib:imu_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
],
visibility = ["//visibility:public"],
deps = [
@@ -413,6 +414,7 @@
"//aos/util:log_interval",
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//frc971/zeroing:imu_zeroer",
],
@@ -443,6 +445,7 @@
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro_fbs",
"//frc971/wpilib:imu_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
] + cpu_select({
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 1c0ac4f..a822ac4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -18,7 +18,7 @@
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/shifter_hall_effect.h"
-#include "frc971/wpilib/imu_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -37,7 +37,7 @@
localizer_control_fetcher_(
event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
- event_loop->MakeFetcher<::frc971::IMUValues>("/drivetrain")),
+ event_loop->MakeFetcher<::frc971::IMUValuesBatch>("/drivetrain")),
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
"/drivetrain")),
@@ -149,37 +149,44 @@
}
while (imu_values_fetcher_.FetchNext()) {
- imu_zeroer_.InsertMeasurement(*imu_values_fetcher_);
+ CHECK(imu_values_fetcher_->has_readings());
last_gyro_time_ = monotonic_now;
- if (!imu_zeroer_.Zeroed()) {
- continue;
- }
- aos::monotonic_clock::time_point reading_time(std::chrono::nanoseconds(
- imu_values_fetcher_->monotonic_timestamp_ns()));
- if (last_imu_update_ == aos::monotonic_clock::min_time) {
+ for (const IMUValues *value : *imu_values_fetcher_->readings()) {
+ imu_zeroer_.InsertMeasurement(*value);
+ if (!imu_zeroer_.Zeroed()) {
+ continue;
+ }
+ const aos::monotonic_clock::time_point reading_time(
+ std::chrono::nanoseconds(value->monotonic_timestamp_ns()));
+ if (last_imu_update_ == aos::monotonic_clock::min_time) {
+ last_imu_update_ = reading_time;
+ }
+ down_estimator_.Predict(imu_zeroer_.ZeroedGyro(),
+ imu_zeroer_.ZeroedAccel(),
+ reading_time - last_imu_update_);
last_imu_update_ = reading_time;
}
- down_estimator_.Predict(imu_zeroer_.ZeroedGyro(), imu_zeroer_.ZeroedAccel(),
- reading_time - last_imu_update_);
- last_imu_update_ = reading_time;
}
bool got_imu_reading = false;
if (imu_values_fetcher_.get() != nullptr) {
imu_zeroer_.ProcessMeasurements();
got_imu_reading = true;
+ CHECK(imu_values_fetcher_->has_readings());
+ const IMUValues *value = imu_values_fetcher_->readings()->Get(
+ imu_values_fetcher_->readings()->size() - 1);
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
- last_accel_ = -imu_values_fetcher_->accelerometer_x();
+ last_accel_ = -value->accelerometer_x();
break;
case IMUType::IMU_FLIPPED_X:
- last_accel_ = imu_values_fetcher_->accelerometer_x();
+ last_accel_ = value->accelerometer_x();
break;
case IMUType::IMU_Y:
- last_accel_ = -imu_values_fetcher_->accelerometer_y();
+ last_accel_ = -value->accelerometer_y();
break;
case IMUType::IMU_Z:
- last_accel_ = imu_values_fetcher_->accelerometer_z();
+ last_accel_ = value->accelerometer_z();
break;
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index b766db8..d350321 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -21,7 +21,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/queues/gyro_generated.h"
-#include "frc971/wpilib/imu_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
namespace frc971 {
@@ -59,7 +59,7 @@
const DrivetrainConfig<double> dt_config_;
::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
- ::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
+ ::aos::Fetcher<::frc971::IMUValuesBatch> imu_values_fetcher_;
::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
zeroing::ImuZeroer imu_zeroer_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index f452bc6..bafedff 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -3,8 +3,9 @@
[
{
"name": "/drivetrain",
- "type": "frc971.IMUValues",
- "frequency": 2000
+ "type": "frc971.IMUValuesBatch",
+ "max_size": 2000,
+ "frequency": 200
},
{
"name": "/drivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 6a90122..3ed8d00 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -10,6 +10,7 @@
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
+#include "frc971/wpilib/imu_batch_generated.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
@@ -107,7 +108,7 @@
drivetrain_status_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- imu_sender_(event_loop->MakeSender<::frc971::IMUValues>("/drivetrain")),
+ imu_sender_(event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -211,7 +212,16 @@
std::chrono::duration_cast<std::chrono::nanoseconds>(
event_loop_->monotonic_now().time_since_epoch())
.count());
- builder.Send(imu_builder.Finish());
+ flatbuffers::Offset<frc971::IMUValues> imu_values_offsets =
+ imu_builder.Finish();
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
+ imu_values_offset = builder.fbb()->CreateVector(&imu_values_offsets, 1);
+
+ frc971::IMUValuesBatch::Builder imu_values_batch_builder =
+ builder.MakeBuilder<frc971::IMUValuesBatch>();
+ imu_values_batch_builder.add_readings(imu_values_offset);
+ builder.Send(imu_values_batch_builder.Finish());
}
// Simulates the drivetrain moving for one timestep.
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index e8b513c..f749626 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -9,7 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/wpilib/imu_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
namespace frc971 {
namespace control_loops {
@@ -95,7 +95,7 @@
drivetrain_output_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Sender<::frc971::IMUValues> imu_sender_;
+ ::aos::Sender<::frc971::IMUValuesBatch> imu_sender_;
DrivetrainConfig<double> dt_config_;
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index 28b1a0b..fa61d9b 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -4,6 +4,7 @@
#include "glog/logging.h"
+#include "aos/containers/sized_array.h"
#include "aos/time/time.h"
#include "hal/HAL.h"
@@ -174,7 +175,7 @@
frc::DigitalInput *data_ready, frc::DigitalOutput *reset)
: event_loop_(event_loop),
imu_values_sender_(
- event_loop_->MakeSender<::frc971::IMUValues>("/drivetrain")),
+ event_loop_->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
initialize_timer_(
event_loop_->AddTimer([this]() { DoInitializeStep(); })),
spi_(spi),
@@ -209,8 +210,12 @@
return;
}
+ auto builder = imu_values_sender_.MakeBuilder();
+
int amount_to_read =
spi_->ReadAutoReceivedData(to_read_.data(), 0, 0 /* don't block */);
+
+ aos::SizedArray<flatbuffers::Offset<IMUValues>, 50> readings_offsets;
while (true) {
if (amount_to_read == 0) break;
CHECK(!to_read_.empty());
@@ -223,7 +228,9 @@
amount_to_read -= amount_read_now;
if (to_read_.empty()) {
- ProcessReading();
+ flatbuffers::Offset<IMUValues> reading_offset =
+ ProcessReading(builder.fbb());
+ readings_offsets.push_back(reading_offset);
// Reset for the next reading.
to_read_ = absl::MakeSpan(read_data_);
@@ -232,6 +239,15 @@
break;
}
}
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<IMUValues>>>
+ readings_offset = builder.fbb()->CreateVector(readings_offsets.data(),
+ readings_offsets.size());
+
+ IMUValuesBatch::Builder imu_values_batch_builder =
+ builder.MakeBuilder<IMUValuesBatch>();
+ imu_values_batch_builder.add_readings(readings_offset);
+ builder.Send(imu_values_batch_builder.Finish());
}
void ADIS16470::DoInitializeStep() {
@@ -331,7 +347,17 @@
if (!self_test_diag_stat.IsNull()) {
imu_builder.add_self_test_diag_stat(self_test_diag_stat);
}
- builder.Send(imu_builder.Finish());
+
+ const flatbuffers::Offset<IMUValues> readings_offsets =
+ imu_builder.Finish();
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<IMUValues>>>
+ readings_offset = builder.fbb()->CreateVector(&readings_offsets, 1);
+
+ IMUValuesBatch::Builder imu_batch_builder =
+ builder.MakeBuilder<IMUValuesBatch>();
+ imu_batch_builder.add_readings(readings_offset);
+ builder.Send(imu_batch_builder.Finish());
if (success) {
state_ = State::kRunning;
} else {
@@ -345,13 +371,12 @@
}
}
-void ADIS16470::ProcessReading() {
+flatbuffers::Offset<IMUValues> ADIS16470::ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb) {
// If we ever see this, we'll need to decide how to handle it. Probably reset
// everything and try again.
CHECK_EQ(0, spi_->GetAutoDroppedCount());
- auto builder = imu_values_sender_.MakeBuilder();
-
absl::Span<const uint32_t> to_process = read_data_;
hal::fpga_clock::time_point fpga_time;
{
@@ -365,10 +390,10 @@
const uint16_t diag_stat_value = (static_cast<uint16_t>(to_process[0]) << 8) |
static_cast<uint16_t>(to_process[1]);
- const auto diag_stat = PackDiagStat(builder.fbb(), diag_stat_value);
+ const auto diag_stat = PackDiagStat(fbb, diag_stat_value);
to_process = to_process.subspan(2);
- IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
+ IMUValues::Builder imu_builder(*fbb);
imu_builder.add_fpga_timestamp(
aos::time::DurationInSeconds(fpga_time.time_since_epoch()));
imu_builder.add_monotonic_timestamp_ns(
@@ -397,7 +422,7 @@
CHECK(to_process.empty()) << "Have leftover bytes: " << to_process.size();
- builder.Send(imu_builder.Finish());
+ return imu_builder.Finish();
}
double ADIS16470::ConvertValue32(absl::Span<const uint32_t> data,
diff --git a/frc971/wpilib/ADIS16470.h b/frc971/wpilib/ADIS16470.h
index 4c9917f..a87ca1b 100644
--- a/frc971/wpilib/ADIS16470.h
+++ b/frc971/wpilib/ADIS16470.h
@@ -9,6 +9,7 @@
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/SPI.h"
#include "frc971/wpilib/fpga_time_conversion.h"
+#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/wpilib/imu_generated.h"
namespace frc971 {
@@ -47,7 +48,8 @@
void DoInitializeStep();
// Processes a complete reading in read_data_.
- void ProcessReading();
+ flatbuffers::Offset<IMUValues> ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb);
// Converts a 32-bit value at data to a scaled output value where a value of 1
// corresponds to lsb_per_output.
@@ -74,7 +76,7 @@
}
aos::EventLoop *const event_loop_;
- aos::Sender<::frc971::IMUValues> imu_values_sender_;
+ aos::Sender<::frc971::IMUValuesBatch> imu_values_sender_;
aos::TimerHandler *const initialize_timer_;
frc::SPI *const spi_;
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index bcdd2ce..734a231 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -267,6 +267,17 @@
gen_reflections = 1,
)
+flatbuffer_cc_library(
+ name = "imu_batch_fbs",
+ srcs = [
+ "imu_batch.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ ":imu_fbs_includes",
+ ],
+)
+
cc_library(
name = "ADIS16470",
srcs = [
@@ -278,7 +289,9 @@
restricted_to = ["//tools:roborio"],
deps = [
":fpga_time_conversion",
+ ":imu_batch_fbs",
":imu_fbs",
+ "//aos/containers:sized_array",
"//aos/events:event_loop",
"//aos/time",
"//third_party:wpilib",
diff --git a/frc971/wpilib/imu_batch.fbs b/frc971/wpilib/imu_batch.fbs
new file mode 100644
index 0000000..8029ced
--- /dev/null
+++ b/frc971/wpilib/imu_batch.fbs
@@ -0,0 +1,9 @@
+include "frc971/wpilib/imu.fbs";
+
+namespace frc971;
+
+table IMUValuesBatch {
+ readings:[IMUValues];
+}
+
+root_type IMUValuesBatch;