ControllerService.hpp

Go to the documentation of this file.

#ifndef FSW_CONTROL_CONTROLLER_SERVICE_HPP
#define FSW_CONTROL_CONTROLLER_SERVICE_HPP
 
#include <atomic>
#include <chrono>
#include <cstdint>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
 
#include "ControllerLUT.hpp"
#include "RobustDDPController.hpp"
#include "elodin/ElodinClient.hpp"
 
namespace fsw {
namespace control {
 
/**
 * @brief C++ controller service with UDP PWM output
 *
 * Pipeline:
 *   sensor data (pushed via setMeasurement) → RobustDDPController::step()
 *   → PWM command UDP packet (combined_gui.py format) → actuator board
 *
 * Optionally writes to Elodin DB if connected.
 */
class ControllerService {
public:
    // ── PWM / actuator config ──────────────────────────────────────────
    /** Where one propellant's PWM commands go. Both fields come from config via
     *  [controller].pwm_*_actuator → [actuator_roles] → [boards.*]; there are deliberately no
     *  defaults, because a default is this process inventing a rig layout it cannot know.
     *  channel == 0 means "config did not say" and the target must never be addressed. */
    struct PWMTarget {
        std::string board_ip;
        uint8_t channel = 0;
        std::string actuator_name;  // the [actuator_roles] key, for logs an operator can act on
 
        bool resolved() const {
            return channel != 0 && !board_ip.empty();
        }
    };
 
    struct PWMConfig {
        PWMTarget fuel;
        PWMTarget ox;
        uint16_t actuator_port = 5005;
        /** Local address PWM commands leave from; "0.0.0.0" leaves the NIC to the kernel.
         *  Resolved by the caller (see net/DaqInterface.hpp) — this is the fire path, and it must
         *  not reach the actuator board over whichever interface the route table happened to
         *  prefer. */
        std::string bind_address = "0.0.0.0";
        float frequency_hz = 10.0f;
        uint32_t duration_ms = 1000;  // PWM burst duration per command
    };
 
    ControllerService();
    ~ControllerService();
 
    /**
     * @brief Initialize the controller service
     * @param pwm_config       PWM output configuration (board IP, channels, …)
     * @param controller_config Controller algorithm configuration
     * @param elodin_host      Elodin DB host (empty string = skip DB)
     * @param elodin_port      Elodin DB port (default 2240)
     * @param lut_path         Optional path to LUT binary. If non-empty and load succeeds,
     *                         bypasses DDP and uses LUT for boolean control (u_safe_F/O > 0.5).
     * @param thrust_curve_path Optional path to thrust curve CSV (time_s,thrust_N). When set
     *                         and fire is active, thrust_desired is interpolated from curve.
     * @return true if initialization succeeded
     */
    bool initialize(const PWMConfig& pwm_config,
                    const RobustDDPController::Config& controller_config,
                    const std::string& elodin_host = "", uint16_t elodin_port = 2240,
                    const std::string& lut_path = "", const std::string& thrust_curve_path = "");
 
    /** Start the controller loop at the given rate. */
    bool start(double loop_rate_hz = 10.0);
 
    /** Stop the controller loop. */
    void stop();
 
    bool is_running() const {
        return running_;
    }
 
    // ── Thread-safe data setters (called from external sensor thread) ──
    void setMeasurement(const RobustDDPController::Measurement& meas);
    void setCommand(const RobustDDPController::Command& cmd);
    void setNavState(const RobustDDPController::NavState& nav);
 
    /**
     * @brief Enable or disable PWM output (FIRE state gate).
     * PWM is only sent while fire_active_ is true.
     * Called by the TCP control thread when FIRE_START / FIRE_STOP is received.
     */
    void setFireActive(bool active);
 
    /**
     * Sequencer state id that means "firing", for the parity fallback that sniffs the sequencer
     * state packet when the TCP FIRE_START/FIRE_STOP path misses an edge. Was the literal 16, so
     * renumbering states silently moved the PWM ignition gate onto whatever state took id 16.
     * Defaults to 16 so an un-migrated config behaves exactly as before.
     */
    void setFireStateId(uint8_t id) {
        fire_state_id_ = id;
    }
 
    /**
     * Master switch for the PWM ignition gate.
     *
     * Disabled means this service never drives a valve, whatever it is told: setFireActive() —
     * the single funnel BOTH activation paths go through, TCP and the state-packet parity
     * fallback — returns immediately. Two reasons to turn it off:
     *
     *   1. [controller_service].sequencer_owns_valves says the sequencer commands the valves
     *      during the burn, so a second writer on the same channels would fight it.
     *   2. The PWM roles could not be resolved from [actuator_roles]. This used to fall back to
     *      hardcoded channel numbers, which on a stand that does not have those roles addressed
     *      whatever valve happened to occupy them — on one profile, a vent.
     */
    void setFireGateEnabled(bool enabled) {
        fire_gate_enabled_ = enabled;
    }
    bool isFireGateEnabled() const {
        return fire_gate_enabled_.load();
    }
 
    /**
     * @brief Override controller output with fixed duty cycles for open-loop validation.
     * When both values are 0 (default), the RobustDDP controller runs normally.
     * Set non-zero to bypass the controller and send these fixed duties on every FIRE tick.
     */
    void setTestDuty(float fuel, float ox);
 
    /** Get last actuation output (for diagnostics / GUI broadcast). */
    RobustDDPController::ActuationCommand getLastActuation() const;
    RobustDDPController::Diagnostics getLastDiagnostics() const;
 
private:
    uint8_t fire_state_id_{16};
    // ── PWM packet sending ─────────────────────────────────────────────
    /**
     * Send a PWM actuator command over UDP.
     * Packet format matches combined_gui.py exactly:
     *   Header  <BBI>: packet_type=10, version=0, timestamp_ms(u32)
     *   Body    <B>  : num_commands
     *   Per-cmd <BIff>: actuator_id, duration_ms, duty_cycle, frequency
     */
    bool sendPWMCommand(const PWMTarget& target, float duty_cycle, float frequency,
                        uint32_t duration_ms);
 
    /** Single UDP packet carrying both channels. Only valid when both targets are on the same
     *  board — the packet has one destination. sendActuationPWM enforces that. */
    bool sendPWMCommands(const PWMTarget& t1, float duty1, const PWMTarget& t2, float duty2,
                         float frequency, uint32_t duration_ms);
 
    /** Send the fuel + ox PWM commands for a given actuation output. One packet when both
     *  actuators are on the same board (preserving the simultaneity the fire path had when
     *  PWMConfig could only express one destination), two when config puts them on different
     *  boards. An unresolved target is never addressed. */
    void sendActuationPWM(const RobustDDPController::ActuationCommand& act);
 
    // ── Elodin DB (optional) ───────────────────────────────────────────
    bool registerControllerTables();
    void writeActuationToDB(const RobustDDPController::ActuationCommand& actuation);
    void writeDiagnosticsToDB(const RobustDDPController::Diagnostics& diagnostics);
    void writeMeasurementToDB(const RobustDDPController::Measurement& measurement);
 
    // ── Controller loop ────────────────────────────────────────────────
    void controllerLoop();
    void elodinSubscriberLoop();
 
    /** Connect the subscriber and subscribe to the tables it reads. Both halves run on every
     *  reconnect — a db restart loses the VTables, so re-connecting without re-subscribing comes
     *  back silent. Returns false (having dropped the connection) if either half fails. */
    bool connectSubscriber();
 
    /** The read half, one connection's worth. Returns when the connection drops or on shutdown;
     *  elodinSubscriberLoop() decides whether to reconnect. */
    void subscriberReadLoop(std::vector<uint8_t>& rx_buffer);
 
    // ── State ──────────────────────────────────────────────────────────
    std::atomic<bool> running_{false};
    std::atomic<bool> fire_active_{false};       // PWM only sent when FIRE state is active
    std::atomic<bool> fire_gate_enabled_{true};  // see setFireGateEnabled
 
    // Open-loop test duty cycles (0 = use DDP controller; non-zero = bypass DDP)
    std::atomic<float> test_duty_fuel_{0.0f};
    std::atomic<float> test_duty_ox_{0.0f};
 
    // Controller algorithm (DDP when LUT not used)
    std::unique_ptr<RobustDDPController> controller_;
 
    // Optional LUT for boolean control (bypasses DDP when loaded)
    ControllerLUT lut_;
 
    // Elodin DB — publisher (write-only; used for publishing actuation/diagnostics)
    std::unique_ptr<elodin::ElodinClient> elodin_client_;
    bool elodin_connected_ = false;
 
    // Elodin DB — subscriber (read-only; dedicated to receiving calibrated PT data)
    std::unique_ptr<elodin::ElodinClient> elodin_subscriber_;
    std::string elodin_host_;
    uint16_t elodin_port_ = 2240;
 
    // PWM output
    PWMConfig pwm_config_;
    int udp_socket_fd_ = -1;
 
    // Thread-safe sensor inputs
    mutable std::mutex input_mutex_;
    RobustDDPController::Measurement current_meas_{};
    RobustDDPController::Command current_cmd_{};
    RobustDDPController::NavState current_nav_{};
    bool has_measurement_ = false;
 
    // Last outputs (thread-safe read)
    mutable std::mutex output_mutex_;
    RobustDDPController::ActuationCommand last_actuation_{};
    RobustDDPController::Diagnostics last_diagnostics_{};
 
    // Loop timing
    std::thread controller_thread_;
    std::thread elodin_subscriber_thread_;
    double loop_rate_hz_ = 10.0;
    double loop_interval_ms_ = 100.0;
 
    // Thrust curve (time-varying target from Layer 2 pressure curves)
    std::vector<double> thrust_curve_times_;
    std::vector<double> thrust_curve_values_;
    std::chrono::steady_clock::time_point fire_start_time_;
    bool thrust_curve_loaded_ = false;
 
    bool loadThrustCurve(const std::string& path);
    double interpolateThrustCurve(double t_elapsed_s) const;
};
 
}  // namespace control
}  // namespace fsw
 
#endif  // FSW_CONTROL_CONTROLLER_SERVICE_HPP