|
STAR
Cal STAR — firmware, DAQ server, and engine design
|
C++ controller service with UDP PWM output. More...
#include <ControllerService.hpp>
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 |
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.
| fsw::control::ControllerService::ControllerService | ( | ) |
Definition at line 68 of file ControllerService.cpp.
References controller_, elodin_client_, and elodin_subscriber_.
| fsw::control::ControllerService::~ControllerService | ( | ) |
Definition at line 74 of file ControllerService.cpp.
References stop(), and udp_socket_fd_.
|
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().
|
private |
Definition at line 536 of file ControllerService.cpp.
References a, controller_, current_cmd_, current_meas_, current_nav_, fsw::control::RobustDDPController::ActuationCommand::duty_F, fsw::control::RobustDDPController::ActuationCommand::duty_O, elapsed(), elodin_client_, elodin_connected_, fire_active_, fire_start_time_, has_measurement_, input_mutex_, interpolateThrustCurve(), last_actuation_, last_diagnostics_, lock, loop_interval_ms_, lut_, output_mutex_, fsw::control::RobustDDPController::Measurement::P_ch_mp1, fsw::control::RobustDDPController::Measurement::P_ch_mp2, fsw::control::RobustDDPController::Measurement::P_d_fuel, fsw::control::RobustDDPController::Measurement::P_d_ox, fsw::control::RobustDDPController::Measurement::P_u_fuel, fsw::control::RobustDDPController::Measurement::P_u_ox, running_, sendActuationPWM(), test_duty_fuel_, test_duty_ox_, thrust_curve_loaded_, fsw::control::RobustDDPController::THRUST_DESIRED, fsw::control::RobustDDPController::Command::thrust_desired, fsw::control::RobustDDPController::Command::type, fsw::control::RobustDDPController::ActuationCommand::u_F_on, fsw::control::RobustDDPController::ActuationCommand::u_O_on, fsw::control::RobustDDPController::ActuationCommand::valid, writeActuationToDB(), writeDiagnosticsToDB(), and writeMeasurementToDB().
Referenced by start().
|
private |
Definition at line 793 of file ControllerService.cpp.
References connectSubscriber(), elodin_subscriber_, running_, and subscriberReadLoop().
Referenced by start().
| 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_.
| RobustDDPController::Diagnostics fsw::control::ControllerService::getLastDiagnostics | ( | ) | const |
Definition at line 378 of file ControllerService.cpp.
References last_diagnostics_, lock, and output_mutex_.
| 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.
| pwm_config | PWM output configuration (board IP, channels, …) |
| controller_config | Controller algorithm configuration |
| elodin_host | Elodin DB host (empty string = skip DB) |
| elodin_port | Elodin DB port (default 2240) |
| 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). |
| 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. |
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().
|
private |
Definition at line 1139 of file ControllerService.cpp.
References thrust_curve_times_, and thrust_curve_values_.
Referenced by controllerLoop(), and setFireActive().
|
inline |
|
inline |
Definition at line 127 of file ControllerService.hpp.
References fire_gate_enabled_.
Referenced by handleControlClient().
|
private |
Definition at line 1113 of file ControllerService.cpp.
References thrust_curve_times_, and thrust_curve_values_.
Referenced by initialize().
|
private |
Definition at line 929 of file ControllerService.cpp.
References elodin_client_, elodin_connected_, and fsw::control::send_msg().
Referenced by initialize().
|
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().
|
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().
|
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().
| 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().
| void fsw::control::ControllerService::setFireActive | ( | bool | active | ) |
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.
Definition at line 250 of file ControllerService.cpp.
References a, active, controller_, current_cmd_, current_meas_, current_nav_, fsw::control::RobustDDPController::ActuationCommand::duty_F, fsw::control::RobustDDPController::ActuationCommand::duty_O, elodin_client_, elodin_connected_, comms::CommsMessage< Fields >::fields, fire_active_, fire_gate_enabled_, fire_start_time_, input_mutex_, interpolateThrustCurve(), lock, lut_, fsw::control::RobustDDPController::Measurement::P_ch_mp1, fsw::control::RobustDDPController::Measurement::P_ch_mp2, fsw::control::RobustDDPController::Measurement::P_u_fuel, fsw::control::RobustDDPController::Measurement::P_u_ox, sendActuationPWM(), test_duty_fuel_, test_duty_ox_, thrust_curve_loaded_, fsw::control::RobustDDPController::THRUST_DESIRED, fsw::control::RobustDDPController::Command::thrust_desired, fsw::control::RobustDDPController::Command::type, fsw::control::RobustDDPController::ActuationCommand::u_F_on, fsw::control::RobustDDPController::ActuationCommand::u_O_on, and fsw::control::RobustDDPController::ActuationCommand::valid.
Referenced by handleControlClient(), and subscriberReadLoop().
|
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:
Definition at line 124 of file ControllerService.hpp.
References fire_gate_enabled_.
Referenced by main().
|
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().
| 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.
| void fsw::control::ControllerService::setNavState | ( | const RobustDDPController::NavState & | nav | ) |
Definition at line 245 of file ControllerService.cpp.
References current_nav_, input_mutex_, and lock.
| 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().
| 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().
| 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().
|
private |
The read half, one connection's worth.
Returns when the connection drops or on shutdown; elodinSubscriberLoop() decides whether to reconnect.
Definition at line 820 of file ControllerService.cpp.
References ch(), current_meas_, comms::CommsMessage< Fields >::deserialize(), elodin_subscriber_, fire_gate_enabled_, fire_state_id_, comms::CommsMessage< Fields >::getField(), has_measurement_, input_mutex_, lock, comms::CommsMessage< uint64_t, uint8_t, std::array< uint8_t, 3 >, float, uint32_t, uint8_t >::nbytes(), running_, and setFireActive().
Referenced by elodinSubscriberLoop().
|
private |
Definition at line 1053 of file ControllerService.cpp.
References fsw::control::RobustDDPController::ActuationCommand::duty_F, fsw::control::RobustDDPController::ActuationCommand::duty_O, elodin_client_, comms::CommsMessage< Fields >::fields, fsw::control::RobustDDPController::ActuationCommand::u_F_on, fsw::control::RobustDDPController::ActuationCommand::u_O_on, and fsw::control::RobustDDPController::ActuationCommand::valid.
Referenced by controllerLoop().
|
private |
Definition at line 1069 of file ControllerService.cpp.
References elodin_client_, and comms::CommsMessage< Fields >::fields.
Referenced by controllerLoop().
|
private |
Definition at line 1089 of file ControllerService.cpp.
References elodin_client_, comms::CommsMessage< Fields >::fields, fsw::control::RobustDDPController::Measurement::P_ch_mp1, fsw::control::RobustDDPController::Measurement::P_ch_mp2, fsw::control::RobustDDPController::Measurement::P_copv, fsw::control::RobustDDPController::Measurement::P_d_fuel, fsw::control::RobustDDPController::Measurement::P_d_ox, fsw::control::RobustDDPController::Measurement::P_reg, fsw::control::RobustDDPController::Measurement::P_u_fuel, and fsw::control::RobustDDPController::Measurement::P_u_ox.
Referenced by controllerLoop().
|
private |
Definition at line 195 of file ControllerService.hpp.
Referenced by controllerLoop(), ControllerService(), initialize(), and setFireActive().
|
private |
Definition at line 226 of file ControllerService.hpp.
|
private |
Definition at line 216 of file ControllerService.hpp.
Referenced by controllerLoop(), initialize(), setCommand(), and setFireActive().
|
private |
Definition at line 215 of file ControllerService.hpp.
Referenced by controllerLoop(), initialize(), setFireActive(), setMeasurement(), and subscriberReadLoop().
|
private |
Definition at line 217 of file ControllerService.hpp.
Referenced by controllerLoop(), setFireActive(), and setNavState().
|
private |
Definition at line 201 of file ControllerService.hpp.
Referenced by controllerLoop(), ControllerService(), initialize(), registerControllerTables(), setFireActive(), writeActuationToDB(), writeDiagnosticsToDB(), and writeMeasurementToDB().
|
private |
Definition at line 202 of file ControllerService.hpp.
Referenced by controllerLoop(), initialize(), registerControllerTables(), and setFireActive().
|
private |
Definition at line 206 of file ControllerService.hpp.
Referenced by connectSubscriber(), initialize(), and start().
|
private |
Definition at line 207 of file ControllerService.hpp.
Referenced by connectSubscriber(), and initialize().
|
private |
Definition at line 205 of file ControllerService.hpp.
Referenced by connectSubscriber(), ControllerService(), elodinSubscriberLoop(), and subscriberReadLoop().
|
private |
Definition at line 227 of file ControllerService.hpp.
|
private |
Definition at line 187 of file ControllerService.hpp.
Referenced by controllerLoop(), and setFireActive().
|
private |
Definition at line 188 of file ControllerService.hpp.
Referenced by isFireGateEnabled(), setFireActive(), setFireGateEnabled(), and subscriberReadLoop().
|
private |
Definition at line 234 of file ControllerService.hpp.
Referenced by controllerLoop(), and setFireActive().
|
private |
Definition at line 143 of file ControllerService.hpp.
Referenced by setFireStateId(), and subscriberReadLoop().
|
private |
Definition at line 218 of file ControllerService.hpp.
Referenced by controllerLoop(), setMeasurement(), and subscriberReadLoop().
|
mutableprivate |
Definition at line 214 of file ControllerService.hpp.
Referenced by controllerLoop(), initialize(), setCommand(), setFireActive(), setMeasurement(), setNavState(), and subscriberReadLoop().
|
private |
Definition at line 222 of file ControllerService.hpp.
Referenced by controllerLoop(), and getLastActuation().
|
private |
Definition at line 223 of file ControllerService.hpp.
Referenced by controllerLoop(), and getLastDiagnostics().
|
private |
Definition at line 229 of file ControllerService.hpp.
Referenced by controllerLoop(), and start().
|
private |
Definition at line 228 of file ControllerService.hpp.
Referenced by start().
|
private |
Definition at line 198 of file ControllerService.hpp.
Referenced by controllerLoop(), initialize(), and setFireActive().
|
mutableprivate |
Definition at line 221 of file ControllerService.hpp.
Referenced by controllerLoop(), getLastActuation(), and getLastDiagnostics().
|
private |
Definition at line 210 of file ControllerService.hpp.
Referenced by initialize(), sendActuationPWM(), sendPWMCommand(), and sendPWMCommands().
|
private |
Definition at line 186 of file ControllerService.hpp.
Referenced by controllerLoop(), elodinSubscriberLoop(), is_running(), start(), stop(), and subscriberReadLoop().
|
private |
Definition at line 191 of file ControllerService.hpp.
Referenced by controllerLoop(), setFireActive(), and setTestDuty().
|
private |
Definition at line 192 of file ControllerService.hpp.
Referenced by controllerLoop(), setFireActive(), and setTestDuty().
|
private |
Definition at line 235 of file ControllerService.hpp.
Referenced by controllerLoop(), initialize(), and setFireActive().
|
private |
Definition at line 232 of file ControllerService.hpp.
Referenced by initialize(), interpolateThrustCurve(), and loadThrustCurve().
|
private |
Definition at line 233 of file ControllerService.hpp.
Referenced by interpolateThrustCurve(), and loadThrustCurve().
|
private |
Definition at line 211 of file ControllerService.hpp.
Referenced by initialize(), sendPWMCommand(), sendPWMCommands(), start(), and ~ControllerService().