Merge changes I39201895,Icb33b4e8
* changes:
Add support for binary flatbuffer config.
Renice logger for fewer deaths
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf.h b/frc971/control_loops/drivetrain/hybrid_ekf.h
index bd358ca..4fd2d43 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf.h
+++ b/frc971/control_loops/drivetrain/hybrid_ekf.h
@@ -160,6 +160,11 @@
// offsets decay, in seconds.
static constexpr double kVelocityOffsetTimeConstant = 1.0;
static constexpr double kLateralVelocityTimeConstant = 1.0;
+ // The maximum allowable timestep--we use this to check for situations where
+ // measurement updates come in too infrequently and this might cause the
+ // integrator and discretization in the prediction step to be overly
+ // aggressive.
+ static constexpr std::chrono::milliseconds kMaxTimestep{20};
// Inputs are [left_volts, right_volts]
typedef Eigen::Matrix<Scalar, kNInputs, 1> Input;
// Outputs are either:
@@ -559,7 +564,7 @@
State *state, StateSquare *P) {
*state = obs->X_hat;
*P = obs->P;
- if (dt.count() != 0) {
+ if (dt.count() != 0 && dt < kMaxTimestep) {
PredictImpl(obs, dt, state, P);
}
if (!(obs->h && obs->dhdx)) {
diff --git a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
index bdfdd4e..48b8b5f 100644
--- a/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
+++ b/frc971/control_loops/drivetrain/hybrid_ekf_test.cc
@@ -406,6 +406,19 @@
}
}
+// Tests that if we have an unusually large gap between measurement updates that
+// nothing crazy happens (a previous implementation had a bug where large
+// time-steps would mess up the prediction step due to how we approximated
+// things).
+TEST_F(HybridEkfTest, ExtraLongUpdateTime) {
+ Input U;
+ U << 0.0, 0.0, 0.1, 0.1;
+ ekf_.RawUpdateEncodersAndGyro(0.0, 0.0, 0.0, U, t0_ + dt_config_.dt);
+ ekf_.RawUpdateEncodersAndGyro(0.0, 0.0, 0.0, U,
+ t0_ + std::chrono::seconds(1000));
+ EXPECT_LT(ekf_.X_hat().norm(), 10.0) << ekf_.X_hat();
+}
+
// Tests encoder/gyro updates when we have some errors in our estimate.
TEST_F(HybridEkfTest, PerfectEncoderUpdateConverges) {
// In order to simulate modelling errors, we add an angular_error and start
diff --git a/frc971/control_loops/position_sensor_sim.cc b/frc971/control_loops/position_sensor_sim.cc
index b874b1d..b605196 100644
--- a/frc971/control_loops/position_sensor_sim.cc
+++ b/frc971/control_loops/position_sensor_sim.cc
@@ -73,8 +73,10 @@
known_absolute_encoder_ = known_absolute_encoder_pos;
- lower_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
- upper_index_edge_.mutable_pot_noise()->set_standard_deviation(pot_noise_stddev);
+ lower_index_edge_.mutable_pot_noise()->set_standard_deviation(
+ pot_noise_stddev);
+ upper_index_edge_.mutable_pot_noise()->set_standard_deviation(
+ pot_noise_stddev);
}
void PositionSensorSimulator::InitializeHallEffectAndPosition(
@@ -230,6 +232,5 @@
return builder->Finish();
}
-
} // namespace control_loops
} // namespace frc971
diff --git a/frc971/control_loops/position_sensor_sim_test.cc b/frc971/control_loops/position_sensor_sim_test.cc
index 93a648a..424a3af 100644
--- a/frc971/control_loops/position_sensor_sim_test.cc
+++ b/frc971/control_loops/position_sensor_sim_test.cc
@@ -4,10 +4,10 @@
#include <random>
-#include "gtest/gtest.h"
+#include "aos/die.h"
#include "frc971/control_loops/control_loops_generated.h"
#include "frc971/control_loops/position_sensor_sim.h"
-#include "aos/die.h"
+#include "gtest/gtest.h"
#include "flatbuffers/flatbuffers.h"
@@ -33,7 +33,8 @@
// Make sure that we don't accidentally hit an index pulse.
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.6 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.6 * index_diff, pot_and_index_position->pot());
@@ -44,7 +45,8 @@
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
@@ -54,7 +56,8 @@
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.99 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.99 * index_diff, pot_and_index_position->pot());
@@ -64,7 +67,8 @@
for (int i = 0; i < 30; i++) {
sim.MoveTo(3.0 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_DOUBLE_EQ(3.0 * index_diff, pot_and_index_position->pot());
@@ -85,7 +89,8 @@
sim.Initialize(4.6 * index_diff, 0);
// Make sure that we get an index pulse on every transition.
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
ASSERT_EQ(0u, index_position->index_pulses());
@@ -144,14 +149,16 @@
flatbuffers::FlatBufferBuilder pot_fbb;
sim.MoveTo(0.75 * index_diff);
- const IndexPosition *index_position = sim.FillSensorValues<IndexPosition>(&fbb);
+ const IndexPosition *index_position =
+ sim.FillSensorValues<IndexPosition>(&fbb);
const PotAndIndexPosition *pot_and_index_position =
sim.FillSensorValues<PotAndIndexPosition>(&pot_fbb);
EXPECT_EQ(1u, index_position->index_pulses());
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -160,7 +167,8 @@
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -169,7 +177,8 @@
EXPECT_EQ(2u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25,
+ pot_and_index_position->latched_encoder());
// Try it with our known index pulse not being our first one.
sim.Initialize(index_diff * 0.25, 0.0, index_diff * 1.5);
@@ -181,7 +190,8 @@
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -190,7 +200,8 @@
EXPECT_EQ(1u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 0.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 0.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 0.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 0.25,
+ pot_and_index_position->latched_encoder());
sim.MoveTo(1.75 * index_diff);
index_position = sim.FillSensorValues<IndexPosition>(&fbb);
@@ -199,7 +210,8 @@
EXPECT_EQ(2u, pot_and_index_position->index_pulses());
EXPECT_DOUBLE_EQ(index_diff * 1.5, pot_and_index_position->latched_pot());
EXPECT_DOUBLE_EQ(index_diff * 1.25, index_position->latched_encoder());
- EXPECT_DOUBLE_EQ(index_diff * 1.25, pot_and_index_position->latched_encoder());
+ EXPECT_DOUBLE_EQ(index_diff * 1.25,
+ pot_and_index_position->latched_encoder());
}
// Tests that the latched values update correctly.
@@ -320,7 +332,7 @@
const double index_diff = 1.0;
PositionSensorSimulator sim(index_diff);
sim.InitializeHallEffectAndPosition(-0.25, 0.0, 0.5);
- //HallEffectAndPosition position;
+ // HallEffectAndPosition position;
flatbuffers::FlatBufferBuilder fbb;
// Go over only the lower edge rising.
@@ -397,6 +409,5 @@
}
}
-
} // namespace control_loops
} // namespace frc971