Skip to content

Commit

Permalink
Add documentation onto controller metrics node
Browse files Browse the repository at this point in the history
  • Loading branch information
AndrewHWang1605 committed Apr 25, 2024
1 parent 32b6504 commit 0748328
Showing 1 changed file with 12 additions and 2 deletions.
14 changes: 12 additions & 2 deletions ff_sim/ff_sim/controller_metrics.py
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand All @@ -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:
Expand All @@ -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)
Expand Down

0 comments on commit 0748328

Please sign in to comment.