ActuatorCommander.hpp

Go to the documentation of this file.

#pragma once
 
#include <atomic>
#include <map>
#include <mutex>
#include <set>
#include <string>
#include <thread>
#include <vector>
 
#include "control/StateMachine.hpp"
#include "elodin/ElodinClient.hpp"
 
namespace sequencer {
 
/**
 * Config entry for one named actuator role loaded from config.toml [actuator_roles].
 *
 * Format in config.toml:  role_name = ["TYPE", channel, board_id]
 *   TYPE:  "NC"  — normally closed (logical pos = hw state)
 *          "NO"  — normally open   (logical pos inverted for hw: 0→hw1, 1→hw0)
 *          An entry with a 4th element ("pwm_fuel"/"pwm_ox") is controlled by
 *          controller_service, not by the sequencer, and is skipped in FIRE.
 */
struct ActuatorRole {
    int channel{0};        // 1-based actuator ID on the board
    int board_id{0};       // Board ID (e.g. 11, 12, 13, 14)
    std::string board_ip;  // IP of the board that owns this actuator
    bool is_no{false};     // Normally Open: invert the logical position
    bool is_pwm{false};    // assigned to controller_service: sequencer skips it in FIRE
    // Elodin table its commanded state is published on: [0x32] ACT_CMD for actuator boards,
    // [0x35] BV_CMD for a ball valve board, which has its own namespace so board 71 and actuator
    // board 11 (both slot 1) never write the same key.
    uint8_t cmd_table_hi{0x32};
};
 
/**
 * Sends UDP actuator commands based on state transitions.
 * Manages the continuous re-send loop (~10 Hz). Debug mode: manual overrides apply until
 * cleared (Sequencer clears them on every state transition so the CSV wins when changing state).
 *
 * Thread-safe: applyForState, setManualOverride, clearAllManualOverrides, stopContinuousLoop.
 */
class ActuatorCommander {
public:
    /**
     * Load actuator roles from config content and state→actuator map from CSV.
     * @param config_content  Raw config.toml file contents (already read).
     * @param csv_path        Path to state_machine_actuators.csv (with fallbacks).
     * @return true on success.
     */
    /**
     * @param delay_csv_path Where the per-actuator staged delays live, from
     *        [state_machine].actuator_delay_csv. Empty falls back to deriving it from `csv_path`.
     *
     *        The derivation used to be the ONLY way this was found: the delays filename was built
     *        by swapping the "state_machine_actuators.csv" suffix on the positions path. That key
     *        was parsed into the config and then read by nothing, so pointing it anywhere had no
     *        effect — and if the positions file was named anything else the swap did not fire, the
     *        POSITIONS file was opened as the delays file, every cell parsed as non-numeric, and
     * the stand silently ran with no staggering at all. Losing a fire-column stagger with no error
     * is not an acceptable failure mode for a missing filename.
     */
    bool load(const std::string& config_content, const std::string& csv_path,
              const std::string& delay_csv_path = "");
 
    /**
     * Pin outgoing actuator commands to a NIC, unless [actuator_service].bind_address already
     * named one explicitly. Call after load().
     *
     * bind_address defaulted to 0.0.0.0 in every shipped profile, so the bind() at the bottom of
     * sendBatch() constrained nothing and the kernel chose the egress interface. That was
     * harmless while the DAQ owned its machine and is not now that it shares the apps box.
     */
    void setDefaultBindAddress(const std::string& address);
 
    /**
     * Send actuator commands for the given state (one shot).
     * Groups commands by board IP, sends one UDP packet per board.
     * PWM actuators are skipped in FIRE state.
     */
    /**
     * Command every actuator for `state`.
     *
     * `is_transition` selects between the two things this does. On an actual state change it runs
     * the CSV's per-actuator delays as a staged schedule; on the 1 Hz republish it sends the
     * settled (final) positions with no delay. Without that split the republish would re-arm every
     * pending delay once a second and the schedule would never finish.
     */
    void applyForState(State state, bool is_transition = false);
 
    /**
     * Start the 1 Hz continuous re-send loop for the given state.
     * Stops any previously running loop first.
     */
    /** `allow_delays` false makes the entry apply immediately — used for abort states, which must
     *  not sit behind a delay. */
    /**
     * @param defer_first_pass skip the immediate pass and start republishing one period later.
     *        For a dynamic state: the caller has already applied the column, so that pass is
     *        redundant, and running it races the script that is about to start commanding valves —
     *        a race the script loses, because the pass resolves positions and only then spends a
     *        couple of milliseconds on retransmits, landing a stale command on top of the script's.
     */
    void startContinuousLoop(State state, bool allow_delays = true, bool defer_first_pass = false);
 
    /** Stop the continuous re-send loop (blocks until the thread exits). */
    void stopContinuousLoop();
 
    /** Send one UDP packet for a single role (debug manual command). */
    bool sendSingleActuator(const std::string& name, int pos);
 
    /**
     * The logical positions the CSV declares for `state_name` (role -> 0 CLOSED / 1 OPEN).
     * Empty when the state has no column.
     *
     * Exposed so a timed hold can be checked against the state it expires into: the hold closes
     * its valves only because the return state's column says CLOSE, and if it does not, the timer
     * expires, the state changes, and the valve stays open.
     */
    std::map<std::string, int> positionsForState(const std::string& state_name) const;
 
    /**
     * The staged delay, in seconds, that `role` waits before moving on entry to `state_name`.
     * 0 when the state, the role, or the delays file has no entry.
     *
     * Exposed so a timed hold can be measured from when the valve it cares about actually opens
     * rather than from the transition: a staggered state opens its valves at different times, so
     * "the state was held for N" and "that valve was open for N" are different claims.
     */
    double delayForRole(const std::string& state_name, const std::string& role) const;
 
    /** Debug: hold this logical position for the role until cleared or state transition. */
    void setManualOverride(const std::string& name, int pos);
 
    /** Clear all manual overrides (e.g. leaving debug mode or transitioning state). */
    void clearAllManualOverrides();
 
    /**
     * A running script's position for one valve — the script layering on top of its state's CSV
     * column.
     *
     * Deliberately NOT manual_overrides_. That map is cleared on every transition AND whenever
     * debug mode is switched off (SequencerService::doSetDebugMode), so a script writing there
     * would have its valve intent silently wiped by an operator leaving debug mode while the
     * script kept running.
     *
     * Setting a position also takes OWNERSHIP of the valve: any staged command still in flight for
     * it is cancelled, and it stops being skipped by the republish's pending-delay filter. Without
     * that, the state's own delays column could move the valve seconds after the script already
     * commanded it, landing last and silently winning.
     *
     * Like setManualOverride, this is not debug-gated here — the gate belongs to the caller.
     */
    void setScriptPosition(const std::string& name, int pos);
 
    /** Drop every script position. Called on any transition, beside clearAllManualOverrides. */
    void clearScriptPositions();
 
    /** Set the Elodin client for publishing commanded state [0x32, ch] to the DB. */
    /**
     * Which state hands PWM actuators to controller_service. During it this commander stops
     * commanding PWM roles entirely, so exactly one thing drives them during a burn. Was the
     * State::FIRE enumerator; now follows [fire] state like every other fire decision.
     */
    void setFireState(State s) {
        fire_state_ = s;
    }
 
    void setElodinClient(fsw::elodin::ElodinClient* client) {
        elodin_ = client;
    }
 
    /** Publish initial de-energized state for all actuators to Elodin DB [0x32].
     *  Does NOT send UDP commands — only populates the DB so the frontend has data on startup. */
    void publishInitialState();
 
    bool isLoaded() const {
        return loaded_;
    }
 
private:
    std::map<std::string, ActuatorRole> roles_;
    /** state -> actuator -> delay in seconds, from state_machine_actuator_delays.csv. */
    std::map<std::string, std::map<std::string, double>> state_actuator_delays_;
    /** Bumped on every transition so an in-flight delayed stage from the previous state aborts
     *  instead of landing after we have already moved on. */
    State fire_state_{State::FIRE};
    std::atomic<uint64_t> schedule_gen_{0};
    /** Roles whose delayed stage has not fired yet. The 1 Hz republish skips these: the board holds
     *  its last commanded position, which is exactly the pre-transition value we want it to keep
     *  until the delay elapses. Without this the republish would send the settled position first
     *  and any delay longer than the republish period could never be observed. */
    std::set<std::string> pending_roles_;
    std::mutex pending_roles_mutex_;
    // CSV: state_name → { actuator_name → logical_pos (0 or 1) }
    std::map<std::string, std::map<std::string, int>> state_actuators_;
 
    std::map<std::string, int> manual_overrides_;
    std::mutex overrides_mutex_;
 
    /** Valves a running script has taken over, role -> logical position. Its own mutex, and never
     *  held at the same time as overrides_mutex_ — applyForState snapshots this first and then
     *  takes the other, so no call path holds both and there is no lock order to get wrong. */
    std::map<std::string, int> script_positions_;
    std::mutex script_positions_mutex_;
 
    /** Warn once per role that a manual override is fighting a running script for a valve. */
    static void logOverrideShadowingScript(const std::string& role);
 
    std::thread loop_thread_;
    std::atomic<bool> loop_running_{false};
    State loop_state_{State::IDLE};
 
    std::string bind_addr_{"0.0.0.0"};
    bool bind_addr_explicit_{false};
    uint16_t actuator_port_{5005};
    bool loaded_{false};
    fsw::elodin::ElodinClient* elodin_{nullptr};
 
    // Resolve state name case-insensitively. Returns end() if not found.
    std::map<std::string, std::map<std::string, int>>::const_iterator findStateActuators(
        const std::string& state_name) const;
 
    // Build and send one UDP actuator command packet to a board.
    /** Send to every board with the retransmits interleaved, so boards stay in step. */
    bool sendBatch(const std::map<std::string, std::vector<std::pair<uint8_t, uint8_t>>>& by_board);
    bool sendUDP(const std::string& board_ip,
                 const std::vector<std::pair<uint8_t, uint8_t>>& id_state_pairs);
 
    // Publish commanded actuator state [table_hi, global_channel] to Elodin DB: 0x32 (ACT_CMD) or
    // 0x35 (BV_CMD), from ActuatorRole::cmd_table_hi. global_channel encodes board + local channel
    // to avoid collisions across boards.
    void publishCommandedState(uint8_t table_hi, uint8_t global_channel, uint8_t logical_pos);
};
 
}  // namespace sequencer