Reduce mallocs when constructing trajectories
We are mostly allocating because we are lazy. Allocate some of the data
up front and pass around pointers, and use spans to represent the rest
of the data and either point it at vectors, or at the underlying
flatbuffer.
Change-Id: I819dfaa85bc1ba9895eb92efc74a1540d93a6c38
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 629150c..5a4e5da 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -1,9 +1,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "Eigen/Dense"
-
#include "aos/json_to_flatbuffer.h"
-
#include "aos/realtime.h"
#include "aos/util/math.h"
#include "frc971/control_loops/control_loops_generated.h"
@@ -18,6 +16,11 @@
SplineDrivetrain::SplineDrivetrain(const DrivetrainConfig<double> &dt_config)
: dt_config_(dt_config),
+ velocity_drivetrain_(
+ std::make_shared<StateFeedbackLoop<2, 2, 2, double,
+ StateFeedbackHybridPlant<2, 2, 2>,
+ HybridKalman<2, 2, 2>>>(
+ dt_config_.make_hybrid_drivetrain_velocity_loop())),
current_xva_(Eigen::Vector3d::Zero()),
next_xva_(Eigen::Vector3d::Zero()),
next_U_(Eigen::Vector2d::Zero()) {}
@@ -73,8 +76,8 @@
}
void SplineDrivetrain::AddTrajectory(const fb::Trajectory *trajectory) {
- trajectories_.emplace_back(
- std::make_unique<FinishedTrajectory>(dt_config_, trajectory));
+ trajectories_.emplace_back(std::make_unique<FinishedTrajectory>(
+ dt_config_, trajectory, velocity_drivetrain_));
UpdateSplineHandles();
}