Actually run down estimator in drivetrain
Change-Id: I8602c448b632e966fe3f252b48a0f7ec0726285e
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 6f46965..a120c0c 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -374,6 +374,7 @@
":drivetrain_position_fbs",
":drivetrain_status_fbs",
":gear",
+ ":improved_down_estimator",
":line_follow_drivetrain",
":localizer",
":localizer_fbs",
@@ -385,6 +386,7 @@
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro_fbs",
"//frc971/wpilib:imu_fbs",
+ "//frc971/zeroing:imu_zeroer",
],
)
@@ -405,6 +407,7 @@
"//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:imu_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
],
@@ -436,6 +439,7 @@
"//aos/controls:control_loop_test",
"//aos/testing:googletest",
"//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:imu_fbs",
] + cpu_select({
"amd64": [
"//third_party/matplotlib-cpp",
@@ -652,7 +656,11 @@
"improved_down_estimator.h",
],
deps = [
+ ":drivetrain_status_fbs",
+ "//aos/events:event_loop",
+ "//frc971/control_loops:control_loops_fbs",
"//frc971/control_loops:runge_kutta",
+ "@//aos/time",
"@com_github_google_glog//:glog",
"@org_tuxfamily_eigen//:eigen",
],
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 51c6f03..a8014d5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -47,13 +47,11 @@
dt_closedloop_(dt_config_, &kf_, localizer_),
dt_spline_(dt_config_),
dt_line_follow_(dt_config_, localizer->target_selector()),
- down_estimator_(MakeDownEstimatorLoop()),
left_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
right_gear_(dt_config_.default_high_gear ? Gear::HIGH : Gear::LOW),
left_high_requested_(dt_config_.default_high_gear),
right_high_requested_(dt_config_.default_high_gear) {
::aos::controls::HPolytope<0>::Init();
- down_U_.setZero();
event_loop->SetRuntimeRealtimePriority(30);
}
@@ -98,7 +96,6 @@
if (!has_been_enabled_ && output) {
has_been_enabled_ = true;
- down_estimator_.mutable_X_hat(1, 0) = 0.0;
}
// TODO(austin): Put gear detection logic here.
@@ -142,17 +139,22 @@
gear_logging_offset = gear_logging_builder.Finish();
}
- const bool is_latest_imu_values = imu_values_fetcher_.Fetch();
- if (is_latest_imu_values) {
- 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;
+ while (imu_values_fetcher_.FetchNext()) {
+ imu_zeroer_.ProcessMeasurement(*imu_values_fetcher_);
+ last_gyro_time_ = monotonic_now;
+ 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) {
+ 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) {
+ got_imu_reading = true;
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
last_accel_ = -imu_values_fetcher_->accelerometer_x();
@@ -164,51 +166,29 @@
last_accel_ = -imu_values_fetcher_->accelerometer_y();
break;
}
-
- if (accel_squared > 1.03 || accel_squared < 0.97) {
- AOS_LOG(DEBUG, "New IMU value, rejecting reading\n");
- } else {
- // -y is our gyro.
- // z accel is down
- // x accel is the front of the robot pointed down.
- Eigen::Matrix<double, 1, 1> Y;
- Y(0, 0) = angle;
- down_estimator_.Correct(Y);
- }
-
- AOS_LOG(DEBUG,
- "New IMU value, rate is %f, angle %f, fused %f, bias "
- "%f\n",
- rate, angle, down_estimator_.X_hat(0), down_estimator_.X_hat(1));
- down_U_(0, 0) = rate;
}
- down_estimator_.UpdateObserver(down_U_, ::aos::controls::kLoopFrequency);
// TODO(austin): Signal the current gear to both loops.
switch (dt_config_.gyro_type) {
case GyroType::IMU_X_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = imu_values_fetcher_->gyro_x();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = imu_zeroer_.ZeroedGyro().x();
}
break;
case GyroType::IMU_Y_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = imu_values_fetcher_->gyro_y();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = imu_zeroer_.ZeroedGyro().y();
}
break;
case GyroType::IMU_Z_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = imu_values_fetcher_->gyro_z();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = imu_zeroer_.ZeroedGyro().z();
}
break;
case GyroType::FLIPPED_IMU_Z_GYRO:
- if (is_latest_imu_values) {
- last_gyro_rate_ = -imu_values_fetcher_->gyro_z();
- last_gyro_time_ = monotonic_now;
+ if (got_imu_reading) {
+ last_gyro_rate_ = -imu_zeroer_.ZeroedGyro().z();
}
break;
case GyroType::SPARTAN_GYRO:
@@ -327,6 +307,9 @@
const flatbuffers::Offset<PolyDriveLogging> poly_drive_logging_offset =
dt_openloop_.PopulateStatus(status->fbb());
+ const flatbuffers::Offset<DownEstimatorState> down_estimator_state_offset =
+ down_estimator_.PopulateStatus(status->fbb());
+
flatbuffers::Offset<LineFollowLogging> line_follow_logging_offset =
dt_line_follow_.PopulateStatus(status);
flatbuffers::Offset<TrajectoryLogging> trajectory_logging_offset =
@@ -360,12 +343,12 @@
builder.add_y(localizer_->y());
builder.add_theta(::aos::math::NormalizeAngle(localizer_->theta()));
- builder.add_ground_angle(down_estimator_.X_hat(0) + dt_config_.down_offset);
builder.add_cim_logging(cim_logging_offset);
builder.add_poly_drive_logging(poly_drive_logging_offset);
builder.add_gear_logging(gear_logging_offset);
builder.add_line_follow_logging(line_follow_logging_offset);
builder.add_trajectory_logging(trajectory_logging_offset);
+ builder.add_down_estimator(down_estimator_state_offset);
status->Send(builder.Finish());
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 9f6a34c..f7924c5 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -13,6 +13,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/drivetrain/gear.h"
+#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/drivetrain/line_follow_drivetrain.h"
#include "frc971/control_loops/drivetrain/localizer.h"
#include "frc971/control_loops/drivetrain/localizer_generated.h"
@@ -21,6 +22,7 @@
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/wpilib/imu_generated.h"
+#include "frc971/zeroing/imu_zeroer.h"
namespace frc971 {
namespace control_loops {
@@ -57,6 +59,11 @@
::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
+
+ zeroing::ImuZeroer imu_zeroer_;
+ DrivetrainUkf down_estimator_;
+ aos::monotonic_clock::time_point last_imu_update_ =
+ aos::monotonic_clock::min_time;
LocalizerInterface *localizer_;
StateFeedbackLoop<7, 2, 4> kf_;
@@ -67,9 +74,6 @@
::aos::monotonic_clock::time_point last_gyro_time_ =
::aos::monotonic_clock::min_time;
- StateFeedbackLoop<2, 1, 1> down_estimator_;
- Eigen::Matrix<double, 1, 1> down_U_;
-
// Current gears for each drive side.
Gear left_gear_;
Gear right_gear_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index a289fed..e29fed9 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -59,7 +59,7 @@
}
virtual ~DrivetrainTest() {}
- void TearDown() { drivetrain_plant_.MaybePlot(); }
+ void TearDown() override { drivetrain_plant_.MaybePlot(); }
void VerifyNearGoal() {
drivetrain_goal_fetcher_.Fetch();
@@ -104,6 +104,31 @@
->is_executed());
}
+ void VerifyDownEstimator() {
+ EXPECT_TRUE(drivetrain_status_fetcher_.Fetch());
+ // TODO(james): Handle Euler angle singularities...
+ const double down_estimator_yaw =
+ CHECK_NOTNULL(drivetrain_status_fetcher_->down_estimator())->yaw();
+ const double localizer_yaw =
+ drivetrain_status_fetcher_->theta();
+ EXPECT_LT(
+ std::abs(aos::math::DiffAngle(down_estimator_yaw, localizer_yaw)),
+ 1e-5);
+ const double true_yaw = (drivetrain_plant_.GetRightPosition() -
+ drivetrain_plant_.GetLeftPosition()) /
+ (dt_config_.robot_radius * 2.0);
+ EXPECT_LT(std::abs(aos::math::DiffAngle(down_estimator_yaw, true_yaw)),
+ 1e-4);
+ // We don't currently simulate any pitch or roll, so we shouldn't be
+ // reporting any.
+ EXPECT_NEAR(
+ 0, drivetrain_status_fetcher_->down_estimator()->longitudinal_pitch(),
+ 1e-10);
+ EXPECT_NEAR(0,
+ drivetrain_status_fetcher_->down_estimator()->lateral_pitch(),
+ 1e-10);
+ }
+
::std::unique_ptr<::aos::EventLoop> test_event_loop_;
::aos::Sender<::frc971::control_loops::drivetrain::Goal>
drivetrain_goal_sender_;
@@ -127,6 +152,8 @@
// Tests that the drivetrain converges on a goal.
TEST_F(DrivetrainTest, ConvergesCorrectly) {
+ // Run for enough time to let the gyro zero.
+ RunFor(std::chrono::seconds(100));
SetEnabled(true);
{
auto builder = drivetrain_goal_sender_.MakeBuilder();
@@ -138,6 +165,7 @@
}
RunFor(chrono::seconds(2));
VerifyNearGoal();
+ VerifyDownEstimator();
}
// Tests that the drivetrain converges on a goal when under the effect of a
diff --git a/frc971/control_loops/drivetrain/drivetrain_status.fbs b/frc971/control_loops/drivetrain/drivetrain_status.fbs
index 1956abd..0e49552 100644
--- a/frc971/control_loops/drivetrain/drivetrain_status.fbs
+++ b/frc971/control_loops/drivetrain/drivetrain_status.fbs
@@ -99,6 +99,27 @@
rel_theta:float;
}
+table DownEstimatorState {
+ quaternion_x:double;
+ quaternion_y:double;
+ quaternion_z:double;
+ quaternion_w:double;
+
+ // Side-to-side and forwards/backwards pitch numbers. Note that we do this
+ // instead of standard roll/pitch/yaw euler angles because it was a pain to
+ // try and numerically stable roll/pitch/yaw numbers, and Eigen's interface
+ // doesn't resolve the redundancies quite how we'd like.
+ // Lateral pitch is the side-to-side pitch of the robot; longitudinal pitch is
+ // the forwards to backwards pitch of the robot; longitudinal_pitch
+ // corresponds with the traditional usage of "pitch".
+ // All angles in radians.
+ lateral_pitch:float;
+ longitudinal_pitch:float;
+ // Current yaw angle (heading) of the robot, as estimated solely by the down
+ // estimator.
+ yaw:float;
+}
+
table Status {
// Estimated speed of the center of the robot in m/s (positive forwards).
robot_speed:double;
@@ -138,7 +159,8 @@
// True if the output voltage was capped last cycle.
output_was_capped:bool;
- // The angle of the robot relative to the ground.
+ // The pitch of the robot relative to the ground--only includes
+ // forwards/backwards rotation.
ground_angle:double;
// Information about shifting logic and curent gear, for logging purposes
@@ -150,6 +172,8 @@
line_follow_logging:LineFollowLogging;
poly_drive_logging:PolyDriveLogging;
+
+ down_estimator:DownEstimatorState;
}
root_type Status;
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 8ebbb1b..56e664b 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -51,8 +51,8 @@
static DrivetrainConfig<double> kDrivetrainConfig{
::frc971::control_loops::drivetrain::ShifterType::HALL_EFFECT_SHIFTER,
::frc971::control_loops::drivetrain::LoopType::CLOSED_LOOP,
- ::frc971::control_loops::drivetrain::GyroType::SPARTAN_GYRO,
- IMUType::IMU_X,
+ ::frc971::control_loops::drivetrain::GyroType::IMU_Z_GYRO,
+ IMUType::IMU_FLIPPED_X,
::y2016::control_loops::drivetrain::MakeDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeVelocityDrivetrainLoop,
::y2016::control_loops::drivetrain::MakeKFDrivetrainLoop,
@@ -101,9 +101,8 @@
drivetrain_status_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- gyro_reading_sender_(
- event_loop->MakeSender<::frc971::sensors::GyroReading>(
- "/drivetrain")),
+ imu_sender_(
+ event_loop->MakeSender<::frc971::IMUValues>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -168,15 +167,24 @@
}
{
- auto builder = gyro_reading_sender_.MakeBuilder();
- frc971::sensors::GyroReading::Builder gyro_builder =
- builder.MakeBuilder<frc971::sensors::GyroReading>();
- gyro_builder.add_angle((right_encoder - left_encoder) /
- (dt_config_.robot_radius * 2.0));
- gyro_builder.add_velocity(
+ auto builder = imu_sender_.MakeBuilder();
+ frc971::IMUValues::Builder imu_builder =
+ builder.MakeBuilder<frc971::IMUValues>();
+ imu_builder.add_gyro_x(0.0);
+ imu_builder.add_gyro_y(0.0);
+ imu_builder.add_gyro_z(
(drivetrain_plant_.X(3, 0) - drivetrain_plant_.X(1, 0)) /
(dt_config_.robot_radius * 2.0));
- builder.Send(gyro_builder.Finish());
+ // Acceleration due to gravity, in m/s/s.
+ constexpr double kG = 9.807;
+ imu_builder.add_accelerometer_x(last_acceleration_.x() / kG);
+ imu_builder.add_accelerometer_y(0.0);
+ imu_builder.add_accelerometer_z(1.0);
+ imu_builder.add_monotonic_timestamp_ns(
+ std::chrono::duration_cast<std::chrono::nanoseconds>(
+ event_loop_->monotonic_now().time_since_epoch())
+ .count());
+ builder.Send(imu_builder.Finish());
}
}
@@ -216,13 +224,18 @@
U(1, 0) += drivetrain_plant_.right_voltage_offset();
drivetrain_plant_.Update(U);
double dt_float = ::aos::time::DurationInSeconds(dt_config_.dt);
- state_ = RungeKuttaU(
+ const auto dynamics =
[this](const ::Eigen::Matrix<double, 5, 1> &X,
const ::Eigen::Matrix<double, 2, 1> &U) {
return ContinuousDynamics(velocity_drivetrain_->plant(),
dt_config_.Tlr_to_la(), X, U);
- },
- state_, U, dt_float);
+ };
+ state_ = RungeKuttaU(dynamics, state_, U, dt_float);
+ const Eigen::Matrix<double, 5, 1> Xdot = dynamics(state_, U);
+ // TODO(james): Account for centripetal accelerations.
+ // TODO(james): Allow inputting arbitrary calibrations, e.g., for testing
+ // situations where the IMU is not perfectly flat in the CG of the robot.
+ last_acceleration_ << (Xdot(3, 0) + Xdot(4, 0)) / 2.0, 0.0, 0.0;
}
void DrivetrainSimulation::MaybePlot() {
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index 2284f63..a906ae2 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/queues/gyro_generated.h"
+#include "frc971/wpilib/imu_generated.h"
namespace frc971 {
namespace control_loops {
@@ -89,7 +89,7 @@
drivetrain_output_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Sender<::frc971::sensors::GyroReading> gyro_reading_sender_;
+ ::aos::Sender<::frc971::IMUValues> imu_sender_;
DrivetrainConfig<double> dt_config_;
@@ -106,6 +106,9 @@
Eigen::Matrix<double, 2, 1> last_U_;
+ // Last robot acceleration, in m/s/s.
+ Eigen::Vector3d last_acceleration_;
+
bool left_gear_high_ = false;
bool right_gear_high_ = false;
bool first_ = true;
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.cc b/frc971/control_loops/drivetrain/improved_down_estimator.cc
index 31de9fe..8b2ff4c 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.cc
@@ -140,7 +140,7 @@
void QuaternionUkf::Predict(const Eigen::Matrix<double, 3, 1> &U,
const Eigen::Matrix<double, 3, 1> &measurement,
- const double dt) {
+ const aos::monotonic_clock::duration dt) {
// Compute the sigma points.
// Our system is pretty linear. The traditional way of dealing with process
// noise is to augment your state vector with the mean of the process noise,
@@ -286,6 +286,36 @@
return X;
}
+flatbuffers::Offset<DownEstimatorState> DrivetrainUkf::PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const {
+ DownEstimatorState::Builder builder(*fbb);
+ builder.add_quaternion_x(X_hat().x());
+ builder.add_quaternion_y(X_hat().y());
+ builder.add_quaternion_z(X_hat().z());
+ builder.add_quaternion_w(X_hat().w());
+
+ {
+ // Note that this algorithm is not very numerically stable near pitches of
+ // +/- pi / 2.
+ const Eigen::Vector3d robot_x_in_global_frame =
+ X_hat() * Eigen::Vector3d::UnitX();
+ builder.add_yaw(
+ std::atan2(robot_x_in_global_frame.y(), robot_x_in_global_frame.x()));
+ const double xy_norm = robot_x_in_global_frame.block<2, 1>(0, 0).norm();
+ builder.add_longitudinal_pitch(
+ std::atan2(-robot_x_in_global_frame.z(), xy_norm));
+ }
+ {
+ const Eigen::Vector3d robot_y_in_global_frame =
+ X_hat() * Eigen::Vector3d::UnitY();
+ const double xy_norm = robot_y_in_global_frame.block<2, 1>(0, 0).norm();
+ builder.add_lateral_pitch(
+ std::atan2(robot_y_in_global_frame.z(), xy_norm));
+ }
+
+ return builder.Finish();
+}
+
} // namespace drivetrain
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator.h b/frc971/control_loops/drivetrain/improved_down_estimator.h
index 75accfa..aa45e4e 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator.h
+++ b/frc971/control_loops/drivetrain/improved_down_estimator.h
@@ -4,6 +4,9 @@
#include "Eigen/Dense"
#include "Eigen/Geometry"
+#include "aos/events/event_loop.h"
+#include "aos/time/time.h"
+#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/runge_kutta.h"
#include "glog/logging.h"
@@ -134,13 +137,14 @@
// the input to the system used by the filter.
void Predict(const Eigen::Matrix<double, kNumInputs, 1> &U,
const Eigen::Matrix<double, kNumMeasurements, 1> &measurement,
- const double dt);
+ const aos::monotonic_clock::duration dt);
// Returns the updated state for X after one time step, given the current
// state and gyro measurements.
virtual Eigen::Matrix<double, kNumStates, 1> A(
const Eigen::Matrix<double, kNumStates, 1> &X,
- const Eigen::Matrix<double, kNumInputs, 1> &U, const double dt) const = 0;
+ const Eigen::Matrix<double, kNumInputs, 1> &U,
+ const aos::monotonic_clock::duration dt) const = 0;
// Returns the current expected accelerometer measurements given the current
// state.
@@ -207,9 +211,10 @@
Eigen::Matrix<double, kNumStates, 1> A(
const Eigen::Matrix<double, kNumStates, 1> &X,
const Eigen::Matrix<double, kNumInputs, 1> &U,
- const double dt) const override {
+ const aos::monotonic_clock::duration dt) const override {
return RungeKutta(
- std::bind(&QuaternionDerivative, U, std::placeholders::_1), X, dt);
+ std::bind(&QuaternionDerivative, U, std::placeholders::_1), X,
+ aos::time::DurationInSeconds(dt));
}
// Returns the expected accelerometer measurement (which is just going to be
@@ -226,6 +231,9 @@
Xquat.conjugate() * Eigen::Matrix<double, 3, 1>(0.0, 0.0, 1.0);
return gprime;
}
+
+ flatbuffers::Offset<DownEstimatorState> PopulateStatus(
+ flatbuffers::FlatBufferBuilder *fbb) const;
};
} // namespace drivetrain
diff --git a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
index 31adaf2..26b27e9 100644
--- a/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
+++ b/frc971/control_loops/drivetrain/improved_down_estimator_test.cc
@@ -99,7 +99,7 @@
Eigen::Matrix<double, 3, 1> measurement;
measurement.setZero();
for (int ii = 0; ii < 200; ++ii) {
- dtukf.Predict(ux * M_PI_2, measurement, 0.005);
+ dtukf.Predict(ux * M_PI_2, measurement, std::chrono::milliseconds(5));
}
const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, ux));
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
@@ -110,6 +110,48 @@
.norm(), 1e-10);
}
+// Tests that the euler angles in the status message are correct.
+TEST(RungeKuttaTest, UkfEulerStatus) {
+ drivetrain::DrivetrainUkf dtukf;
+ const Eigen::Vector3d ux = Eigen::Vector3d::UnitX();
+ const Eigen::Vector3d uy = Eigen::Vector3d::UnitY();
+ const Eigen::Vector3d uz = Eigen::Vector3d::UnitZ();
+ // First, rotate 3 radians in the yaw axis, then 0.5 radians in the pitch
+ // axis, and then 0.1 radians about the roll axis.
+ constexpr double kYaw = 3.0;
+ constexpr double kPitch = 0.5;
+ constexpr double kRoll = 0.1;
+ Eigen::Matrix<double, 3, 1> measurement;
+ measurement.setZero();
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(uz * kYaw, measurement, std::chrono::milliseconds(5));
+ }
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(uy * kPitch, measurement, std::chrono::milliseconds(5));
+ }
+ for (int ii = 0; ii < 200; ++ii) {
+ dtukf.Predict(ux * kRoll, measurement, std::chrono::milliseconds(5));
+ }
+ const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(kYaw, uz) *
+ Eigen::AngleAxis<double>(kPitch, uy) *
+ Eigen::AngleAxis<double>(kRoll, ux));
+ flatbuffers::FlatBufferBuilder fbb;
+ fbb.ForceDefaults(1);
+ fbb.Finish(dtukf.PopulateStatus(&fbb));
+
+ aos::FlatbufferDetachedBuffer<drivetrain::DownEstimatorState> state(
+ fbb.Release());
+ EXPECT_EQ(kPitch, state.message().longitudinal_pitch());
+ EXPECT_EQ(kYaw, state.message().yaw());
+ // The longitudinal pitch is not actually the same number as the roll, so we
+ // don't check it here.
+
+ EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
+ << "Expected: " << expected.coeffs()
+ << " Got: " << dtukf.X_hat().coeffs();
+}
+
+
// Tests that if the gyro indicates no movement but that the accelerometer shows
// that we are slightly rotated, that we eventually adjust our estimate to be
// correct.
@@ -127,7 +169,7 @@
(Eigen::Vector3d(0.0, 0.0, 1.0) - dtukf.H(dtukf.X_hat().coeffs()))
.norm());
for (int ii = 0; ii < 200; ++ii) {
- dtukf.Predict({0.0, 0.0, 0.0}, measurement, 0.005);
+ dtukf.Predict({0.0, 0.0, 0.0}, measurement, std::chrono::milliseconds(5));
}
const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(M_PI_2, ux));
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 0.01))
@@ -147,7 +189,7 @@
// are only rotating about the Z (yaw) axis.
measurement << 0.3, 1.0, 0.0;
for (int ii = 0; ii < 200; ++ii) {
- dtukf.Predict({0.0, 0.0, 1.0}, measurement, 0.005);
+ dtukf.Predict({0.0, 0.0, 1.0}, measurement, std::chrono::milliseconds(5));
}
const Eigen::Quaterniond expected(Eigen::AngleAxis<double>(1.0, uz));
EXPECT_TRUE(QuaternionEqual(expected, dtukf.X_hat(), 1e-1))
diff --git a/frc971/zeroing/imu_zeroer.h b/frc971/zeroing/imu_zeroer.h
index e95c231..5f53e0d 100644
--- a/frc971/zeroing/imu_zeroer.h
+++ b/frc971/zeroing/imu_zeroer.h
@@ -11,6 +11,8 @@
class ImuZeroer {
public:
// Average 5 seconds of data (assuming 2kHz sampling rate).
+ // TODO(james): Make the gyro zero in a constant amount of time, rather than a
+ // constant number of samples...
static constexpr size_t kSamplesToAverage = 10000.0;
ImuZeroer();