Process LocalizerOutput messages in 2022 drivetrain
Make it so that the roborio will actually pay attention to the pi's
localizer.
Change-Id: I8db97cbba65beef8c2ec863a792e57960ead996a
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 767fb61..66a3dcd 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -204,7 +204,9 @@
break;
}
- ready_ = imu_zeroer_.Zeroed();
+ ready_ = dt_config_.gyro_type == GyroType::SPARTAN_GYRO ||
+ dt_config_.gyro_type == GyroType::FLIPPED_SPARTAN_GYRO ||
+ imu_zeroer_.Zeroed();
// TODO(james): How aggressively can we fault here? If we fault to
// aggressively, we might have issues during startup if wpilib_interface takes
@@ -213,11 +215,18 @@
last_gyro_rate_ = 0.0;
}
- localizer_->Update(
- {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
- monotonic_now, position->left_encoder(), position->right_encoder(),
- down_estimator_.avg_recent_yaw_rates(),
- down_estimator_.avg_recent_accel());
+ if (imu_values_fetcher_.valid()) {
+ localizer_->Update(
+ {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
+ monotonic_now, position->left_encoder(), position->right_encoder(),
+ down_estimator_.avg_recent_yaw_rates(),
+ down_estimator_.avg_recent_accel());
+ } else {
+ localizer_->Update(
+ {last_last_voltage_(kLeftVoltage), last_last_voltage_(kRightVoltage)},
+ monotonic_now, position->left_encoder(), position->right_encoder(),
+ last_gyro_rate_, Eigen::Vector3d::Zero());
+ }
// If we get a new message setting the absolute position, then reset the
// localizer.