diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 1dbb30151e..31d4e2368c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -268,9 +268,10 @@ bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_inde { uint16_t cell_min = 65535; uint16_t cell_max = 0; - for (int k = 0; k < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; k++) + + for (int k = 0; k <_params._batt_cells_amount ; k++)//MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN { - if ((_state.cell_voltages.cells[k]!=65535)) //TODO if valid cell valtage =0 + if (_state.cell_voltages.cells[k]!=65535) //TODO if valid cell valtage =0 { if (_state.cell_voltages.cells[k] < cell_min) { diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index eb4756c20d..5ae21589b0 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -169,6 +169,8 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Increment: 0.01 // @User: Standard AP_GROUPINFO("DROPOUT_V", 20, AP_BattMonitor_Params, _arming_dropout_voltage, 0), + + AP_GROUPINFO("CELL_TOTAL", 21, AP_BattMonitor_Params, _batt_cells_amount, 0), AP_GROUPEND diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index ae2e05c75b..18cca08943 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -57,4 +57,5 @@ public: AP_Int32 _arming_minimum_capacity; /// capacity level required to arm AP_Float _arming_minimum_voltage; /// voltage level required to arm AP_Float _arming_dropout_voltage; /// voltage level required to arm + AP_Int8 _batt_cells_amount; /// voltage level required to arm };