Skip to content

Feature/controller gain #77

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
tomasaas merged 2 commits into from
Jun 15, 2026
Merged

Feature/controller gain #77

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
a8f1306
Added feature controller gain.
tomasaas Jun 15, 2026
a335480
Commit containing correct config-values and updated PID-values.
tomasaas Jun 15, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions app.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -122,6 +122,7 @@
app.config["SETPOINT_OVERRIDE"] = init_setpoint_override(resource_monitor=app.config["RESOURCE"])
app.config["CONTROLLER"].set_setpoint_client(app.config["SETPOINT_OVERRIDE"])
app.config["CONTROLLER"].set_pid_rates(_config.get_section("pid_setpoint_rates") or {})
app.config["CONTROLLER"].set_controller_gains(_config.get_section("controller_gains") or {})

# Start control loop telemetry receiver (UDP port 5005)
app.config["CONTROL_TELEM"] = init_control_telemetry(port=5005)
Expand Down
11 changes: 11 additions & 0 deletions data/config.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,5 +27,16 @@
"roll": 45.0,
"pitch": 45.0,
"yaw": 60.0
},
"controller_gains": {
"master": 1.0,
"axes": {
"surge": 0.46,
"sway": 0.5,
"heave": 0.53,
"roll": 0.45,
"pitch": 0.5,
"yaw": 1.0
}
}
}
26 changes: 13 additions & 13 deletions data/data.json
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
{
"imu": {
"yaw": -67.73,
"pitch": 9.46,
"roll": 161.53,
"yr": -0.13,
"pr": 0.37,
"rr": -0.11,
"ax": 0.11,
"ay": -0.009,
"az": 0.024
"yaw": 128.39,
"pitch": 0.44,
"roll": -179.89,
"yr": -0.31,
"pr": -0.6,
"rr": 0.43,
"ax": -0.071,
"ay": -0.031,
"az": -0.041
},
"9dof": {
"acceleration": {
Expand Down Expand Up @@ -74,14 +74,14 @@
"dptSet": 0.0
},
"resources": {
"sequence": 5873,
"uptime_ms": 5881564,
"cpu_percent": 4,
"sequence": 9783,
"uptime_ms": 9796085,
"cpu_percent": 7,
"heap_used_percent": 2,
"heap_free_kb": 501,
"heap_total_kb": 512,
"thread_count": 20,
"udp_rx_count": 93052,
"udp_rx_count": 133160,
"udp_rx_errors": 0
},
"control_telemetry": {
Expand Down
78 changes: 23 additions & 55 deletions data/pid_configs.json
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -95,39 +95,7 @@
"kd": 0.0075
}
},
"temp1": {
"surge": {
"kp": 0.0,
"ki": 0.0,
"kd": 0.0
},
"sway": {
"kp": 0.0,
"ki": 0.0,
"kd": 0.0
},
"heave": {
"kp": 0.0,
"ki": 0.0,
"kd": 0.0
},
"roll": {
"kp": 0.0175,
"ki": 0.0,
"kd": 0.0016
},
"pitch": {
"kp": 0.012,
"ki": 0.0,
"kd": 0.0
},
"yaw": {
"kp": 0.008,
"ki": 0.0,
"kd": 0.0075
}
},
"temp2": {
"temp4": {
"surge": {
"kp": 0.0,
"ki": 0.0,
Expand All @@ -146,20 +114,20 @@
"roll": {
"kp": 0.01775,
"ki": 0.0,
"kd": 0.002
"kd": 0.0025
},
"pitch": {
"kp": 0.015,
"ki": 0.0,
"kd": 0.0
"kd": 0.00175
},
"yaw": {
"kp": 0.008,
"ki": 0.0,
"kd": 0.0075
}
},
"temp3": {
"manipulator": {
"surge": {
"kp": 0.0,
"ki": 0.0,
Expand All @@ -176,22 +144,22 @@
"kd": 0.0
},
"roll": {
"kp": 0.01775,
"ki": 0.0,
"kd": 0.0025
"kp": 0.01725,
"ki": 0.004,
"kd": 0.002
},
"pitch": {
"kp": 0.015,
"ki": 0.0,
"kd": 0.00175
"kp": 0.029,
"ki": 0.002,
"kd": 0.0018
},
"yaw": {
"kp": 0.008,
"ki": 0.0,
"kd": 0.0075
"kp": 0.028,
"ki": 0.004,
"kd": 0.004
}
},
"temp4": {
"manipulator-fine": {
"surge": {
"kp": 0.0,
"ki": 0.0,
Expand All @@ -208,19 +176,19 @@
"kd": 0.0
},
"roll": {
"kp": 0.01775,
"ki": 0.0,
"kd": 0.0025
"kp": 0.015,
"ki": 0.004,
"kd": 0.0019
},
"pitch": {
"kp": 0.015,
"ki": 0.0,
"kd": 0.00175
"kp": 0.029,
"ki": 0.002,
"kd": 0.0018
},
"yaw": {
"kp": 0.008,
"ki": 0.0,
"kd": 0.0075
"kp": 0.028,
"ki": 0.004,
"kd": 0.004
}
}
}
57 changes: 53 additions & 4 deletions lib/controller.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
ATTITUDE_AXES = ("roll", "pitch", "yaw")
ATTITUDE_LIMITS_DEG = {"roll": 180.0, "pitch": 90.0, "yaw": 180.0}
DEFAULT_PID_SETPOINT_RATES = {axis: 90.0 for axis in ATTITUDE_AXES}
DEFAULT_CONTROLLER_GAINS = {"master": 1.0, "axes": {axis: 1.0 for axis in CONTROL_AXES}}


def _use_sdl_gamecontroller():
Expand Down Expand Up @@ -53,6 +54,29 @@ def _neutral_axes():
return {axis: 0.0 for axis in CONTROL_AXES}


def _clean_controller_gains(gains):
cleaned = {"master": 1.0, "axes": {axis: 1.0 for axis in CONTROL_AXES}}
if not isinstance(gains, dict):
return cleaned

try:
cleaned["master"] = _clamp(gains.get("master", 1.0), 0.0, 1.0)
except (TypeError, ValueError):
pass

axes = gains.get("axes", {})
if not isinstance(axes, dict):
axes = gains
for axis in CONTROL_AXES:
if axis not in axes:
continue
try:
cleaned["axes"][axis] = _clamp(axes[axis], 0.0, 1.0)
except (TypeError, ValueError):
pass
return cleaned


def _controller_errors():
errors = [pygame.error]
if sdl2 is not None:
Expand Down Expand Up @@ -116,6 +140,7 @@ def __init__(self, bitmask_client: BitmaskClient = None, rate_hz: float = 60.0):
self._pid_enabled = False
self._pid_setpoints = {}
self._pid_setpoint_rates = dict(DEFAULT_PID_SETPOINT_RATES)
self._controller_gains = _clean_controller_gains(DEFAULT_CONTROLLER_GAINS)
self._last_pid_update = time.monotonic()
self._last_manual_command = _neutral_axes()
self._last_output_command = _neutral_axes()
Expand Down Expand Up @@ -230,6 +255,22 @@ def set_pid_rates(self, rates):
self._pid_setpoint_rates[axis] = _clamp(value, 0.0, 90.0)
return dict(self._pid_setpoint_rates)

def get_controller_gains(self):
with self._runtime_lock:
return {
"master": self._controller_gains["master"],
"axes": dict(self._controller_gains["axes"]),
}

def set_controller_gains(self, gains):
cleaned = _clean_controller_gains(gains)
with self._runtime_lock:
self._controller_gains = cleaned
return {
"master": cleaned["master"],
"axes": dict(cleaned["axes"]),
}

def get_control_state(self):
with self._debug_lock:
override_active = self._debug_override is not None
Expand All @@ -246,6 +287,10 @@ def get_control_state(self):
"pid_setpoints": dict(self._pid_setpoints),
"active_setpoints": dict(self._pid_setpoints) if self._pid_enabled else {},
"pid_setpoint_rates": dict(self._pid_setpoint_rates),
"controller_gains": {
"master": self._controller_gains["master"],
"axes": dict(self._controller_gains["axes"]),
},
"control_path": control_path,
"override_active": override_active,
"manual_command_before_pid": dict(self._last_manual_command),
Expand Down Expand Up @@ -556,13 +601,17 @@ def _dispatch_manual_axes(self, axes, source):
now = time.monotonic()
setpoints_to_send = None
with self._runtime_lock:
gains = self._controller_gains
manual_after_gain = {
axis: manual[axis] * gains["master"] * gains["axes"].get(axis, 1.0) for axis in CONTROL_AXES
}
if self._killed:
output = _neutral_axes()
self._last_manual_command = dict(manual)
self._last_manual_command = dict(manual_after_gain)
self._last_output_command = dict(output)
self._last_runtime_source = "KILLED"
else:
output = dict(manual)
output = dict(manual_after_gain)
if self._pid_enabled:
dt = _clamp(now - self._last_pid_update, 0.0, 0.25)
self._last_pid_update = now
Expand All @@ -571,14 +620,14 @@ def _dispatch_manual_axes(self, axes, source):
output[axis] = 0.0
if axis not in self._pid_setpoints:
continue
delta = manual[axis] * self._pid_setpoint_rates[axis] * dt
delta = manual_after_gain[axis] * self._pid_setpoint_rates[axis] * dt
if abs(delta) < 0.000001:
continue
self._pid_setpoints[axis] = _clamp_setpoint(axis, self._pid_setpoints[axis] + delta)
changed = True
if changed:
setpoints_to_send = dict(self._pid_setpoints)
self._last_manual_command = dict(manual)
self._last_manual_command = dict(manual_after_gain)
self._last_output_command = dict(output)
self._last_runtime_source = source

Expand Down
Loading
Loading