stsL/argo
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects 1 Releases Wiki Activity

Save pre-V2 motor controls and UI checkpoint

Browse Source
This commit is contained in:
PitresAli
2026-10-22 19:31:28 +02:00
parent a4d8f1b7af
commit 5e91c28f1e
15 changed files with 1041 additions and 236 deletions
Download Patch File Download Diff File Expand all files Collapse all files
SUB Control/app.py
+154 -8
View File
@@ -87,6 +87,8 @@ class MavlinkWorker:
# At 20 Hz this produces five small steps from neutral to full command.
self.ramp_seconds = 0.25
self.gain = 0.20
self.independent_motor_scaling = False
self.motor_gains = {motor: 20 for motor in range(1, 13)}
self.rate_hz = 20
self.target_system = 0
self.target_component = 0
@@ -110,7 +112,7 @@ class MavlinkWorker:
self.heavy_pending_pings = {}
self.heavy_quality_latest = None
self.telemetry_rates_hz = {
"SYS_STATUS": 1, "SERVO_OUTPUT_RAW": 2,
"SYS_STATUS": 1, "BATTERY_STATUS": 1, "SERVO_OUTPUT_RAW": 2,
"RC_CHANNELS": 2, "ESC_TELEMETRY_1_TO_4": 1,
}
self.ping_sequence = 0
@@ -128,6 +130,11 @@ class MavlinkWorker:
self.param_retries = 0
self.param_types = {}
self.pending_sets = {}
self.motor_test_lock = threading.Lock()
self.motor_test_request = None
self.motor_test_mode = False
self.motor_test_acknowledged = False
self.motor_test_last_motor = None
self.log_lock = threading.Lock()
self.log_file = None
self.log_path = None
@@ -200,8 +207,10 @@ class MavlinkWorker:
# ArduSub cannot use MANUAL_CONTROL while disarmed. Keeping a
# 20 Hz neutral stream alive here only competes with ARM and
# its acknowledgement on constrained links.
motor_test_mode = self._tick_motor_test(now)
if self.target_system and self.armed and now >= next_control:
self._send_control()
if not motor_test_mode:
self._send_control()
# The first disarm packet is preceded by a neutral command.
# Until disarm is confirmed, keep sending neutral at a
# lower rate instead of consuming 20 Hz on the weak link.
@@ -245,6 +254,10 @@ class MavlinkWorker:
self.next_arm_retry_at = 0.0
self.arm_retry_deadline = 0.0
self.arm_confirmation = 0
with self.motor_test_lock:
self.motor_test_request = None
self.motor_test_mode = False
self.motor_test_last_motor = None
self.stream_config.clear()
self.pending_pings.clear()
self.ping_results.clear()
@@ -337,6 +350,13 @@ class MavlinkWorker:
armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
self.armed = armed
self.config_paused = armed
if not armed:
with self.motor_test_lock:
was_testing = self.motor_test_mode
self.motor_test_request = None
self.motor_test_mode = False
if was_testing:
self.events.put(("motor_test_stopped", "Vehicle disarmed"))
if self.pending_arm_state is not None and armed == self.pending_arm_state:
self.pending_arm_state = None
if not armed:
@@ -361,6 +381,18 @@ class MavlinkWorker:
if int(msg.command) == mavutil.mavlink.MAV_CMD_SET_MESSAGE_INTERVAL:
self.log("RX_STREAM_CONFIG_ACK", result_name)
return
if int(msg.command) == mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST:
self.log("RX_MOTOR_TEST_ACK", result_name)
if result_name in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS"):
if not self.motor_test_acknowledged:
self.motor_test_acknowledged = True
self.events.put(("motor_test_ack", result_name))
else:
with self.motor_test_lock:
self.motor_test_request = None
self.motor_test_mode = False
self.events.put(("motor_test_rejected", result_name))
return
if (int(msg.command) == mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM
and result_name not in ("MAV_RESULT_ACCEPTED", "MAV_RESULT_IN_PROGRESS")):
self.pending_arm_state = None
@@ -375,8 +407,33 @@ class MavlinkWorker:
self.events.put(("statustext", text))
elif msg_type == "SYS_STATUS":
voltage_mv = int(msg.voltage_battery)
if 0 < voltage_mv < 65535:
self.events.put(("battery", voltage_mv / 1000.0))
current_ca = int(msg.current_battery)
remaining = int(msg.battery_remaining)
self.events.put(("battery", {
"voltage": voltage_mv / 1000.0 if 0 < voltage_mv < 65535 else None,
"current_a": current_ca / 100.0 if current_ca >= 0 else None,
"remaining_percent": remaining if 0 <= remaining <= 100 else None,
}))
elif msg_type == "BATTERY_STATUS":
voltages = [int(value) for value in getattr(msg, "voltages", [])]
valid_voltages = [value for value in voltages if 0 < value < 65535]
voltage = sum(valid_voltages) / 1000.0 if valid_voltages else None
current_ca = int(msg.current_battery)
consumed = int(msg.current_consumed)
remaining = int(msg.battery_remaining)
self.events.put(("battery", {
"voltage": voltage,
"current_a": current_ca / 100.0 if current_ca >= 0 else None,
"consumed_mah": consumed if consumed >= 0 else None,
"remaining_percent": remaining if 0 <= remaining <= 100 else None,
}))
elif msg_type == "SERVO_OUTPUT_RAW":
base = int(getattr(msg, "port", 0)) * 8 + 1
outputs = {}
for index in range(1, 9):
raw = int(getattr(msg, f"servo{index}_raw", 0))
outputs[str(base + index - 1)] = raw if 0 < raw < 65535 else None
self.events.put(("servo_output", outputs))
elif msg_type == "PARAM_VALUE":
param_id = msg.param_id
if isinstance(param_id, bytes):
@@ -479,6 +536,67 @@ class MavlinkWorker:
"probe_supported": total > 0,
}))
def _send_motor_test(self, motor, throttle_percent):
if not self.connection or not self.target_system:
raise RuntimeError("No autopilot heartbeat")
self.log("TX_MOTOR_TEST", f"motor={motor} throttle={throttle_percent}%")
self._send_mav(self.connection, "command_int_send",
self.target_system, self.target_component,
mavutil.mavlink.MAV_FRAME_GLOBAL,
mavutil.mavlink.MAV_CMD_DO_MOTOR_TEST, 0, 0,
float(motor), 0.0, float(throttle_percent), 1.0, 1, 0, 0,
)
def start_motor_test(self, motor, throttle_percent):
motor, throttle_percent = int(motor), int(throttle_percent)
if not 1 <= motor <= 12 or not 0 <= throttle_percent <= 100:
raise ValueError("Choose a motor and a test throttle from 0% to 100%")
if not self.connection or not self.target_system:
raise RuntimeError("Connect to the vehicle before testing a motor")
if not self.armed or time.monotonic() - self.last_heartbeat > 2.0:
raise RuntimeError("Arm the vehicle and confirm fresh telemetry before motor testing")
with self.motor_test_lock:
if self.motor_test_mode:
raise RuntimeError("Finish the current motor test before starting another")
self.motor_test_request = {"motor": motor, "throttle": throttle_percent, "next_at": 0.0}
self.motor_test_last_motor = motor
self.motor_test_mode = True
self.motor_test_acknowledged = False
self.set_axes(Axes())
self.log("OPERATOR", f"held motor test for motor {motor} at {throttle_percent}%")
return True
def stop_motor_test(self):
with self.motor_test_lock:
request = self.motor_test_request
if request is not None:
self.motor_test_last_motor = request["motor"]
motor = self.motor_test_last_motor
was_testing = self.motor_test_mode
self.motor_test_request = None
if was_testing and motor and self.connection and self.target_system:
self._send_motor_test(motor, 0)
return True
def _tick_motor_test(self, now):
motor = throttle = None
with self.motor_test_lock:
request = self.motor_test_request
active = self.motor_test_mode
if request is not None and not self.armed:
self.motor_test_request = None
self.motor_test_mode = False
active = False
self.events.put(("motor_test_stopped", "Vehicle disarmed"))
elif request is not None and now >= request["next_at"]:
request["next_at"] = now + 0.25
motor, throttle = request["motor"], request["throttle"]
else:
motor = throttle = None
if motor is not None:
self._send_motor_test(motor, throttle)
return active
def _tick_link_probe(self, now):
if self.heavy_quality_test is not None:
self._tick_heavy_quality(now)
@@ -753,6 +871,8 @@ class MavlinkWorker:
active = not (force_neutral or expired)
target = self.axes if active else Axes()
gain = self.gain
independent_motor_scaling = self.independent_motor_scaling
motor_gains = dict(self.motor_gains)
if force_neutral:
self.output_axes = Axes()
else:
@@ -775,10 +895,25 @@ class MavlinkWorker:
)
limited_forward = axes.forward / horizontal_mix_peak
limited_yaw = axes.yaw / horizontal_mix_peak
x = round(limited_forward * gain * 1000)
y = round(axes.lateral * gain * 1000)
r = round(limited_yaw * gain * 1000)
z = round(NEUTRAL_Z + axes.vertical * gain * 500)
if independent_motor_scaling:
# The desktop controller's SimpleROV-3 profile maps motors 1/2
# to surge/turn and motor 3 to vertical thrust. Convert the two
# independently scaled horizontal demands back into MANUAL_CONTROL
# axes so ArduSub's configured mixer remains in charge.
motor_1 = (limited_forward + limited_yaw) * motor_gains.get(1, 100) / 100.0
motor_2 = (limited_forward - limited_yaw) * motor_gains.get(2, 100) / 100.0
limited_forward = (motor_1 + motor_2) / 2.0
limited_yaw = (motor_1 - motor_2) / 2.0
vertical_gain = motor_gains.get(3, 100) / 100.0
x = round(limited_forward * 1000)
y = round(axes.lateral * 1000)
r = round(limited_yaw * 1000)
z = round(NEUTRAL_Z + axes.vertical * vertical_gain * 500)
else:
x = round(limited_forward * gain * 1000)
y = round(axes.lateral * gain * 1000)
r = round(limited_yaw * gain * 1000)
z = round(NEUTRAL_Z + axes.vertical * gain * 500)
self.log(
"TX_MANUAL_CONTROL",
f"x={x} y={y} z={z} r={r} active={active} "
@@ -851,6 +986,17 @@ class MavlinkWorker:
manual,
)
def set_motor_scaling(self, independent, gains):
clean = {motor: max(0, min(100, int(gains.get(motor, 100)))) for motor in range(1, 13)}
with self.lock:
self.independent_motor_scaling = bool(independent)
self.motor_gains = clean
def motor_output_scale(self, motor):
with self.lock:
return (self.motor_gains.get(int(motor), 100) / 100.0
if self.independent_motor_scaling else self.gain)
def set_axes(self, axes: Axes, sequence=None):
def conditioned(value):
# Ignore tiny pointer/key return noise, then quantize to stable 1% steps.