diff --git a/scripts/fgc b/scripts/fgc index 99bfffc..720b8e5 100755 --- a/scripts/fgc +++ b/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" } diff --git a/src/core/Application.cpp b/src/core/Application.cpp index e4b87f9..88b0a27 100644 --- a/src/core/Application.cpp +++ b/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; diff --git a/src/serial/SerialMotorController.cpp b/src/serial/SerialMotorController.cpp index 8faa9bb..c15e709 100644 --- a/src/serial/SerialMotorController.cpp +++ b/src/serial/SerialMotorController.cpp @@ -5,7 +5,9 @@ #include "fgc/TelemetryParser.h" #include +#include #include +#include #include #include #include @@ -81,6 +83,14 @@ struct SerialMotorController::Impl { // NEED capture: single lines, queued for the control thread to answer once. std::vector 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 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 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(cmd + "\n"); - auto trace = std::make_shared(cmd); - boost::asio::post(io, [this, data, trace] { - LOG_TRACE_CAT(LogCat::Serial) << "TX " << *trace; - boost::asio::async_write(serial, boost::asio::buffer(*data), - [data](const boost::system::error_code& ec, std::size_t) { - if (ec) LOG_WARN << "Serial write failed: " << ec.message(); - }); + ++tx_pending; + boost::asio::post(io, [this, line = cmd] { + LOG_TRACE_CAT(LogCat::Serial) << "TX " << line; + tx_queue.push_back(line + "\n"); + if (tx_queue.size() == 1) writeNext(); // otherwise one is in flight }); } + // 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;