Stop the program cleanly on a service stop, and leave the gimbal at rest before configuring its motors

scripts/fgc

@@ -233,10 +233,14 @@ cmd__run() {
 
     # A trapped signal makes `wait` return 128+n while the child is still
     # shutting down, so keep waiting until it is actually gone before reporting.
+    # `|| rc=$?`, not `; rc=$?`: under `set -e` that non-zero `wait` exited this
+    # wrapper on the spot, the pane died, and the app was killed mid-teardown -
+    # every `fgc stop` and systemd stop was an unclean kill.
     local rc=0
-    wait "$child"; rc=$?
+    wait "$child" || rc=$?
     while kill -0 "$child" 2>/dev/null; do
-        wait "$child"; rc=$?
+        rc=0
+        wait "$child" || rc=$?
     done
     exit "$rc"
 }

src/core/Application.cpp

@@ -1449,11 +1449,41 @@ struct Application::Impl {
         return safe;
     }
 
+    // A homed axis that is not standing still: a move nobody is watching. Homing
+    // is left alone - it ends itself, and a STOP would end it in ERROR instead.
+    static bool leftoverMove(const AxisTelemetry& a) {
+        return a.state == AxisState::Ready && !a.standstill;
+    }
+    bool anyLeftoverMove() {
+        const MotorTelemetry t = motor->telemetry();
+        return leftoverMove(t.yaw) || (t.pitch_present && leftoverMove(t.pitch));
+    }
+
+    // A program stopped mid-sweep leaves the firmware finishing its last MOVE,
+    // and the firmware refuses to re-scale a moving motor: the CURRENT sent
+    // below would be refused and this start would run on whatever current the
+    // board last had. So stop that move and let it come to rest first.
+    void stopLeftoverMotion() {
+        using namespace std::chrono_literals;
+        if (!anyLeftoverMove()) return;
+        LOG_INFO << "gimbal still moving from before this start; stopping it before "
+                    "configuring the motors";
+        motor->sendCommand("STOP ALL");
+        const auto deadline = std::chrono::steady_clock::now() + 10s;
+        while (std::chrono::steady_clock::now() < deadline) {
+            std::this_thread::sleep_for(100ms);
+            if (!anyLeftoverMove()) return;
+        }
+        LOG_WARN << "gimbal did not come to a standstill within 10 s; configuring it anyway "
+                    "(a motor current sent while it moves is refused)";
+    }
+
     // Tell the firmware the anchor point and endstop clearance for each axis.
     // The centre count is what finishHoming() re-origins onto, so all counts stay
     // positive and reproducible across power cycles; the margin is what
     // clampTarget() keeps clear of each hard limit.
     void sendAnchorConfig() {
+        stopLeftoverMotion();
         motor->sendCommand("CENTER Y " + std::to_string(cfg.geometry.yaw.center_count));
         motor->sendCommand("MARGIN Y " + std::to_string(cfg.yaw_soft_margin_counts));
         if (cfg.hardware.hasAxis("pitch")) {
@@ -2958,6 +2988,13 @@ struct Application::Impl {
         }
 
         LOG_INFO << "Shutting down";
+        // First: leave the gimbal at rest, not finishing a move nobody will
+        // watch - and that the next start would find still under way. Closing
+        // the camera below takes seconds; the gimbal should not wait for it.
+        if (anyLeftoverMove()) {
+            LOG_INFO << "stopping the gimbal's move before exit";
+            motor->sendCommand("STOP ALL");
+        }
         if (tester) tester->cancel();  // stop any in-flight self-test before teardown
         if (calib) calib->cancel();  // stop any in-flight calibration before teardown
         if (ui) ui->stop();
@@ -2966,7 +3003,7 @@ struct Application::Impl {
         camera->close();
         if (imu) imu->stop();
         if (env) env->stop();
-        motor->stop();
+        motor->stop();   // sends what is queued (the STOP above) before closing
         channel->disconnect();
         // A --test run interrupted before its verdict has no verdict to report.
         if (!opts.run_test.empty() && running && exit_code_ == 0) exit_code_ = kExitTestNotRun;

src/serial/SerialMotorController.cpp

@@ -5,7 +5,9 @@
 #include "fgc/TelemetryParser.h"
 
 #include <atomic>
+#include <chrono>
 #include <cctype>
+#include <deque>
 #include <istream>
 #include <mutex>
 #include <thread>
@@ -81,6 +83,14 @@ struct SerialMotorController::Impl {
     // NEED capture: single lines, queued for the control thread to answer once.
     std::vector<std::string>     need_lines;
 
+    // Lines waiting to go out, oldest first. Touched only on the io_context
+    // thread (every access is inside a posted handler or a completion), so it
+    // needs no lock.
+    std::deque<std::string>      tx_queue;
+    // Lines handed to sendLine and not yet written, from any thread: stop()
+    // waits on it so a last command (the STOP on exit) is not dropped unsent.
+    std::atomic<int>             tx_pending{0};
+
     // Write one command line (newline-terminated) to the controller. Used by
     // sendCommand and by the internal DUMP re-request.
     //
@@ -90,18 +100,34 @@ struct SerialMotorController::Impl {
     // scheduler's MOVEs, or a TUI command) while the io_thread runs async_read
     // on the same serial_port races the reader and corrupts inbound data — this
     // was shredding the DUMP burst whenever the link was busy.
+    //
+    // And one line at a time: async_write is a series of write_some calls, and a
+    // second async_write started before the first completes may interleave its
+    // bytes with the first's. A burst - the 24 SETs of a drivetrain re-send -
+    // then reached the firmware as a spliced line ("SET Y SW_MODE 22SET P ..."),
+    // answered "ERR bad argument" and silently lost. So lines queue, and each
+    // write starts only when the previous one has finished.
     void sendLine(const std::string& cmd) {
-        auto data  = std::make_shared<std::string>(cmd + "\n");
+        ++tx_pending;
-        auto trace = std::make_shared<std::string>(cmd);
+        boost::asio::post(io, [this, line = cmd] {
-        boost::asio::post(io, [this, data, trace] {
+            LOG_TRACE_CAT(LogCat::Serial) << "TX " << line;
-            LOG_TRACE_CAT(LogCat::Serial) << "TX " << *trace;
+            tx_queue.push_back(line + "\n");
-            boost::asio::async_write(serial, boost::asio::buffer(*data),
+            if (tx_queue.size() == 1) writeNext();   // otherwise one is in flight
-                                     [data](const boost::system::error_code& ec, std::size_t) {
-                                         if (ec) LOG_WARN << "Serial write failed: " << ec.message();
-                                     });
         });
     }
 
+    // Write the front of tx_queue; on completion drop it and go on. Runs on the
+    // io_context thread only.
+    void writeNext() {
+        boost::asio::async_write(serial, boost::asio::buffer(tx_queue.front()),
+                                 [this](const boost::system::error_code& ec, std::size_t) {
+                                     if (ec) LOG_WARN << "Serial write failed: " << ec.message();
+                                     tx_queue.pop_front();
+                                     --tx_pending;
+                                     if (!tx_queue.empty()) writeNext();
+                                 });
+    }
+
     void doRead() {
         boost::asio::async_read_until(
             serial, buffer, '\n',
@@ -252,6 +278,13 @@ void SerialMotorController::start() {
 
 void SerialMotorController::stop() {
     if (!impl_) return;
+    // Let what is queued go out first (the STOP sent on exit, above all), but
+    // never hang shutdown on a link that has stopped accepting writes.
+    if (impl_->connected) {
+        const auto deadline = std::chrono::steady_clock::now() + std::chrono::milliseconds(500);
+        while (impl_->tx_pending > 0 && std::chrono::steady_clock::now() < deadline)
+            std::this_thread::sleep_for(std::chrono::milliseconds(5));
+    }
     impl_->io.stop();
     if (impl_->io_thread.joinable()) impl_->io_thread.join();
     boost::system::error_code ec;