Skip to content

Commit 9194ddb

Browse files
committed
feat(ESC): Component editor ESC connection interdependencies
1 parent 08a3619 commit 9194ddb

9 files changed

Lines changed: 1722 additions & 144 deletions

ardupilot_methodic_configurator/data_model_vehicle_components_import.py

Lines changed: 21 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -26,15 +26,16 @@
2626
from ardupilot_methodic_configurator.data_model_vehicle_components_validation import (
2727
BATT_MONITOR_CONNECTION,
2828
CAN_PORTS,
29-
ESC_TELEMETRY_PROTOCOLS,
29+
ESC_TELEMETRY_ONLY_PROTOCOLS,
3030
GNSS_RECEIVER_CONNECTION,
3131
I2C_PORTS,
32+
PWM_OUT_PORTS,
3233
RC_PROTOCOLS_DICT,
3334
SERIAL_PORTS,
3435
SERIAL_PROTOCOLS_DICT,
3536
SERVO_FUNCTION_ESC_CONTROL,
3637
ComponentDataModelValidation,
37-
get_mot_pwm_type_sub_dict,
38+
get_esc_connection_sub_dict,
3839
)
3940

4041

@@ -179,7 +180,7 @@ def process_fc_parameters(
179180
elif "GPS_TYPE" in doc:
180181
self._verify_dict_is_uptodate(doc, GNSS_RECEIVER_CONNECTION, "GPS_TYPE", "values")
181182
fw_type = str(self.get_component_value(("Flight Controller", "Firmware", "Type")) or "")
182-
self._verify_dict_is_uptodate(doc, get_mot_pwm_type_sub_dict(fw_type), "MOT_PWM_TYPE", "values")
183+
self._verify_dict_is_uptodate(doc, get_esc_connection_sub_dict(fw_type), "MOT_PWM_TYPE", "values")
183184
self._verify_dict_is_uptodate(doc, RC_PROTOCOLS_DICT, "RC_PROTOCOLS", "Bitmask")
184185

185186
# Process parameters in sequence
@@ -330,7 +331,7 @@ def _process_serial_components(self, fc_parameters: dict) -> bool: # pylint: di
330331
self.set_component_value(("GNSS Receiver", "FC Connection", "Type"), serial)
331332
gnss += 1
332333
elif component == "ESC":
333-
if protocol in ESC_TELEMETRY_PROTOCOLS:
334+
if protocol in ESC_TELEMETRY_ONLY_PROTOCOLS:
334335
# Serial ESC->FC telemetry only (DShot with UART feedback, or Hobbywing Datalink v2).
335336
# FC->ESC connection is still PWM/DShot; _set_esc_type_from_fc_parameters handles it.
336337
# Do NOT increment esc so that function is still called.
@@ -372,13 +373,13 @@ def _set_esc_type_from_fc_parameters(self, fc_parameters: dict[str, float], doc:
372373
protocol = doc["MOT_PWM_TYPE"]["values"].get(str(mot_pwm_type), "")
373374
if protocol:
374375
self.set_component_value(("ESC", "FC->ESC Connection", "Protocol"), protocol)
375-
# Fallback to MOT_PWM_TYPE_DICT if doc is not available
376+
# Fallback to ESC_CONNECTION_DICT if doc is not available
376377
else:
377-
mot_pwm_sub = get_mot_pwm_type_sub_dict(
378+
esc_conn_sub = get_esc_connection_sub_dict(
378379
str(self.get_component_value(("Flight Controller", "Firmware", "Type")) or "")
379380
)
380-
if str(mot_pwm_type) in mot_pwm_sub:
381-
protocol = str(mot_pwm_sub[str(mot_pwm_type)]["protocol"])
381+
if str(mot_pwm_type) in esc_conn_sub:
382+
protocol = str(esc_conn_sub[str(mot_pwm_type)]["protocol"])
382383
self.set_component_value(("ESC", "FC->ESC Connection", "Protocol"), protocol)
383384

384385
# Set ESC->FC Telemetry: DShot supports BDShot telemetry on the same PWM wire.
@@ -388,9 +389,9 @@ def _set_esc_type_from_fc_parameters(self, fc_parameters: dict[str, float], doc:
388389
current_telemetry_type = self.get_component_value(("ESC", "ESC->FC Telemetry", "Type"))
389390
if protocol and protocol.startswith("DShot"):
390391
if current_telemetry_type not in SERIAL_PORTS and current_telemetry_type not in CAN_PORTS:
391-
# No dedicated serial/CAN telemetry port detected; fall back to BDShot on the PWM wire
392+
# No dedicated serial/CAN telemetry port detected; fall back to BDShotOnly on the PWM wire
392393
self.set_component_value(("ESC", "ESC->FC Telemetry", "Type"), esc_conn_type)
393-
self.set_component_value(("ESC", "ESC->FC Telemetry", "Protocol"), "BDShot")
394+
self.set_component_value(("ESC", "ESC->FC Telemetry", "Protocol"), "BDShotOnly")
394395
elif not current_telemetry_type or (
395396
current_telemetry_type not in SERIAL_PORTS and current_telemetry_type not in CAN_PORTS
396397
):
@@ -688,10 +689,18 @@ def _set_motor_poles_from_fc_parameters(self, fc_parameters: dict[str, float]) -
688689
poles = 0.0
689690
if "MOT_PWM_TYPE" in fc_parameters:
690691
mot_pwm_type_str = str(int(fc_parameters["MOT_PWM_TYPE"]))
691-
mot_pwm_sub = get_mot_pwm_type_sub_dict(
692+
esc_conn_sub = get_esc_connection_sub_dict(
692693
str(self.get_component_value(("Flight Controller", "Firmware", "Type")) or "")
693694
)
694-
if mot_pwm_type_str in mot_pwm_sub and mot_pwm_sub[mot_pwm_type_str].get("is_dshot", False):
695+
entry = esc_conn_sub.get(mot_pwm_type_str)
696+
is_dshot = (
697+
entry is not None
698+
and isinstance(entry.get("ESC_to_FC"), dict)
699+
and ("same_as_FC_to_ESC",) in entry["ESC_to_FC"] # type: ignore[operator]
700+
and isinstance(entry.get("type"), tuple)
701+
and entry["type"] == PWM_OUT_PORTS
702+
)
703+
if is_dshot:
695704
if fc_parameters.get("SERVO_BLH_POLES"): # DShot ESCs
696705
poles = fc_parameters["SERVO_BLH_POLES"]
697706
elif fc_parameters.get("SERVO_FTW_MASK") and fc_parameters.get("SERVO_FTW_POLES"):

0 commit comments

Comments
 (0)