Merge pull request #3793 from pleroy/Merge

Wilkinson is 賈憲

ksp_plugin/flight_plan.cpp

@@ -12,6 +12,7 @@
 #include "integrators/embedded_explicit_runge_kutta_nyström_integrator.hpp"
 #include "integrators/methods.hpp"
 #include "ksp_plugin/integrators.hpp"
+#include "quantities/named_quantities.hpp"
 #include "quantities/si.hpp"
 #include "testing_utilities/make_not_null.hpp"
 
@@ -24,6 +25,7 @@ using namespace principia::integrators::_embedded_explicit_generalized_runge_kut
 using namespace principia::integrators::_embedded_explicit_runge_kutta_nyström_integrator;  // NOLINT
 using namespace principia::integrators::_methods;
 using namespace principia::ksp_plugin::_integrators;
+using namespace principia::quantities::_named_quantities;
 using namespace principia::quantities::_si;
 using namespace principia::testing_utilities::_make_not_null;
 using namespace std::chrono_literals;
@@ -37,8 +39,9 @@ inline absl::Status DoesNotFit() {
   return absl::Status(FlightPlan::does_not_fit, "Does not fit");
 }
 
-inline absl::Status Singular() {
-  return absl::Status(FlightPlan::singular, "Singular");
+inline absl::Status Singular(Square<Speed> const& Δv²) {
+  return absl::Status(FlightPlan::singular,
+                      absl::StrCat("Singular: ", DebugString(Δv²)));
 }
 
 FlightPlan::FlightPlan(
@@ -87,14 +90,22 @@ FlightPlan::FlightPlan(FlightPlan const& other)
       generalized_adaptive_step_parameters_(
           other.generalized_adaptive_step_parameters_) {
   MakeProlongator(desired_final_time_);
-  bool first = true;
+  bool first_segment = true;
   for (auto const& other_segment : other.trajectory_.segments()) {
-    if (!first) {
+    if (!first_segment) {
+      // The first segment was created by the constructor of the trajectory.
       trajectory_.NewSegment();
     }
+    bool first_point = true;
     for (auto const& [time, degrees_of_freedom] : other_segment) {
-      CHECK_OK(trajectory_.Append(time, degrees_of_freedom));
+      // For segments other than the first, |NewSegment| copied the last point
+      // of the previous segment.
+      if (!first_point || first_segment) {
+        CHECK_OK(trajectory_.Append(time, degrees_of_freedom));
+      }
+      first_point = false;
     }
+    first_segment = false;
   }
   for (auto it = trajectory_.segments().begin();
        it != trajectory_.segments().end();
@@ -145,7 +156,7 @@ absl::Status FlightPlan::Insert(NavigationManœuvre::Burn const& burn,
       index == 0 ? initial_mass_ : manœuvres_[index - 1].final_mass(),
       burn);
   if (manœuvre.IsSingular()) {
-    return Singular();
+    return Singular(manœuvre.Δv().Norm²());
   }
   if (!manœuvre.FitsBetween(start_of_previous_coast(index),
                             start_of_burn(index))) {
@@ -181,7 +192,7 @@ absl::Status FlightPlan::Replace(NavigationManœuvre::Burn const& burn,
   NavigationManœuvre const manœuvre(manœuvres_[index].initial_mass(),
                                     burn);
   if (manœuvre.IsSingular()) {
-    return Singular();
+    return Singular(manœuvre.Δv().Norm²());
   }
   if (index == number_of_manœuvres() - 1) {
     // This is the last manœuvre.  If it doesn't fit just because the flight

ksp_plugin/flight_plan_optimization_driver.cpp

@@ -5,6 +5,8 @@
 #include <utility>
 
 #include "absl/status/status.h"
+#include "absl/synchronization/mutex.h"
+#include "glog/logging.h"
 
 namespace principia {
 namespace ksp_plugin {
@@ -17,12 +19,11 @@ FlightPlanOptimizationDriver::FlightPlanOptimizationDriver(
     FlightPlan const& flight_plan,
     FlightPlanOptimizer::MetricFactory metric_factory)
     : flight_plan_under_optimization_(flight_plan),
-      flight_plan_optimizer_(
-          &flight_plan_under_optimization_,
-          std::move(metric_factory),
-          std::bind(&FlightPlanOptimizationDriver::UpdateLastFlightPlan,
-                    this,
-                    _1)),
+      flight_plan_optimizer_(&flight_plan_under_optimization_,
+                             std::move(metric_factory),
+                             [this](FlightPlan const& flight_plan) {
+                               UpdateLastFlightPlan(flight_plan);
+                             }),
       last_flight_plan_(
           make_not_null_shared<FlightPlan>(flight_plan_under_optimization_)) {}
 
@@ -63,12 +64,20 @@ void FlightPlanOptimizationDriver::RequestOptimization(
       if (optimization_status.ok()) {
         last_flight_plan_ =
             make_not_null_shared<FlightPlan>(flight_plan_under_optimization_);
+      } else {
+        LOG(WARNING) << "Optimization returned " << optimization_status;
       }
       optimizer_idle_ = true;
     });
   }
 }
 
+std::optional<FlightPlanOptimizationDriver::Parameters> const&
+FlightPlanOptimizationDriver::last_parameters() const {
+  absl::ReaderMutexLock l(&lock_);
+  return last_parameters_;
+}
+
 void FlightPlanOptimizationDriver::Wait() const {
   absl::ReaderMutexLock l(&lock_);
   lock_.Await(absl::Condition(&optimizer_idle_));

ksp_plugin/flight_plan_optimization_driver.hpp

@@ -46,6 +46,10 @@ class FlightPlanOptimizationDriver {
   // optimization is already happening.
   void RequestOptimization(Parameters const& parameters);
 
+  // Returns the last parameters passed to |RequestOptimization|, or nullopt if
+  // |RequestOptimization| was not called.
+  std::optional<Parameters> const& last_parameters() const;
+
   // Waits for the current optimization (if any) to complete.
   void Wait() const;
 
@@ -59,6 +63,7 @@ class FlightPlanOptimizationDriver {
   mutable absl::Mutex lock_;
   jthread optimizer_;
   bool optimizer_idle_ GUARDED_BY(lock_) = true;
+  std::optional<Parameters> last_parameters_ GUARDED_BY(lock_);
 
   // The last flight plan evaluated by the optimizer.
   not_null<std::shared_ptr<FlightPlan>> last_flight_plan_ GUARDED_BY(lock_);