commit 5a543419bfa57c83b60e5dbc86a2b31c5496a7a9
author James Kuszmaul <jabukuszmaul@gmail.com> Sat Apr 13 15:49:42 2019 -0700
committer James Kuszmaul <jabukuszmaul@gmail.com> Sun Apr 14 11:43:38 2019 -0700
tree 1f526225be85d168f60202066073360678268931
parent c4eb1b2b2697881bdfb523c74478e8b89df4951f

Add distance remaining to trajectory status

Change-Id: I76b08464483f141aadae88ed53ca5814e1f75081

frc971/control_loops/drivetrain/splinedrivetrain.cc

diff --git a/frc971/control_loops/drivetrain/splinedrivetrain.cc b/frc971/control_loops/drivetrain/splinedrivetrain.cc
index 2eb434a..1f7e2b0 100644
--- a/frc971/control_loops/drivetrain/splinedrivetrain.cc
+++ b/frc971/control_loops/drivetrain/splinedrivetrain.cc
@@ -218,6 +218,9 @@
         (current_spline_idx_ != -1) && IsAtEnd();
     status->trajectory_logging.goal_spline_handle = current_spline_handle_;
     status->trajectory_logging.current_spline_idx = current_spline_idx_;
+    status->trajectory_logging.distance_remaining =
+        current_trajectory_ ? current_trajectory_->length() - current_xva_.x()
+                            : 0.0;
 
     int32_t planning_spline_idx = planning_spline_idx_;
     if (current_spline_idx_ == planning_spline_idx) {