Tune down estimator for ADIS16505
* Update down_estimator_plotter for the ADIS16505 setup (would be
nice to refactor things to make it so that you can apply a plot
config to a particular submessage type...).
* Crank up the threshold to consider the IMU still, to avoid resetting
in the middle of real motion.
* Also check in the IMU transform from the jankily-mounted IMU on the
2020 robot.
Change-Id: I4c6791524c3436614db045d3f2ba371f2400b084
Signed-off-by: James Kuszmaul <jabukuszmaul@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.h b/frc971/control_loops/drivetrain/drivetrain_config.h
index b5990c3..9150e8a 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.h
+++ b/frc971/control_loops/drivetrain/drivetrain_config.h
@@ -40,6 +40,15 @@
IMU_Z = 3, // Use the z-axis of the IMU.
};
+struct DownEstimatorConfig {
+ // Threshold, in g's, to use for detecting whether we are stopped in the down
+ // estimator.
+ double gravity_threshold = 0.025;
+ // Number of cycles of being still to require before taking accelerometer
+ // corrections.
+ int do_accel_corrections = 50;
+};
+
template <typename Scalar = double>
struct DrivetrainConfig {
// Shifting method we are using.
@@ -107,6 +116,8 @@
// True if we are running a simulated drivetrain.
bool is_simulated = false;
+ DownEstimatorConfig down_estimator_config{};
+
// Converts the robot state to a linear distance position, velocity.
static Eigen::Matrix<Scalar, 2, 1> LeftRightToLinear(
const Eigen::Matrix<Scalar, 7, 1> &left_right) {