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/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;