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