Merge changes I9decb651,Id57ac32f,I552be858
* changes:
Tune the spline controller for auto modes
Tune the y2023 localizer a bit
Allow ImuWatcher to ignore pico timestamps
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index 0d100ed..f448760 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -67,9 +67,10 @@
// Returns the current localizer state.
Eigen::Matrix<double, 5, 1> trajectory_state() {
+ // Use the regular kalman filter's left/right velocity because they are
+ // generally smoother.
return (Eigen::Matrix<double, 5, 1>() << localizer_->x(), localizer_->y(),
- localizer_->theta(), localizer_->left_velocity(),
- localizer_->right_velocity())
+ localizer_->theta(), DrivetrainXHat()(1), DrivetrainXHat()(3))
.finished();
}
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 824f0a8..edda034 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -657,8 +657,8 @@
CHECK_NOTNULL(drivetrain_status_fetcher_->trajectory_logging())->theta();
// As a sanity check, compare both against absolute angle and the spline's
// goal angle.
- EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 2e-2);
- EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 2e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), 0.0), 5e-2);
+ EXPECT_NEAR(0.0, ::aos::math::DiffAngle(actual(2), expected_theta), 5e-2);
}
// Tests that simple spline with a single goal message.
@@ -1005,7 +1005,7 @@
const ::Eigen::Vector2d actual = drivetrain_plant_.GetPosition();
// Expect the x position comparison to fail; everything else to succeed.
spline_estimate_tolerance_ = 0.11;
- spline_control_tolerance_ = 0.11;
+ spline_control_tolerance_ = 0.09;
EXPECT_GT(std::abs(estimated_x - expected_x), spline_control_tolerance_);
EXPECT_NEAR(estimated_y, expected_y, spline_control_tolerance_);
EXPECT_NEAR(actual(0), estimated_x, spline_estimate_tolerance_);
diff --git a/frc971/control_loops/drivetrain/trajectory.cc b/frc971/control_loops/drivetrain/trajectory.cc
index 27b31a0..33a8577 100644
--- a/frc971/control_loops/drivetrain/trajectory.cc
+++ b/frc971/control_loops/drivetrain/trajectory.cc
@@ -765,7 +765,7 @@
// TODO(james): Pull these out into a config.
Eigen::Matrix<double, 5, 5> Q;
Q.setIdentity();
- Q.diagonal() << 20.0, 20.0, 20.0, 10.0, 10.0;
+ Q.diagonal() << 30.0, 30.0, 20.0, 15.0, 15.0;
Q *= 2.0;
Q = (Q * Q).eval();
diff --git a/frc971/imu_reader/imu_watcher.cc b/frc971/imu_reader/imu_watcher.cc
index ed9c65f..e4e100a 100644
--- a/frc971/imu_reader/imu_watcher.cc
+++ b/frc971/imu_reader/imu_watcher.cc
@@ -18,7 +18,8 @@
std::function<
void(aos::monotonic_clock::time_point, aos::monotonic_clock::time_point,
std::optional<Eigen::Vector2d>, Eigen::Vector3d, Eigen::Vector3d)>
- callback)
+ callback,
+ TimestampSource timestamp_source)
: dt_config_(dt_config),
callback_(std::move(callback)),
zeroer_(zeroing::ImuZeroer::FaultBehavior::kTemporary),
@@ -28,7 +29,8 @@
right_encoder_(
-EncoderWrapDistance(drivetrain_distance_per_encoder_tick) / 2.0,
EncoderWrapDistance(drivetrain_distance_per_encoder_tick)) {
- event_loop->MakeWatcher("/localizer", [this](const IMUValuesBatch &values) {
+ event_loop->MakeWatcher("/localizer", [this, timestamp_source](
+ const IMUValuesBatch &values) {
CHECK(values.has_readings());
for (const IMUValues *value : *values.readings()) {
zeroer_.InsertAndProcessMeasurement(*value);
@@ -69,18 +71,21 @@
left_encoder_.Unwrap(value->left_encoder()),
right_encoder_.Unwrap(value->right_encoder())});
{
- // If we can't trust the imu reading, just naively increment the
- // pico timestamp.
- const aos::monotonic_clock::time_point pico_timestamp =
- zeroer_.Faulted()
- ? (last_pico_timestamp_.has_value()
- ? last_pico_timestamp_.value() + kNominalDt
- : aos::monotonic_clock::epoch())
- : aos::monotonic_clock::time_point(
- std::chrono::microseconds(value->pico_timestamp_us()));
const aos::monotonic_clock::time_point pi_read_timestamp =
aos::monotonic_clock::time_point(
std::chrono::nanoseconds(value->monotonic_timestamp_ns()));
+ // If we can't trust the imu reading, just naively increment the
+ // pico timestamp.
+ const aos::monotonic_clock::time_point pico_timestamp =
+ timestamp_source == TimestampSource::kPi
+ ? pi_read_timestamp
+ : (zeroer_.Faulted()
+ ? (last_pico_timestamp_.has_value()
+ ? last_pico_timestamp_.value() + kNominalDt
+ : aos::monotonic_clock::epoch())
+ : aos::monotonic_clock::time_point(
+ std::chrono::microseconds(
+ value->pico_timestamp_us())));
// TODO(james): If we get large enough drift off of the pico,
// actually do something about it.
if (!pico_offset_.has_value()) {
diff --git a/frc971/imu_reader/imu_watcher.h b/frc971/imu_reader/imu_watcher.h
index 8867266..fe1f77d 100644
--- a/frc971/imu_reader/imu_watcher.h
+++ b/frc971/imu_reader/imu_watcher.h
@@ -20,6 +20,16 @@
// Expected frequency of messages from the pico-based IMU.
static constexpr std::chrono::microseconds kNominalDt{500};
+ enum class TimestampSource {
+ // Use the pico's timestamp to provide timestamps to the callbacks.
+ kPico,
+ // Use pi-based timestamps--this can result in a clock that has marginally
+ // more jitter relative to the sample times than the pico's clock, but
+ // is less likely to encounter major issues when there is some sort of issue
+ // with the pico <-> pi interface.
+ kPi,
+ };
+
// The callback specified by the user will take:
// sample_time_pico: The pico-based timestamp corresponding to the measurement
// time. This will be offset by roughly pico_offset_error from the pi's
@@ -40,7 +50,8 @@
std::function<void(
aos::monotonic_clock::time_point, aos::monotonic_clock::time_point,
std::optional<Eigen::Vector2d>, Eigen::Vector3d, Eigen::Vector3d)>
- callback);
+ callback,
+ TimestampSource timestamp_source = TimestampSource::kPico);
const zeroing::ImuZeroer &zeroer() const { return zeroer_; }
diff --git a/y2023/localizer/localizer.cc b/y2023/localizer/localizer.cc
index 32f5604..4bb07e2 100644
--- a/y2023/localizer/localizer.cc
+++ b/y2023/localizer/localizer.cc
@@ -13,12 +13,19 @@
"Scale the target pose distortion factor by this when computing "
"the noise.");
DEFINE_double(
- max_implied_yaw_error, 30.0,
+ max_implied_yaw_error, 3.0,
"Reject target poses that imply a robot yaw of more than this many degrees "
"off from our estimate.");
-DEFINE_double(max_distance_to_target, 6.0,
+DEFINE_double(
+ max_implied_teleop_yaw_error, 30.0,
+ "Reject target poses that imply a robot yaw of more than this many degrees "
+ "off from our estimate.");
+DEFINE_double(max_distance_to_target, 3.5,
"Reject target poses that have a 3d distance of more than this "
"many meters.");
+DEFINE_double(max_auto_image_robot_speed, 2.0,
+ "Reject target poses when the robot is travelling faster than "
+ "this speed in auto.");
namespace y2023::localizer {
namespace {
@@ -97,7 +104,8 @@
y2023::constants::Values::DrivetrainEncoderToMeters(1),
std::bind(&Localizer::HandleImu, this, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3,
- std::placeholders::_4, std::placeholders::_5)),
+ std::placeholders::_4, std::placeholders::_5),
+ frc971::controls::ImuWatcher::TimestampSource::kPi),
utils_(event_loop),
status_sender_(event_loop->MakeSender<Status>("/localizer")),
output_sender_(event_loop->MakeSender<frc971::controls::LocalizerOutput>(
@@ -335,8 +343,19 @@
double camera_implied_theta = Z(Corrector::kTheta);
constexpr double kDegToRad = M_PI / 180.0;
- if (std::abs(camera_implied_theta - theta_at_capture) >
- FLAGS_max_implied_yaw_error * kDegToRad) {
+ const double robot_speed =
+ (state_at_capture.value()(StateIdx::kLeftVelocity) +
+ state_at_capture.value()(StateIdx::kRightVelocity)) /
+ 2.0;
+ const double yaw_threshold =
+ (utils_.MaybeInAutonomous() ? FLAGS_max_implied_yaw_error
+ : FLAGS_max_implied_teleop_yaw_error) *
+ kDegToRad;
+ if (utils_.MaybeInAutonomous() &&
+ (std::abs(robot_speed) > FLAGS_max_auto_image_robot_speed)) {
+ return RejectImage(camera_index, RejectionReason::ROBOT_TOO_FAST, &builder);
+ } else if (std::abs(aos::math::NormalizeAngle(
+ camera_implied_theta - theta_at_capture)) > yaw_threshold) {
return RejectImage(camera_index, RejectionReason::HIGH_IMPLIED_YAW_ERROR,
&builder);
} else if (distance_to_target > FLAGS_max_distance_to_target) {
diff --git a/y2023/localizer/status.fbs b/y2023/localizer/status.fbs
index ded492e..0ea779f 100644
--- a/y2023/localizer/status.fbs
+++ b/y2023/localizer/status.fbs
@@ -22,6 +22,8 @@
// Pose estimate had a high distance to target.
// We don't trust estimates very far out.
HIGH_DISTANCE_TO_TARGET = 6,
+ // The robot was travelling too fast; we don't trust the target.
+ ROBOT_TOO_FAST = 7,
}
table RejectionCount {