Convert imu_values to event loops.
Change-Id: Ia31a022550ef076526e2198e1dbf60570caf6a16
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 4e13a40..e7cfae6 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -21,7 +21,6 @@
#include "frc971/wpilib/imu.q.h"
using frc971::sensors::gyro_reading;
-using frc971::imu_values;
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -38,6 +37,8 @@
dt_config_(dt_config),
localizer_control_fetcher_(event_loop->MakeFetcher<LocalizerControl>(
".frc971.control_loops.drivetrain.localizer_control")),
+ imu_values_fetcher_(
+ event_loop->MakeFetcher<::frc971::IMUValues>(".frc971.imu_values")),
localizer_(localizer),
kf_(dt_config_.make_kf_drivetrain_loop()),
dt_openloop_(dt_config_, &kf_),
@@ -134,28 +135,26 @@
status->gear_logging.controller_index = kf_.index();
}
- const bool is_latest_imu_values = ::frc971::imu_values.FetchLatest();
+ const bool is_latest_imu_values = imu_values_fetcher_.Fetch();
if (is_latest_imu_values) {
- const double rate = -::frc971::imu_values->gyro_y;
- const double accel_squared = ::frc971::imu_values->accelerometer_x *
- ::frc971::imu_values->accelerometer_x +
- ::frc971::imu_values->accelerometer_y *
- ::frc971::imu_values->accelerometer_y +
- ::frc971::imu_values->accelerometer_z *
- ::frc971::imu_values->accelerometer_z;
- const double angle = ::std::atan2(::frc971::imu_values->accelerometer_x,
- ::frc971::imu_values->accelerometer_z) +
+ const double rate = -imu_values_fetcher_->gyro_y;
+ const double accel_squared =
+ ::std::pow(imu_values_fetcher_->accelerometer_x, 2.0) +
+ ::std::pow(imu_values_fetcher_->accelerometer_y, 2.0) +
+ ::std::pow(imu_values_fetcher_->accelerometer_z, 2.0);
+ const double angle = ::std::atan2(imu_values_fetcher_->accelerometer_x,
+ imu_values_fetcher_->accelerometer_z) +
0.008;
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
- last_accel_ = -::frc971::imu_values->accelerometer_x;
+ last_accel_ = -imu_values_fetcher_->accelerometer_x;
break;
case IMUType::IMU_FLIPPED_X:
- last_accel_ = ::frc971::imu_values->accelerometer_x;
+ last_accel_ = imu_values_fetcher_->accelerometer_x;
break;
case IMUType::IMU_Y:
- last_accel_ = -::frc971::imu_values->accelerometer_y;
+ last_accel_ = -imu_values_fetcher_->accelerometer_y;
break;
}
@@ -183,29 +182,29 @@
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values.get());
- last_gyro_rate_ = imu_values->gyro_x;
+ LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ last_gyro_rate_ = imu_values_fetcher_->gyro_x;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Y_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values.get());
- last_gyro_rate_ = imu_values->gyro_y;
+ LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ last_gyro_rate_ = imu_values_fetcher_->gyro_y;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::IMU_Z_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values.get());
- last_gyro_rate_ = imu_values->gyro_z;
+ LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ last_gyro_rate_ = imu_values_fetcher_->gyro_z;
last_gyro_time_ = monotonic_now;
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
if (is_latest_imu_values) {
- LOG_STRUCT(DEBUG, "using", *imu_values.get());
- last_gyro_rate_ = -imu_values->gyro_z;
+ LOG_STRUCT(DEBUG, "using", *imu_values_fetcher_.get());
+ last_gyro_rate_ = -imu_values_fetcher_->gyro_z;
last_gyro_time_ = monotonic_now;
}
break;
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index f07d840..39d7054 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -9,12 +9,13 @@
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/control_loops/drivetrain/drivetrain_config.h"
#include "frc971/control_loops/drivetrain/gear.h"
-#include "frc971/control_loops/drivetrain/localizer.h"
-#include "frc971/control_loops/drivetrain/polydrivetrain.h"
-#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
+#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/localizer.q.h"
+#include "frc971/control_loops/drivetrain/polydrivetrain.h"
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
+#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
+#include "frc971/wpilib/imu.q.h"
namespace frc971 {
namespace control_loops {
@@ -47,6 +48,7 @@
const DrivetrainConfig<double> dt_config_;
::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
+ ::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
LocalizerInterface *localizer_;
StateFeedbackLoop<7, 2, 4> kf_;
diff --git a/frc971/wpilib/ADIS16448.cc b/frc971/wpilib/ADIS16448.cc
index aec4d4f..75f082d 100644
--- a/frc971/wpilib/ADIS16448.cc
+++ b/frc971/wpilib/ADIS16448.cc
@@ -123,6 +123,8 @@
: event_loop_(event_loop),
joystick_state_fetcher_(event_loop_->MakeFetcher<::aos::JoystickState>(
".aos.joystick_state")),
+ imu_values_sender_(
+ event_loop_->MakeSender<::frc971::IMUValues>(".frc971.imu_values")),
spi_(new frc::SPI(port)),
dio1_(dio1) {
// 1MHz is the maximum supported for burst reads, but we
@@ -241,7 +243,7 @@
}
}
- auto message = imu_values.MakeMessage();
+ auto message = imu_values_sender_.MakeMessage();
message->fpga_timestamp = ::aos::time::DurationInSeconds(
dio1_->ReadRisingTimestamp().time_since_epoch());
message->monotonic_timestamp_ns =
diff --git a/frc971/wpilib/ADIS16448.h b/frc971/wpilib/ADIS16448.h
index a1d6dcd..5af6a4f 100644
--- a/frc971/wpilib/ADIS16448.h
+++ b/frc971/wpilib/ADIS16448.h
@@ -14,6 +14,7 @@
#include "aos/events/event-loop.h"
#include "aos/logging/logging.h"
#include "aos/robot_state/robot_state.q.h"
+#include "frc971/wpilib/imu.q.h"
#include "frc971/wpilib/spi_rx_clearer.h"
namespace frc971 {
@@ -94,6 +95,7 @@
::aos::EventLoop *event_loop_;
::aos::Fetcher<::aos::JoystickState> joystick_state_fetcher_;
+ ::aos::Sender<::frc971::IMUValues> imu_values_sender_;
// TODO(Brian): This object has no business owning these ones.
const ::std::unique_ptr<frc::SPI> spi_;
diff --git a/frc971/wpilib/imu.q b/frc971/wpilib/imu.q
index c404126..4d9dec7 100644
--- a/frc971/wpilib/imu.q
+++ b/frc971/wpilib/imu.q
@@ -1,6 +1,7 @@
package frc971;
// Values returned from an IMU.
+// Published on ".frc971.imu_values"
message IMUValues {
// Gyro readings in radians/second.
// Positive is clockwise looking at the connector.
@@ -37,5 +38,3 @@
// CLOCK_MONOTONIC time in nanoseconds when the values were captured.
int64_t monotonic_timestamp_ns;
};
-
-queue IMUValues imu_values;