Skip to content

Commit 46d609c

Browse files
committed
feat(vehicle-components): add BATT_ARM_VOLT, MOT_BAT_VOLT_MIN per-cell support
add battery voltage path Volt per cell arm/min in validation constants and schema path coverage add regression tests for arm/min voltage conversions
1 parent c64993a commit 46d609c

12 files changed

+338
-72
lines changed

ardupilot_methodic_configurator/battery_cell_voltages.py

Lines changed: 31 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -15,57 +15,73 @@
1515
"absolute_max": 4.2,
1616
"absolute_min": 2.5,
1717
"recommended_max": 4.1,
18+
"recommended_arm": 3.6,
1819
"recommended_low": 3.1,
1920
"recommended_crit": 2.8,
21+
"recommended_min": 2.7,
2022
},
2123
"LiIonSS": {
2224
"absolute_max": 4.2,
2325
"absolute_min": 2.4,
2426
"recommended_max": 4.2,
27+
"recommended_arm": 3.6,
2528
"recommended_low": 3.0,
2629
"recommended_crit": 2.7,
30+
"recommended_min": 2.6,
2731
},
2832
"LiIonSSHV": {
2933
"absolute_max": 4.45,
3034
"absolute_min": 2.4,
3135
"recommended_max": 4.45,
36+
"recommended_arm": 3.8,
3237
"recommended_low": 3.0,
3338
"recommended_crit": 2.7,
39+
"recommended_min": 2.6,
3440
},
3541
"Lipo": {
3642
"absolute_max": 4.2,
3743
"absolute_min": 3.0,
3844
"recommended_max": 4.2,
45+
"recommended_arm": 3.8,
3946
"recommended_low": 3.6,
4047
"recommended_crit": 3.3,
48+
"recommended_min": 3.2,
4149
},
4250
"LipoHV": {
4351
"absolute_max": 4.35,
4452
"absolute_min": 3.0,
4553
"recommended_max": 4.35,
54+
"recommended_arm": 3.9,
4655
"recommended_low": 3.6,
4756
"recommended_crit": 3.3,
57+
"recommended_min": 3.2,
4858
},
4959
"LipoHVSS": {
5060
"absolute_max": 4.2,
5161
"absolute_min": 2.9,
5262
"recommended_max": 4.2,
63+
"recommended_arm": 3.8,
5364
"recommended_low": 3.5,
5465
"recommended_crit": 3.2,
66+
"recommended_min": 3.1,
5567
},
5668
"NiCd": {
5769
"absolute_max": 1.45,
5870
"absolute_min": 1.0,
5971
"recommended_max": 1.4,
72+
"recommended_arm": 1.28,
6073
"recommended_low": 1.2,
6174
"recommended_crit": 1.1,
75+
"recommended_min": 1.05,
6276
},
6377
"NiMH": {
6478
"absolute_max": 1.45,
6579
"absolute_min": 1.0,
6680
"recommended_max": 1.4,
81+
"recommended_arm": 1.28,
6782
"recommended_low": 1.2,
6883
"recommended_crit": 1.1,
84+
"recommended_min": 1.05,
6985
},
7086
# Add more chemistries as needed
7187
}
@@ -101,16 +117,28 @@ def limit_min_voltage(chemistry: str) -> float:
101117
def recommended_max_voltage(chemistry: str) -> float:
102118
if chemistry not in battery_cell_voltages:
103119
return nan
104-
return battery_cell_voltages[chemistry].get("recommended_max", 4.2)
120+
return battery_cell_voltages[chemistry].get("recommended_max", nan)
121+
122+
@staticmethod
123+
def recommended_arm_voltage(chemistry: str) -> float:
124+
if chemistry not in battery_cell_voltages:
125+
return nan
126+
return battery_cell_voltages[chemistry].get("recommended_arm", nan)
105127

106128
@staticmethod
107129
def recommended_low_voltage(chemistry: str) -> float:
108130
if chemistry not in battery_cell_voltages:
109131
return nan
110-
return battery_cell_voltages[chemistry].get("recommended_low", 3.6)
132+
return battery_cell_voltages[chemistry].get("recommended_low", nan)
111133

112134
@staticmethod
113135
def recommended_crit_voltage(chemistry: str) -> float:
114136
if chemistry not in battery_cell_voltages:
115137
return nan
116-
return battery_cell_voltages[chemistry].get("recommended_crit", 3.3)
138+
return battery_cell_voltages[chemistry].get("recommended_crit", nan)
139+
140+
@staticmethod
141+
def recommended_min_voltage(chemistry: str) -> float:
142+
if chemistry not in battery_cell_voltages:
143+
return nan
144+
return battery_cell_voltages[chemistry].get("recommended_min", nan)

ardupilot_methodic_configurator/configuration_steps_ArduCopter.json

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -129,14 +129,14 @@
129129
"BATT_FS_LOW_ACT": { "New Value": 2, "Change Reason": "Return and land at home or rally point" }
130130
},
131131
"derived_parameters": {
132-
"BATT_ARM_VOLT": { "New Value": "(vehicle_components['Battery']['Specifications']['Number of cells']-1)*0.1+(vehicle_components['Battery']['Specifications']['Volt per cell crit']+0.3)*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "Do not allow arming below this voltage" },
132+
"BATT_ARM_VOLT": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell arm']+0.0)*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "Only arm above this voltage, to avoid taking off with insufficient battery capacity" },
133133
"BATT_CAPACITY": { "New Value": "(vehicle_components['Battery']['Specifications']['Capacity mAh'])", "Change Reason": "Total battery capacity specified in the component editor" },
134134
"BATT_CRT_VOLT": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell crit'])*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "(Critical voltage + 0.0) x no. of cells" },
135135
"BATT_LOW_VOLT": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell low'])*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "(Low voltage + 0.0) x no. of cells" },
136136
"BATT_MONITOR": { "New Value": "vehicle_components['Battery Monitor']['FC Connection']['Protocol']", "Change Reason": "Selected in component editor window" },
137137
"BATT_I2C_BUS": { "New Value": "1 if vehicle_components['Battery Monitor']['FC Connection']['Type'] == 'I2C2' else 2 if vehicle_components['Battery Monitor']['FC Connection']['Type'] == 'I2C3' else 3 if vehicle_components['Battery Monitor']['FC Connection']['Type'] == 'I2C4' else 0", "Change Reason": "Selected in component editor window" },
138-
"MOT_BAT_VOLT_MAX": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell max']+0.0)*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "(Max voltage + 0.0) x no. of cells" },
139-
"MOT_BAT_VOLT_MIN": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell crit']+0.0)*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "(Critical voltage + 0.0) x no. of cells" }
138+
"MOT_BAT_VOLT_MAX": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell max']+0.0)*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "Scale the PIDs up when battery voltage is below this threshold" },
139+
"MOT_BAT_VOLT_MIN": { "New Value": "(vehicle_components['Battery']['Specifications']['Volt per cell min']+0.0)*vehicle_components['Battery']['Specifications']['Number of cells']", "Change Reason": "Scale the PIDs up when battery voltage is above this threshold" }
140140
},
141141
"rename_connection": "vehicle_components['Battery Monitor']['FC Connection']['Type']",
142142
"old_filenames": [],

ardupilot_methodic_configurator/data_model_vehicle_components_import.py

Lines changed: 112 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,12 @@
99
"""
1010

1111
import contextlib
12+
from dataclasses import dataclass
1213

1314
# from logging import debug as logging_debug
1415
from logging import error as logging_error
1516
from logging import warning as logging_warning
17+
from math import isnan, nan
1618
from typing import Any, Optional
1719

1820
from ardupilot_methodic_configurator import _
@@ -55,6 +57,17 @@ def is_single_bit_set(value: int) -> bool:
5557
return value > 0 and value & (value - 1) == 0
5658

5759

60+
@dataclass
61+
class BatteryVoltageSpecs:
62+
"""Data class to hold battery voltage specifications for import processing."""
63+
64+
estimated_cell_count: int
65+
limit_min: float
66+
limit_max: float
67+
detected_chemistry: str
68+
fc_parameters: dict[str, float]
69+
70+
5871
class ComponentDataModelImport(ComponentDataModelBase):
5972
"""
6073
A class to handle component data import from FC parameters separate from UI logic.
@@ -371,6 +384,30 @@ def _set_battery_type_from_fc_parameters(self, fc_parameters: dict[str, float])
371384
else:
372385
logging_warning(_("BATT_MONITOR parameter not found in fc_parameters"))
373386

387+
# Detect battery chemistry from voltage parameters before estimating cell count
388+
# Only update if current chemistry is clearly wrong or not set
389+
current_chemistry = str(self.get_component_value(("Battery", "Specifications", "Chemistry")))
390+
detected_chemistry = self._detect_battery_chemistry_from_voltages(fc_parameters, current_chemistry)
391+
if detected_chemistry and detected_chemistry != current_chemistry:
392+
self.set_component_value(("Battery", "Specifications", "Chemistry"), detected_chemistry)
393+
394+
# if chemistry detection fails, use current chemistry if valid, otherwise default to "Lipo"
395+
final_chemistry = detected_chemistry or current_chemistry or "Lipo"
396+
if final_chemistry not in BatteryCell.chemistries():
397+
final_chemistry = "Lipo"
398+
399+
specs = BatteryVoltageSpecs(
400+
estimated_cell_count=self._estimate_battery_cell_count(fc_parameters),
401+
limit_min=BatteryCell.limit_min_voltage(final_chemistry),
402+
limit_max=BatteryCell.limit_max_voltage(final_chemistry),
403+
detected_chemistry=final_chemistry,
404+
fc_parameters=fc_parameters,
405+
)
406+
self._import_bat_values_from_fc(specs)
407+
408+
def _import_bat_values_from_fc(self, specs: BatteryVoltageSpecs) -> None:
409+
fc_parameters = specs.fc_parameters
410+
374411
if "BATT_CAPACITY" in fc_parameters:
375412
try:
376413
batt_capacity = int(fc_parameters["BATT_CAPACITY"])
@@ -383,34 +420,54 @@ def _set_battery_type_from_fc_parameters(self, fc_parameters: dict[str, float])
383420
else:
384421
logging_warning(_("BATT_CAPACITY parameter not found in fc_parameters"))
385422

386-
# Detect battery chemistry from voltage parameters before estimating cell count
387-
# Only update if current chemistry is clearly wrong or not set
388-
current_chemistry = str(self.get_component_value(("Battery", "Specifications", "Chemistry")))
389-
detected_chemistry = self._detect_battery_chemistry_from_voltages(fc_parameters, current_chemistry)
390-
if detected_chemistry and detected_chemistry != current_chemistry:
391-
self.set_component_value(("Battery", "Specifications", "Chemistry"), detected_chemistry)
423+
if specs.estimated_cell_count > 0:
424+
self.import_bat_voltage(specs, "MOT_BAT_VOLT_MAX", "max")
425+
self.import_bat_voltage(specs, "BATT_ARM_VOLT", "arm")
426+
self.import_bat_voltage(specs, "BATT_LOW_VOLT", "low")
427+
self.import_bat_voltage(specs, "BATT_CRT_VOLT", "crit")
428+
self.import_bat_voltage(specs, "MOT_BAT_VOLT_MIN", "min")
392429

393-
# Estimate number of cells from voltage parameters
394-
self._estimate_battery_cell_count(fc_parameters)
430+
def import_bat_voltage(self, specs: BatteryVoltageSpecs, param_name: str, voltage_type: str) -> None:
431+
if param_name in specs.fc_parameters:
432+
try:
433+
cell = specs.fc_parameters[param_name] / specs.estimated_cell_count
434+
if specs.limit_min <= cell <= specs.limit_max:
435+
self.set_component_value(("Battery", "Specifications", f"Volt per cell {voltage_type}"), cell)
436+
else:
437+
logging_warning(
438+
_("Calculated cell %s voltage %.2f V is out of expected range for %s chemistry (%.2f - %.2f V)"),
439+
voltage_type,
440+
cell,
441+
specs.detected_chemistry,
442+
specs.limit_min,
443+
specs.limit_max,
444+
)
445+
except (ValueError, TypeError) as e:
446+
logging_error(_("Error processing %s parameter: %s"), param_name, str(e))
395447

396448
def _get_volt_per_cell_for_type(self, chemistry: str, voltage_type: str) -> float:
397449
"""
398450
Get voltage per cell for a given chemistry and voltage type.
399451
400452
Args:
401453
chemistry: Battery chemistry type
402-
voltage_type: One of "recommended_max", "recommended_low", "recommended_crit"
454+
voltage_type: One of "recommended_max", "recommended_arm", "recommended_low", "recommended_crit", "recommended_min"
403455
404456
Returns:
405457
Voltage per cell value
406458
407459
"""
408460
if voltage_type == "recommended_max":
409461
return BatteryCell.recommended_max_voltage(chemistry)
462+
if voltage_type == "recommended_arm":
463+
return BatteryCell.recommended_arm_voltage(chemistry)
410464
if voltage_type == "recommended_low":
411465
return BatteryCell.recommended_low_voltage(chemistry)
412-
# voltage_type == "recommended_crit"
413-
return BatteryCell.recommended_crit_voltage(chemistry)
466+
if voltage_type == "recommended_crit":
467+
return BatteryCell.recommended_crit_voltage(chemistry)
468+
if voltage_type == "recommended_min":
469+
return BatteryCell.recommended_min_voltage(chemistry)
470+
return nan
414471

415472
def _detect_battery_chemistry_from_voltages(
416473
self, fc_parameters: dict[str, float], current_chemistry: Optional[str] = None
@@ -431,11 +488,16 @@ def _detect_battery_chemistry_from_voltages(
431488
or current chemistry is acceptable
432489
433490
"""
434-
# Priority: MOT_BAT_VOLT_MAX > BATT_LOW_VOLT > BATT_CRT_VOLT
491+
# Priority: MOT_BAT_VOLT_MAX > BATT_LOW_VOLT > BATT_CRT_VOLT > BATT_ARM_VOLT > MOT_BAT_VOLT_MIN
435492
voltage_params = [
436493
("MOT_BAT_VOLT_MAX", "recommended_max"),
437494
("BATT_LOW_VOLT", "recommended_low"),
438495
("BATT_CRT_VOLT", "recommended_crit"),
496+
(
497+
"BATT_ARM_VOLT",
498+
"recommended_arm",
499+
), # lower priority, less commonly set, but can provide additional clues if available
500+
("MOT_BAT_VOLT_MIN", "recommended_min"), # lowest priority, but can be a last resort for detection
439501
]
440502

441503
# Try each voltage parameter in priority order
@@ -451,22 +513,28 @@ def _detect_battery_chemistry_from_voltages(
451513
# Check if current chemistry is consistent with this parameter
452514
if current_chemistry and current_chemistry in BatteryCell.chemistries():
453515
volt_per_cell = self._get_volt_per_cell_for_type(current_chemistry, voltage_type)
454-
if volt_per_cell > 0:
455-
estimated_cells = total_voltage / volt_per_cell
456-
current_chemistry_score = abs(estimated_cells - round(estimated_cells))
516+
if isnan(volt_per_cell) or volt_per_cell <= 0:
517+
logging_warning(
518+
_("Current chemistry %s does not have valid volt per cell for %s, ignoring current chemistry"),
519+
current_chemistry,
520+
voltage_type,
521+
)
522+
continue
523+
estimated_cells = total_voltage / volt_per_cell
524+
current_chemistry_score = abs(estimated_cells - round(estimated_cells))
457525

458-
# If current chemistry gives acceptable results (<40% error), keep it
459-
# This very conservative threshold prevents changing chemistry unless clearly wrong
460-
if current_chemistry_score < 0.40:
461-
return None # Keep current chemistry
526+
# If current chemistry gives acceptable results (<40% error), keep it
527+
# This very conservative threshold prevents changing chemistry unless clearly wrong
528+
if current_chemistry_score < 0.40:
529+
return None # Keep current chemistry
462530

463531
# Find best matching chemistry
464532
best_chemistry = None
465533
best_score = float("inf")
466534

467535
for chemistry in BatteryCell.chemistries():
468536
volt_per_cell = self._get_volt_per_cell_for_type(chemistry, voltage_type)
469-
if volt_per_cell <= 0:
537+
if isnan(volt_per_cell) or volt_per_cell <= 0:
470538
continue
471539

472540
estimated_cells = total_voltage / volt_per_cell
@@ -520,19 +588,19 @@ def _estimate_cells_from_voltage_param(
520588

521589
return None
522590

523-
def _estimate_battery_cell_count(self, fc_parameters: dict[str, float]) -> None:
591+
def _estimate_battery_cell_count(self, fc_parameters: dict[str, float]) -> int:
524592
"""
525593
Estimate battery cell count from voltage parameters.
526594
527-
Uses MOT_BAT_VOLT_MAX, BATT_LOW_VOLT, or BATT_CRT_VOLT along with
528-
current volt-per-cell values to estimate the number of cells.
595+
Uses MOT_BAT_VOLT_MAX, BATT_LOW_VOLT, BATT_CRT_VOLT, BATT_ARM_VOLT, or MOT_BAT_VOLT_MIN
596+
along with current volt-per-cell values to estimate the number of cells.
529597
530598
Args:
531599
fc_parameters: Dictionary of flight controller parameters
532600
533601
"""
534602
# Try to estimate cell count from available voltage parameters
535-
# Priority: MOT_BAT_VOLT_MAX > BATT_LOW_VOLT > BATT_CRT_VOLT
603+
# Priority: MOT_BAT_VOLT_MAX > BATT_LOW_VOLT > BATT_CRT_VOLT > BATT_ARM_VOLT > MOT_BAT_VOLT_MIN
536604
estimated_cells = None
537605

538606
if "MOT_BAT_VOLT_MAX" in fc_parameters:
@@ -550,24 +618,36 @@ def _estimate_battery_cell_count(self, fc_parameters: dict[str, float]) -> None:
550618
"BATT_CRT_VOLT", fc_parameters["BATT_CRT_VOLT"], "Volt per cell crit"
551619
)
552620

621+
# lower prio, because less often used
622+
if estimated_cells is None and "BATT_ARM_VOLT" in fc_parameters:
623+
estimated_cells = self._estimate_cells_from_voltage_param(
624+
"BATT_ARM_VOLT", fc_parameters["BATT_ARM_VOLT"], "Volt per cell arm"
625+
)
626+
627+
if estimated_cells is None and "MOT_BAT_VOLT_MIN" in fc_parameters:
628+
estimated_cells = self._estimate_cells_from_voltage_param(
629+
"MOT_BAT_VOLT_MIN", fc_parameters["MOT_BAT_VOLT_MIN"], "Volt per cell min"
630+
)
631+
553632
# If no estimation succeeded, all volt per cell values must be invalid
554633
if estimated_cells is None:
555634
logging_error(_("All volt per cell values are zero or invalid; cannot estimate battery cell count"))
556-
return
635+
return 0
557636

558637
# Validate and set the estimated cell count
559638
cell_path = ("Battery", "Specifications", "Number of cells")
560639
if cell_path in ComponentDataModelValidation.VALIDATION_RULES:
561640
_type, (min_cells, max_cells), _doc = ComponentDataModelValidation.VALIDATION_RULES[cell_path]
562641
if min_cells <= estimated_cells <= max_cells:
563642
self.set_component_value(cell_path, estimated_cells)
564-
else:
565-
logging_error(
566-
_("Estimated battery cell count %s is out of valid range (%d to %d)"),
567-
estimated_cells,
568-
min_cells,
569-
max_cells,
570-
)
643+
return estimated_cells
644+
logging_error(
645+
_("Estimated battery cell count %s is out of valid range (%d to %d)"),
646+
estimated_cells,
647+
min_cells,
648+
max_cells,
649+
)
650+
return 0
571651

572652
def _set_motor_poles_from_fc_parameters(self, fc_parameters: dict[str, float]) -> None:
573653
"""Process motor parameters and update the data model."""

0 commit comments

Comments
 (0)