Add a dual_imu value blender
Blends the murata and tdk values and provides an IMUValuesBatch message.
Will try to use the murata until we reach saturation according to the
tdk, from then we switch to using the tdk values.
Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: I11fcceb278cfd2a22c18d68bad074acc6a563562
diff --git a/frc971/imu_fdcan/BUILD b/frc971/imu_fdcan/BUILD
index b47f367..c8a6cd7 100644
--- a/frc971/imu_fdcan/BUILD
+++ b/frc971/imu_fdcan/BUILD
@@ -13,6 +13,12 @@
visibility = ["//visibility:public"],
)
+static_flatbuffer(
+ name = "dual_imu_blender_status_fbs",
+ srcs = ["dual_imu_blender_status.fbs"],
+ visibility = ["//visibility:public"],
+)
+
cc_binary(
name = "can_translator",
srcs = ["can_translator_main.cc"],
@@ -41,13 +47,42 @@
],
)
+cc_binary(
+ name = "dual_imu_blender",
+ srcs = ["dual_imu_blender_main.cc"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dual_imu_blender_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
+
+cc_library(
+ name = "dual_imu_blender_lib",
+ srcs = [
+ "dual_imu_blender_lib.cc",
+ ],
+ hdrs = [
+ "dual_imu_blender_lib.h",
+ ],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":dual_imu_blender_status_fbs",
+ ":dual_imu_fbs",
+ "//aos/events:event_loop",
+ "//frc971/can_logger:can_logging_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
+ ],
+)
+
cc_test(
name = "can_translator_lib_test",
srcs = [
"can_translator_lib_test.cc",
],
data = [
- ":can_translator_test_config",
+ ":dual_imu_test_config",
],
deps = [
":can_translator_lib",
@@ -60,14 +95,35 @@
],
)
+cc_test(
+ name = "dual_imu_blender_lib_test",
+ srcs = [
+ "dual_imu_blender_lib_test.cc",
+ ],
+ data = [
+ ":dual_imu_test_config",
+ ],
+ deps = [
+ ":dual_imu_blender_lib",
+ ":dual_imu_blender_status_fbs",
+ ":dual_imu_fbs",
+ "//aos/events:simulated_event_loop",
+ "//aos/testing:googletest",
+ "//frc971/can_logger:can_logging_fbs",
+ "@com_github_google_glog//:glog",
+ ],
+)
+
aos_config(
- name = "can_translator_test_config",
- src = "can_translator_test_config_source.json",
+ name = "dual_imu_test_config",
+ src = "dual_imu_test_config_source.json",
flatbuffers = [
"//aos/logging:log_message_fbs",
":dual_imu_fbs",
":can_translator_status_fbs",
"//frc971/can_logger:can_logging_fbs",
+ ":dual_imu_blender_status_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
"//aos/events:event_loop_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
diff --git a/frc971/imu_fdcan/can_translator_lib_test.cc b/frc971/imu_fdcan/can_translator_lib_test.cc
index ce9dd59..2fb2fe4 100644
--- a/frc971/imu_fdcan/can_translator_lib_test.cc
+++ b/frc971/imu_fdcan/can_translator_lib_test.cc
@@ -12,7 +12,7 @@
public:
CANTranslatorTest()
: config_(aos::configuration::ReadConfig(
- "frc971/imu_fdcan/can_translator_test_config.json")),
+ "frc971/imu_fdcan/dual_imu_test_config.json")),
event_loop_factory_(&config_.message()),
can_translator_event_loop_(
event_loop_factory_.MakeEventLoop("can_translator")),
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.cc b/frc971/imu_fdcan/dual_imu_blender_lib.cc
new file mode 100644
index 0000000..09655e3
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.cc
@@ -0,0 +1,128 @@
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+
+#include "gflags/gflags.h"
+
+DEFINE_bool(murata_only, false,
+ "If true then only use the murata value and ignore the tdk.");
+
+// Saturation for the gyro is measured in +- radians/s
+static constexpr double kMurataGyroSaturation = (300.0 * M_PI) / 180;
+
+// Measured in gs
+static constexpr double kMurataAccelSaturation = 6.0;
+
+// Coefficient to multiply the saturation values by to give some room on where
+// we switch to tdk.
+static constexpr double kSaturationCoeff = 0.9;
+
+using frc971::imu_fdcan::DualImuBlender;
+
+DualImuBlender::DualImuBlender(aos::EventLoop *event_loop)
+ : imu_values_batch_sender_(
+ event_loop->MakeSender<frc971::IMUValuesBatchStatic>("/localizer")),
+ dual_imu_blender_status_sender_(
+ event_loop->MakeSender<frc971::imu::DualImuBlenderStatusStatic>(
+ "/imu")) {
+ // TODO(max): Give this a proper priority
+ event_loop->SetRuntimeRealtimePriority(15);
+
+ event_loop->MakeWatcher("/imu", [this](const frc971::imu::DualImu &dual_imu) {
+ HandleDualImu(&dual_imu);
+ });
+}
+
+void DualImuBlender::HandleDualImu(const frc971::imu::DualImu *dual_imu) {
+ aos::Sender<frc971::IMUValuesBatchStatic>::StaticBuilder
+ imu_values_batch_builder_ = imu_values_batch_sender_.MakeStaticBuilder();
+
+ aos::Sender<frc971::imu::DualImuBlenderStatusStatic>::StaticBuilder
+ dual_imu_blender_status_builder =
+ dual_imu_blender_status_sender_.MakeStaticBuilder();
+
+ frc971::IMUValuesStatic *imu_values =
+ CHECK_NOTNULL(imu_values_batch_builder_->add_readings()->emplace_back());
+
+ imu_values->set_pico_timestamp_us(dual_imu->board_timestamp_us());
+ imu_values->set_monotonic_timestamp_ns(dual_imu->kernel_timestamp());
+ imu_values->set_data_counter(dual_imu->packet_counter());
+
+ if (std::abs(dual_imu->tdk()->gyro_x()) >=
+ kSaturationCoeff * kMurataGyroSaturation) {
+ dual_imu_blender_status_builder->set_gyro_x(imu::ImuType::TDK);
+ imu_values->set_gyro_x(dual_imu->tdk()->gyro_x());
+ } else {
+ dual_imu_blender_status_builder->set_gyro_x(imu::ImuType::MURATA);
+ imu_values->set_gyro_x(dual_imu->murata()->gyro_x());
+ }
+
+ if (std::abs(dual_imu->tdk()->gyro_y()) >=
+ kSaturationCoeff * kMurataGyroSaturation) {
+ dual_imu_blender_status_builder->set_gyro_y(imu::ImuType::TDK);
+ imu_values->set_gyro_y(dual_imu->tdk()->gyro_y());
+ } else {
+ dual_imu_blender_status_builder->set_gyro_y(imu::ImuType::MURATA);
+ imu_values->set_gyro_y(dual_imu->murata()->gyro_y());
+ }
+
+ if (std::abs(dual_imu->tdk()->gyro_z()) >=
+ kSaturationCoeff * kMurataGyroSaturation) {
+ dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::TDK);
+ imu_values->set_gyro_z(dual_imu->tdk()->gyro_z());
+ } else {
+ dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::MURATA);
+ imu_values->set_gyro_z(dual_imu->murata()->gyro_z());
+ }
+
+ if (std::abs(dual_imu->tdk()->accelerometer_x()) >=
+ kSaturationCoeff * kMurataAccelSaturation) {
+ dual_imu_blender_status_builder->set_accelerometer_x(imu::ImuType::TDK);
+ imu_values->set_accelerometer_x(dual_imu->tdk()->accelerometer_x());
+ } else {
+ dual_imu_blender_status_builder->set_accelerometer_x(imu::ImuType::MURATA);
+ imu_values->set_accelerometer_x(dual_imu->murata()->accelerometer_x());
+ }
+
+ if (std::abs(dual_imu->tdk()->accelerometer_y()) >=
+ kSaturationCoeff * kMurataAccelSaturation) {
+ dual_imu_blender_status_builder->set_accelerometer_y(imu::ImuType::TDK);
+ imu_values->set_accelerometer_y(dual_imu->tdk()->accelerometer_y());
+ } else {
+ dual_imu_blender_status_builder->set_accelerometer_y(imu::ImuType::MURATA);
+ imu_values->set_accelerometer_y(dual_imu->murata()->accelerometer_y());
+ }
+
+ if (std::abs(dual_imu->tdk()->accelerometer_z()) >=
+ kSaturationCoeff * kMurataAccelSaturation) {
+ dual_imu_blender_status_builder->set_accelerometer_z(imu::ImuType::TDK);
+ imu_values->set_accelerometer_z(dual_imu->tdk()->accelerometer_z());
+ } else {
+ dual_imu_blender_status_builder->set_accelerometer_z(imu::ImuType::MURATA);
+ imu_values->set_accelerometer_z(dual_imu->murata()->accelerometer_z());
+ }
+
+ if (FLAGS_murata_only) {
+ imu_values->set_gyro_x(dual_imu->murata()->gyro_x());
+ imu_values->set_gyro_y(dual_imu->murata()->gyro_y());
+ imu_values->set_gyro_z(dual_imu->murata()->gyro_z());
+
+ imu_values->set_accelerometer_x(dual_imu->murata()->accelerometer_x());
+ imu_values->set_accelerometer_y(dual_imu->murata()->accelerometer_y());
+ imu_values->set_accelerometer_z(dual_imu->murata()->accelerometer_z());
+
+ dual_imu_blender_status_builder->set_gyro_x(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_gyro_y(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_gyro_z(imu::ImuType::MURATA);
+
+ dual_imu_blender_status_builder->set_accelerometer_x(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_accelerometer_y(imu::ImuType::MURATA);
+ dual_imu_blender_status_builder->set_accelerometer_z(imu::ImuType::MURATA);
+ }
+
+ dual_imu_blender_status_builder.CheckOk(
+ dual_imu_blender_status_builder.Send());
+
+ imu_values->set_temperature(
+ dual_imu->murata()->chip_states()->Get(0)->temperature());
+
+ imu_values_batch_builder_.CheckOk(imu_values_batch_builder_.Send());
+}
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib.h b/frc971/imu_fdcan/dual_imu_blender_lib.h
new file mode 100644
index 0000000..04044d2
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_lib.h
@@ -0,0 +1,27 @@
+#ifndef FRC971_IMU_FDCAN_DUAL_IMU_BLENDER_H_
+#define FRC971_IMU_FDCAN_DUAL_IMU_BLENDER_H_
+
+#include "aos/events/event_loop.h"
+#include "frc971/imu_fdcan/dual_imu_blender_status_static.h"
+#include "frc971/imu_fdcan/dual_imu_generated.h"
+#include "frc971/wpilib/imu_batch_static.h"
+
+namespace frc971::imu_fdcan {
+
+// Takes in the values from the dual_imu and creates an IMUValuesBatch. Will use
+// the murata until we've hit saturation according to the tdk, then we will
+// switch to using tdk IMU values.
+class DualImuBlender {
+ public:
+ DualImuBlender(aos::EventLoop *event_loop);
+
+ void HandleDualImu(const frc971::imu::DualImu *dual_imu);
+
+ private:
+ aos::Sender<IMUValuesBatchStatic> imu_values_batch_sender_;
+ aos::Sender<imu::DualImuBlenderStatusStatic> dual_imu_blender_status_sender_;
+};
+
+} // namespace frc971::imu_fdcan
+
+#endif // FRC971_IMU_FDCAN_DUAL_IMU_BLENDER_H_
diff --git a/frc971/imu_fdcan/dual_imu_blender_lib_test.cc b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
new file mode 100644
index 0000000..b3a3015
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_lib_test.cc
@@ -0,0 +1,302 @@
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+
+#include "glog/logging.h"
+#include "gtest/gtest.h"
+
+#include "aos/events/simulated_event_loop.h"
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+#include "frc971/imu_fdcan/dual_imu_blender_status_generated.h"
+#include "frc971/imu_fdcan/dual_imu_generated.h"
+#include "frc971/imu_fdcan/dual_imu_static.h"
+
+class DualImuBlenderTest : public ::testing::Test {
+ public:
+ DualImuBlenderTest()
+ : config_(aos::configuration::ReadConfig(
+ "frc971/imu_fdcan/dual_imu_test_config.json")),
+ event_loop_factory_(&config_.message()),
+ dual_imu_blender_event_loop_(
+ event_loop_factory_.MakeEventLoop("dual_imu_blender")),
+ dual_imu_event_loop_(event_loop_factory_.MakeEventLoop("dual_imu")),
+ imu_values_batch_fetcher_(
+ dual_imu_event_loop_->MakeFetcher<frc971::IMUValuesBatch>(
+ "/localizer")),
+ dual_imu_blender_status_fetcher_(
+ dual_imu_blender_event_loop_
+ ->MakeFetcher<frc971::imu::DualImuBlenderStatus>("/imu")),
+ dual_imu_sender_(
+ dual_imu_event_loop_->MakeSender<frc971::imu::DualImuStatic>(
+ "/imu")),
+ dual_imu_blender_(dual_imu_blender_event_loop_.get()) {}
+
+ void CheckImuType(frc971::imu::ImuType type) {
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_x(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_y(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->gyro_z(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->accelerometer_x(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->accelerometer_y(), type);
+ EXPECT_EQ(dual_imu_blender_status_fetcher_->accelerometer_z(), type);
+ }
+
+ protected:
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config_;
+ aos::SimulatedEventLoopFactory event_loop_factory_;
+
+ std::unique_ptr<aos::EventLoop> dual_imu_blender_event_loop_;
+ std::unique_ptr<aos::EventLoop> dual_imu_event_loop_;
+
+ aos::Fetcher<frc971::IMUValuesBatch> imu_values_batch_fetcher_;
+ aos::Fetcher<frc971::imu::DualImuBlenderStatus>
+ dual_imu_blender_status_fetcher_;
+
+ aos::Sender<frc971::imu::DualImuStatic> dual_imu_sender_;
+
+ frc971::imu_fdcan::DualImuBlender dual_imu_blender_;
+};
+
+// Sanity check that some sane values in are the same values out
+TEST_F(DualImuBlenderTest, SanityCheck) {
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+ frc971::imu::ChipStateStatic *murata_due_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(0);
+ dual_imu_builder->set_kernel_timestamp(0);
+
+ tdk->set_gyro_x(0.3);
+ tdk->set_gyro_y(0.2);
+ tdk->set_gyro_z(0.2);
+
+ murata->set_gyro_x(0.351);
+ murata->set_gyro_y(0.284);
+ murata->set_gyro_z(0.293);
+
+ tdk->set_accelerometer_x(1.5);
+ tdk->set_accelerometer_y(1.5);
+ tdk->set_accelerometer_z(1.5);
+
+ murata->set_accelerometer_x(1.58);
+ murata->set_accelerometer_y(1.51);
+ murata->set_accelerometer_z(1.52);
+
+ murata_uno_chip_state->set_temperature(20);
+ murata_due_chip_state->set_temperature(10);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 0.351,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 0.284,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 0.293,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 1.58, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 1.51, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 1.52, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::MURATA);
+}
+
+TEST_F(DualImuBlenderTest, Saturation) {
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(0);
+ dual_imu_builder->set_kernel_timestamp(0);
+
+ tdk->set_gyro_x(0.7);
+ tdk->set_gyro_y(0.7);
+ tdk->set_gyro_z(0.7);
+
+ murata->set_gyro_x(0.71);
+ murata->set_gyro_y(0.79);
+ murata->set_gyro_z(0.78);
+
+ tdk->set_accelerometer_x(1.0);
+ tdk->set_accelerometer_y(1.0);
+ tdk->set_accelerometer_z(1.0);
+
+ murata->set_accelerometer_x(1.3);
+ murata->set_accelerometer_y(1.1);
+ murata->set_accelerometer_z(1.1);
+
+ murata_uno_chip_state->set_temperature(20);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 0.71,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 0.79,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 0.78,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 1.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 1.1, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 1.1, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::MURATA);
+
+ // Make sure we switch to TDK on saturation
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(1);
+ dual_imu_builder->set_kernel_timestamp(1);
+
+ tdk->set_gyro_x(6.0);
+ tdk->set_gyro_y(6.0);
+ tdk->set_gyro_z(6.0);
+
+ murata->set_gyro_x(5.2);
+ murata->set_gyro_y(5.2);
+ murata->set_gyro_z(5.2);
+
+ tdk->set_accelerometer_x(6.2);
+ tdk->set_accelerometer_y(6.3);
+ tdk->set_accelerometer_z(6.5);
+
+ murata->set_accelerometer_x(5.5);
+ murata->set_accelerometer_y(5.5);
+ murata->set_accelerometer_z(5.5);
+
+ murata_uno_chip_state->set_temperature(20);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), 6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), 6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), 6.0,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ 6.2, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ 6.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ 6.5, 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->temperature(), 20,
+ 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::TDK);
+
+ // Check negative values as well
+ dual_imu_blender_event_loop_->OnRun([this] {
+ aos::Sender<frc971::imu::DualImuStatic>::StaticBuilder dual_imu_builder =
+ dual_imu_sender_.MakeStaticBuilder();
+
+ frc971::imu::SingleImuStatic *murata = dual_imu_builder->add_murata();
+
+ auto *murata_chip_states = murata->add_chip_states();
+ frc971::imu::ChipStateStatic *murata_uno_chip_state =
+ murata_chip_states->emplace_back();
+
+ frc971::imu::SingleImuStatic *tdk = dual_imu_builder->add_tdk();
+
+ dual_imu_builder->set_board_timestamp_us(1);
+ dual_imu_builder->set_kernel_timestamp(1);
+
+ tdk->set_gyro_x(-6.0);
+ tdk->set_gyro_y(-6.0);
+ tdk->set_gyro_z(-6.0);
+
+ murata->set_gyro_x(-5.2);
+ murata->set_gyro_y(-5.2);
+ murata->set_gyro_z(-5.2);
+
+ tdk->set_accelerometer_x(-6.2);
+ tdk->set_accelerometer_y(-6.3);
+ tdk->set_accelerometer_z(-6.5);
+
+ murata->set_accelerometer_x(-5.5);
+ murata->set_accelerometer_y(-5.5);
+ murata->set_accelerometer_z(-5.5);
+
+ murata_uno_chip_state->set_temperature(20);
+
+ dual_imu_builder.CheckOk(dual_imu_builder.Send());
+ });
+
+ event_loop_factory_.RunFor(std::chrono::milliseconds(200));
+
+ ASSERT_TRUE(imu_values_batch_fetcher_.Fetch());
+ ASSERT_TRUE(dual_imu_blender_status_fetcher_.Fetch());
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_x(), -6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_y(), -6.0,
+ 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->gyro_z(), -6.0,
+ 0.0001);
+
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_x(),
+ -6.2, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_y(),
+ -6.3, 0.0001);
+ EXPECT_NEAR(imu_values_batch_fetcher_->readings()->Get(0)->accelerometer_z(),
+ -6.5, 0.0001);
+
+ CheckImuType(frc971::imu::ImuType::TDK);
+}
diff --git a/frc971/imu_fdcan/dual_imu_blender_main.cc b/frc971/imu_fdcan/dual_imu_blender_main.cc
new file mode 100644
index 0000000..1d5ee22
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_main.cc
@@ -0,0 +1,20 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "frc971/imu_fdcan/dual_imu_blender_lib.h"
+
+using frc971::imu_fdcan::DualImuBlender;
+
+int main(int argc, char **argv) {
+ ::aos::InitGoogle(&argc, &argv);
+
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig("aos_config.json");
+
+ ::aos::ShmEventLoop event_loop(&config.message());
+
+ DualImuBlender blender(&event_loop);
+
+ event_loop.Run();
+
+ return 0;
+}
diff --git a/frc971/imu_fdcan/dual_imu_blender_status.fbs b/frc971/imu_fdcan/dual_imu_blender_status.fbs
new file mode 100644
index 0000000..e3c9893
--- /dev/null
+++ b/frc971/imu_fdcan/dual_imu_blender_status.fbs
@@ -0,0 +1,19 @@
+namespace frc971.imu;
+
+enum ImuType : ubyte {
+ MURATA = 0,
+ TDK = 1,
+}
+
+table DualImuBlenderStatus {
+ // These values explain if we're using the tdk or the murata for our accelerometers and gyro.
+ gyro_x: ImuType (id: 0);
+ gyro_y: ImuType (id: 1);
+ gyro_z: ImuType (id: 2);
+
+ accelerometer_x: ImuType (id: 3);
+ accelerometer_y: ImuType (id: 4);
+ accelerometer_z: ImuType (id: 5);
+}
+
+root_type DualImuBlenderStatus;
diff --git a/frc971/imu_fdcan/can_translator_test_config_source.json b/frc971/imu_fdcan/dual_imu_test_config_source.json
similarity index 70%
rename from frc971/imu_fdcan/can_translator_test_config_source.json
rename to frc971/imu_fdcan/dual_imu_test_config_source.json
index e82c87d..eda06da 100644
--- a/frc971/imu_fdcan/can_translator_test_config_source.json
+++ b/frc971/imu_fdcan/dual_imu_test_config_source.json
@@ -20,9 +20,19 @@
"frequency": 200
},
{
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "frequency": 1100
+ },
+ {
"name": "/imu",
"type": "frc971.imu.CanTranslatorStatus",
"frequency": 100
},
+ {
+ "name": "/imu",
+ "type": "frc971.imu.DualImuBlenderStatus",
+ "frequency": 100
+ },
]
}
diff --git a/frc971/wpilib/imu_batch.fbs b/frc971/wpilib/imu_batch.fbs
index 1483314..bead5df 100644
--- a/frc971/wpilib/imu_batch.fbs
+++ b/frc971/wpilib/imu_batch.fbs
@@ -2,8 +2,10 @@
namespace frc971;
+attribute "static_length";
+
table IMUValuesBatch {
- readings:[IMUValues] (id: 0);
+ readings:[IMUValues] (id: 0, static_length: 1);
}
root_type IMUValuesBatch;
diff --git a/y2024/BUILD b/y2024/BUILD
index eb266cf..92bf035 100644
--- a/y2024/BUILD
+++ b/y2024/BUILD
@@ -74,6 +74,7 @@
start_binaries = [
"//aos/events/logging:logger_main",
"//frc971/imu_fdcan:can_translator",
+ "//frc971/imu_fdcan:dual_imu_blender",
"//aos/network:message_bridge_client",
"//aos/network:message_bridge_server",
"//aos/network:web_proxy_main",
@@ -116,6 +117,7 @@
"//aos/network:message_bridge_server_fbs",
"//frc971/imu_fdcan:dual_imu_fbs",
"//frc971/imu_fdcan:can_translator_status_fbs",
+ "//frc971/imu_fdcan:dual_imu_blender_status_fbs",
"//y2024/constants:constants_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_fbs",
"//frc971/can_logger:can_logging_fbs",
diff --git a/y2024/y2024_imu.json b/y2024/y2024_imu.json
index 12b00d4..e22e010 100644
--- a/y2024/y2024_imu.json
+++ b/y2024/y2024_imu.json
@@ -238,6 +238,14 @@
"num_senders": 2
},
{
+ "name": "/imu",
+ "type": "frc971.imu.DualImuBlenderStatus",
+ "source_node": "imu",
+ "frequency": 1100,
+ "num_senders": 1,
+ "max_size": 200
+ },
+ {
"name": "/imu/constants",
"type": "y2024.Constants",
"source_node": "imu",
@@ -310,6 +318,13 @@
]
},
{
+ "name": "dual_imu_blender",
+ "executable_name": "dual_imu_blender",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
"name": "web_proxy",
"executable_name": "web_proxy_main",
"args": [