From 07483282ad644f5412b82f604cde0b47c26b48cb Mon Sep 17 00:00:00 2001 From: Andrew Wang Date: Wed, 24 Apr 2024 19:16:08 -0700 Subject: [PATCH] Add documentation onto controller metrics node --- ff_sim/ff_sim/controller_metrics.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/ff_sim/ff_sim/controller_metrics.py b/ff_sim/ff_sim/controller_metrics.py index 1c1ad2c..cb541f8 100755 --- a/ff_sim/ff_sim/controller_metrics.py +++ b/ff_sim/ff_sim/controller_metrics.py @@ -50,7 +50,15 @@ class ControllerMetricsPublisher(Node): - """Class to listen to free flyer commands and calculate metrics""" + """ + Class to listen to free flyer commands and calculate metrics + Calculates two key metrics: + 1. total_gas_time: Measures total time that thrusters are on (summed over each thruster). Provides + a proxy for total gas expenditure over time + 2. running_duty_cycles: Measures average duty cycle for each thruster over a time window specified by + self.duty_cycle_window. + + """ def __init__(self): super().__init__("ff_ctrl_metrics") @@ -75,7 +83,7 @@ def __init__(self): ) def process_new_wheel_cmd(self, msg: WheelVelCommand) -> None: - """Doesn't process wheel command""" + """Placeholder for now""" pass def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: @@ -85,10 +93,12 @@ def process_new_binary_thrust_cmd(self, msg: ThrusterCommand) -> None: dtnsec = now.nanosec - self.curr_time.nanosec dt = dtsec + dtnsec / 1e9 + # Perform Euler integration for how long each thruster was on thrusters = np.array(msg.switches, dtype=float) self.running_total_gas += self.prev_thruster_sum * dt self.prev_thruster_sum = np.sum(thrusters) + # Calculate rolling average of duty cycle for each thruster self.steps += 1 if not self.rolled_up: # Ensure valid duty cycles at the beginning self.time_hist.append(dt)