Fix broken move constructor in HybridStateFeedbackLoop
This was corrupting P and causing the observer to fail.
Change-Id: I5f99802536266e929013745b681e14715afc1d13
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index a6160e7..d769541 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -75,6 +75,9 @@
::std::swap(coefficients_, other.coefficients_);
X_.swap(other.X_);
Y_.swap(other.Y_);
+ A_.swap(other.A_);
+ B_.swap(other.B_);
+ DelayedU_.swap(other.DelayedU_);
}
virtual ~StateFeedbackHybridPlant() {}
@@ -167,8 +170,6 @@
DelayedU_ = U;
}
- Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
-
Eigen::Matrix<Scalar, number_of_states, 1> Update(
const Eigen::Matrix<Scalar, number_of_states, 1> X,
const Eigen::Matrix<Scalar, number_of_inputs, 1> &U,
@@ -202,6 +203,8 @@
number_of_states, number_of_inputs, number_of_outputs>>>
coefficients_;
+ Eigen::Matrix<Scalar, number_of_inputs, 1> DelayedU_;
+
int index_;
DISALLOW_COPY_AND_ASSIGN(StateFeedbackHybridPlant);
@@ -243,9 +246,13 @@
*observers)
: coefficients_(::std::move(*observers)) {}
- HybridKalman(HybridKalman &&other)
- : X_hat_(other.X_hat_), index_(other.index_) {
+ HybridKalman(HybridKalman &&other) : index_(other.index_) {
::std::swap(coefficients_, other.coefficients_);
+
+ X_hat_.swap(other.X_hat_);
+ P_.swap(other.P_);
+ Q_.swap(other.Q_);
+ R_.swap(other.R_);
}
// Getters for Q
diff --git a/frc971/control_loops/state_feedback_loop.h b/frc971/control_loops/state_feedback_loop.h
index 83a54f0..d63bca7 100644
--- a/frc971/control_loops/state_feedback_loop.h
+++ b/frc971/control_loops/state_feedback_loop.h
@@ -397,11 +397,11 @@
: plant_(::std::move(other.plant_)),
controller_(::std::move(other.controller_)),
observer_(::std::move(other.observer_)) {
+ ff_U_.swap(other.ff_U_);
R_.swap(other.R_);
next_R_.swap(other.next_R_);
U_.swap(other.U_);
U_uncapped_.swap(other.U_uncapped_);
- ff_U_.swap(other.ff_U_);
}
virtual ~StateFeedbackLoop() {}