STAR
Cal STAR — firmware, DAQ server, and engine design
Loading...
Searching...
No Matches
fsw::control::ControllerService Class Reference

C++ controller service with UDP PWM output. More...

#include <ControllerService.hpp>

Collaboration diagram for fsw::control::ControllerService:
[legend]

Classes

struct  PWMTarget
 Where one propellant's PWM commands go. More...
struct  PWMConfig

Public Member Functions

 ControllerService ()
 ~ControllerService ()
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="")
 Initialize the controller service.
bool start (double loop_rate_hz=10.0)
 Start the controller loop at the given rate.
void stop ()
 Stop the controller loop.
bool is_running () const
void setMeasurement (const RobustDDPController::Measurement &meas)
void setCommand (const RobustDDPController::Command &cmd)
void setNavState (const RobustDDPController::NavState &nav)
void setFireActive (bool active)
 Enable or disable PWM output (FIRE state gate).
void setFireStateId (uint8_t id)
 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.
void setFireGateEnabled (bool enabled)
 Master switch for the PWM ignition gate.
bool isFireGateEnabled () const
void setTestDuty (float fuel, float ox)
 Override controller output with fixed duty cycles for open-loop validation.
RobustDDPController::ActuationCommand getLastActuation () const
 Get last actuation output (for diagnostics / GUI broadcast).
RobustDDPController::Diagnostics getLastDiagnostics () const

Private Member Functions

bool sendPWMCommand (const PWMTarget &target, float duty_cycle, float frequency, uint32_t duration_ms)
 Send a PWM actuator command over UDP.
bool sendPWMCommands (const PWMTarget &t1, float duty1, const PWMTarget &t2, float duty2, float frequency, uint32_t duration_ms)
 Single UDP packet carrying both channels.
void sendActuationPWM (const RobustDDPController::ActuationCommand &act)
 Send the fuel + ox PWM commands for a given actuation output.
bool registerControllerTables ()
void writeActuationToDB (const RobustDDPController::ActuationCommand &actuation)
void writeDiagnosticsToDB (const RobustDDPController::Diagnostics &diagnostics)
void writeMeasurementToDB (const RobustDDPController::Measurement &measurement)
void controllerLoop ()
void elodinSubscriberLoop ()
bool connectSubscriber ()
 Connect the subscriber and subscribe to the tables it reads.
void subscriberReadLoop (std::vector< uint8_t > &rx_buffer)
 The read half, one connection's worth.
bool loadThrustCurve (const std::string &path)
double interpolateThrustCurve (double t_elapsed_s) const

Private Attributes

uint8_t fire_state_id_ {16}
std::atomic< bool > running_ {false}
std::atomic< bool > fire_active_ {false}
std::atomic< bool > fire_gate_enabled_ {true}
std::atomic< float > test_duty_fuel_ {0.0f}
std::atomic< float > test_duty_ox_ {0.0f}
std::unique_ptr< RobustDDPController > controller_
ControllerLUT lut_
std::unique_ptr< elodin::ElodinClient > elodin_client_
bool elodin_connected_ = false
std::unique_ptr< elodin::ElodinClient > elodin_subscriber_
std::string elodin_host_
uint16_t elodin_port_ = 2240
PWMConfig pwm_config_
int udp_socket_fd_ = -1
std::mutex input_mutex_
RobustDDPController::Measurement current_meas_ {}
RobustDDPController::Command current_cmd_ {}
RobustDDPController::NavState current_nav_ {}
bool has_measurement_ = false
std::mutex output_mutex_
RobustDDPController::ActuationCommand last_actuation_ {}
RobustDDPController::Diagnostics last_diagnostics_ {}
std::thread controller_thread_
std::thread elodin_subscriber_thread_
double loop_rate_hz_ = 10.0
double loop_interval_ms_ = 100.0
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

Detailed Description

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.

Definition at line 29 of file ControllerService.hpp.

Constructor & Destructor Documentation

◆ ControllerService()

fsw::control::ControllerService::ControllerService ( )

Definition at line 68 of file ControllerService.cpp.

References controller_, elodin_client_, and elodin_subscriber_.

◆ ~ControllerService()

fsw::control::ControllerService::~ControllerService ( )

Definition at line 74 of file ControllerService.cpp.

References stop(), and udp_socket_fd_.

Member Function Documentation

◆ connectSubscriber()

bool fsw::control::ControllerService::connectSubscriber ( )
private

Connect the subscriber and subscribe to the tables it reads.

Connect the subscriber and subscribe to everything 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.

Both halves must run on every reconnect, not just the first connect: the VTables live in the db process, so a db restart loses them and a reconnect that skips re-subscription comes back permanently silent. SequencerService::tryConnectElodin() documents the same rule for the publishing side, and heartbeat_service treats a failed subscribe as a failed connection for exactly this reason — so this returns false and lets the caller retry the whole thing.

Definition at line 763 of file ControllerService.cpp.

References ch(), elodin_host_, elodin_port_, elodin_subscriber_, and tables().

Referenced by elodinSubscriberLoop(), and initialize().

◆ controllerLoop()

◆ elodinSubscriberLoop()

void fsw::control::ControllerService::elodinSubscriberLoop ( )
private

Definition at line 793 of file ControllerService.cpp.

References connectSubscriber(), elodin_subscriber_, running_, and subscriberReadLoop().

Referenced by start().

◆ getLastActuation()

RobustDDPController::ActuationCommand fsw::control::ControllerService::getLastActuation ( ) const

Get last actuation output (for diagnostics / GUI broadcast).

Definition at line 373 of file ControllerService.cpp.

References last_actuation_, lock, and output_mutex_.

◆ getLastDiagnostics()

RobustDDPController::Diagnostics fsw::control::ControllerService::getLastDiagnostics ( ) const

Definition at line 378 of file ControllerService.cpp.

References last_diagnostics_, lock, and output_mutex_.

◆ initialize()

bool fsw::control::ControllerService::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 = "" )

Initialize the controller service.

Parameters
pwm_configPWM output configuration (board IP, channels, …)
controller_configController algorithm configuration
elodin_hostElodin DB host (empty string = skip DB)
elodin_portElodin DB port (default 2240)
lut_pathOptional path to LUT binary. If non-empty and load succeeds, bypasses DDP and uses LUT for boolean control (u_safe_F/O > 0.5).
thrust_curve_pathOptional path to thrust curve CSV (time_s,thrust_N). When set and fire is active, thrust_desired is interpolated from curve.
Returns
true if initialization succeeded

Definition at line 82 of file ControllerService.cpp.

References fsw::net::bindToDaqInterface(), connectSubscriber(), controller_, current_cmd_, current_meas_, elodin_client_, elodin_connected_, elodin_host_, elodin_port_, input_mutex_, loadThrustCurve(), lock, lut_, pwm_config_, registerControllerTables(), thrust_curve_loaded_, thrust_curve_times_, fsw::control::RobustDDPController::THRUST_DESIRED, and udp_socket_fd_.

Referenced by main().

◆ interpolateThrustCurve()

double fsw::control::ControllerService::interpolateThrustCurve ( double t_elapsed_s) const
private

Definition at line 1139 of file ControllerService.cpp.

References thrust_curve_times_, and thrust_curve_values_.

Referenced by controllerLoop(), and setFireActive().

◆ is_running()

bool fsw::control::ControllerService::is_running ( ) const
inline

Definition at line 85 of file ControllerService.hpp.

References running_.

Referenced by main().

◆ isFireGateEnabled()

bool fsw::control::ControllerService::isFireGateEnabled ( ) const
inline

Definition at line 127 of file ControllerService.hpp.

References fire_gate_enabled_.

Referenced by handleControlClient().

◆ loadThrustCurve()

bool fsw::control::ControllerService::loadThrustCurve ( const std::string & path)
private

Definition at line 1113 of file ControllerService.cpp.

References thrust_curve_times_, and thrust_curve_values_.

Referenced by initialize().

◆ registerControllerTables()

bool fsw::control::ControllerService::registerControllerTables ( )
private

Definition at line 929 of file ControllerService.cpp.

References elodin_client_, elodin_connected_, and fsw::control::send_msg().

Referenced by initialize().

◆ sendActuationPWM()

void fsw::control::ControllerService::sendActuationPWM ( const RobustDDPController::ActuationCommand & act)
private

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.

Definition at line 509 of file ControllerService.cpp.

References fsw::control::RobustDDPController::ActuationCommand::duty_F, fsw::control::RobustDDPController::ActuationCommand::duty_O, pwm_config_, sendPWMCommand(), sendPWMCommands(), and fsw::control::RobustDDPController::ActuationCommand::valid.

Referenced by controllerLoop(), and setFireActive().

◆ sendPWMCommand()

bool fsw::control::ControllerService::sendPWMCommand ( const PWMTarget & target,
float duty_cycle,
float frequency,
uint32_t duration_ms )
private

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 : num_commands Per-cmd <BIff>: actuator_id, duration_ms, duty_cycle, frequency

Definition at line 387 of file ControllerService.cpp.

References fsw::control::COMMS_VERSION, fsw::control::PACKET_TYPE_PWM, pwm_config_, fsw::control::SINGLE_CMD_PACKET_SIZE, target, and udp_socket_fd_.

Referenced by sendActuationPWM().

◆ sendPWMCommands()

bool fsw::control::ControllerService::sendPWMCommands ( const PWMTarget & t1,
float duty1,
const PWMTarget & t2,
float duty2,
float frequency,
uint32_t duration_ms )
private

Single UDP packet carrying both channels.

Only valid when both targets are on the same board — the packet has one destination. sendActuationPWM enforces that.

Definition at line 451 of file ControllerService.cpp.

References fsw::control::ControllerService::PWMTarget::board_ip, fsw::control::ControllerService::PWMTarget::channel, fsw::control::COMMS_VERSION, fsw::control::DUAL_CMD_PACKET_SIZE, fsw::control::PACKET_TYPE_PWM, pwm_config_, fsw::control::ControllerService::PWMTarget::resolved(), and udp_socket_fd_.

Referenced by sendActuationPWM().

◆ setCommand()

void fsw::control::ControllerService::setCommand ( const RobustDDPController::Command & cmd)

Definition at line 240 of file ControllerService.cpp.

References current_cmd_, input_mutex_, and lock.

Referenced by main().

◆ setFireActive()

◆ setFireGateEnabled()

void fsw::control::ControllerService::setFireGateEnabled ( bool enabled)
inline

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.

Definition at line 124 of file ControllerService.hpp.

References fire_gate_enabled_.

Referenced by main().

◆ setFireStateId()

void fsw::control::ControllerService::setFireStateId ( uint8_t id)
inline

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.

Definition at line 107 of file ControllerService.hpp.

References fire_state_id_.

Referenced by main().

◆ setMeasurement()

void fsw::control::ControllerService::setMeasurement ( const RobustDDPController::Measurement & meas)

Definition at line 234 of file ControllerService.cpp.

References current_meas_, has_measurement_, input_mutex_, and lock.

◆ setNavState()

void fsw::control::ControllerService::setNavState ( const RobustDDPController::NavState & nav)

Definition at line 245 of file ControllerService.cpp.

References current_nav_, input_mutex_, and lock.

◆ setTestDuty()

void fsw::control::ControllerService::setTestDuty ( float fuel,
float ox )

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.

Definition at line 362 of file ControllerService.cpp.

References test_duty_fuel_, and test_duty_ox_.

Referenced by main().

◆ start()

bool fsw::control::ControllerService::start ( double loop_rate_hz = 10.0)

Start the controller loop at the given rate.

Definition at line 189 of file ControllerService.cpp.

References controller_thread_, controllerLoop(), elodin_host_, elodin_subscriber_thread_, elodinSubscriberLoop(), loop_interval_ms_, loop_rate_hz_, running_, and udp_socket_fd_.

Referenced by main().

◆ stop()

void fsw::control::ControllerService::stop ( )

Stop the controller loop.

Definition at line 216 of file ControllerService.cpp.

References controller_thread_, elodin_subscriber_thread_, and running_.

Referenced by main(), and ~ControllerService().

◆ subscriberReadLoop()

void fsw::control::ControllerService::subscriberReadLoop ( std::vector< uint8_t > & rx_buffer)
private

◆ writeActuationToDB()

◆ writeDiagnosticsToDB()

void fsw::control::ControllerService::writeDiagnosticsToDB ( const RobustDDPController::Diagnostics & diagnostics)
private

Definition at line 1069 of file ControllerService.cpp.

References elodin_client_, and comms::CommsMessage< Fields >::fields.

Referenced by controllerLoop().

◆ writeMeasurementToDB()

Member Data Documentation

◆ controller_

std::unique_ptr<RobustDDPController> fsw::control::ControllerService::controller_
private

◆ controller_thread_

std::thread fsw::control::ControllerService::controller_thread_
private

Definition at line 226 of file ControllerService.hpp.

Referenced by start(), and stop().

◆ current_cmd_

RobustDDPController::Command fsw::control::ControllerService::current_cmd_ {}
private

Definition at line 216 of file ControllerService.hpp.

Referenced by controllerLoop(), initialize(), setCommand(), and setFireActive().

◆ current_meas_

RobustDDPController::Measurement fsw::control::ControllerService::current_meas_ {}
private

◆ current_nav_

RobustDDPController::NavState fsw::control::ControllerService::current_nav_ {}
private

Definition at line 217 of file ControllerService.hpp.

Referenced by controllerLoop(), setFireActive(), and setNavState().

◆ elodin_client_

std::unique_ptr<elodin::ElodinClient> fsw::control::ControllerService::elodin_client_
private

◆ elodin_connected_

bool fsw::control::ControllerService::elodin_connected_ = false
private

◆ elodin_host_

std::string fsw::control::ControllerService::elodin_host_
private

Definition at line 206 of file ControllerService.hpp.

Referenced by connectSubscriber(), initialize(), and start().

◆ elodin_port_

uint16_t fsw::control::ControllerService::elodin_port_ = 2240
private

Definition at line 207 of file ControllerService.hpp.

Referenced by connectSubscriber(), and initialize().

◆ elodin_subscriber_

std::unique_ptr<elodin::ElodinClient> fsw::control::ControllerService::elodin_subscriber_
private

◆ elodin_subscriber_thread_

std::thread fsw::control::ControllerService::elodin_subscriber_thread_
private

Definition at line 227 of file ControllerService.hpp.

Referenced by start(), and stop().

◆ fire_active_

std::atomic<bool> fsw::control::ControllerService::fire_active_ {false}
private

Definition at line 187 of file ControllerService.hpp.

Referenced by controllerLoop(), and setFireActive().

◆ fire_gate_enabled_

std::atomic<bool> fsw::control::ControllerService::fire_gate_enabled_ {true}
private

◆ fire_start_time_

std::chrono::steady_clock::time_point fsw::control::ControllerService::fire_start_time_
private

Definition at line 234 of file ControllerService.hpp.

Referenced by controllerLoop(), and setFireActive().

◆ fire_state_id_

uint8_t fsw::control::ControllerService::fire_state_id_ {16}
private

Definition at line 143 of file ControllerService.hpp.

Referenced by setFireStateId(), and subscriberReadLoop().

◆ has_measurement_

bool fsw::control::ControllerService::has_measurement_ = false
private

Definition at line 218 of file ControllerService.hpp.

Referenced by controllerLoop(), setMeasurement(), and subscriberReadLoop().

◆ input_mutex_

std::mutex fsw::control::ControllerService::input_mutex_
mutableprivate

◆ last_actuation_

RobustDDPController::ActuationCommand fsw::control::ControllerService::last_actuation_ {}
private

Definition at line 222 of file ControllerService.hpp.

Referenced by controllerLoop(), and getLastActuation().

◆ last_diagnostics_

RobustDDPController::Diagnostics fsw::control::ControllerService::last_diagnostics_ {}
private

Definition at line 223 of file ControllerService.hpp.

Referenced by controllerLoop(), and getLastDiagnostics().

◆ loop_interval_ms_

double fsw::control::ControllerService::loop_interval_ms_ = 100.0
private

Definition at line 229 of file ControllerService.hpp.

Referenced by controllerLoop(), and start().

◆ loop_rate_hz_

double fsw::control::ControllerService::loop_rate_hz_ = 10.0
private

Definition at line 228 of file ControllerService.hpp.

Referenced by start().

◆ lut_

ControllerLUT fsw::control::ControllerService::lut_
private

Definition at line 198 of file ControllerService.hpp.

Referenced by controllerLoop(), initialize(), and setFireActive().

◆ output_mutex_

std::mutex fsw::control::ControllerService::output_mutex_
mutableprivate

Definition at line 221 of file ControllerService.hpp.

Referenced by controllerLoop(), getLastActuation(), and getLastDiagnostics().

◆ pwm_config_

PWMConfig fsw::control::ControllerService::pwm_config_
private

◆ running_

std::atomic<bool> fsw::control::ControllerService::running_ {false}
private

◆ test_duty_fuel_

std::atomic<float> fsw::control::ControllerService::test_duty_fuel_ {0.0f}
private

Definition at line 191 of file ControllerService.hpp.

Referenced by controllerLoop(), setFireActive(), and setTestDuty().

◆ test_duty_ox_

std::atomic<float> fsw::control::ControllerService::test_duty_ox_ {0.0f}
private

Definition at line 192 of file ControllerService.hpp.

Referenced by controllerLoop(), setFireActive(), and setTestDuty().

◆ thrust_curve_loaded_

bool fsw::control::ControllerService::thrust_curve_loaded_ = false
private

Definition at line 235 of file ControllerService.hpp.

Referenced by controllerLoop(), initialize(), and setFireActive().

◆ thrust_curve_times_

std::vector<double> fsw::control::ControllerService::thrust_curve_times_
private

Definition at line 232 of file ControllerService.hpp.

Referenced by initialize(), interpolateThrustCurve(), and loadThrustCurve().

◆ thrust_curve_values_

std::vector<double> fsw::control::ControllerService::thrust_curve_values_
private

Definition at line 233 of file ControllerService.hpp.

Referenced by interpolateThrustCurve(), and loadThrustCurve().

◆ udp_socket_fd_

int fsw::control::ControllerService::udp_socket_fd_ = -1
private

The documentation for this class was generated from the following files: