|
|
|
@ -169,9 +169,9 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
@@ -169,9 +169,9 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
|
|
|
|
|
bool fs_voltage_inversion = is_positive(_params._critical_voltage) && |
|
|
|
|
is_positive(_params._low_voltage) && |
|
|
|
|
(_params._low_voltage < _params._critical_voltage); |
|
|
|
|
uint8_t cell_min_index; |
|
|
|
|
uint8_t cell_max_index; |
|
|
|
|
uint16_t dropout_voltage; |
|
|
|
|
uint8_t cell_min_index=0; |
|
|
|
|
uint8_t cell_max_index=0; |
|
|
|
|
uint16_t dropout_voltage=0; |
|
|
|
|
bool fs_cells_dropout_voltage = cells_dropout_voltage_checks(cell_min_index,cell_max_index,dropout_voltage); |
|
|
|
|
|
|
|
|
|
bool result = update_check(buflen, buffer, low_voltage, "low voltage failsafe"); |
|
|
|
@ -182,9 +182,17 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
@@ -182,9 +182,17 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
|
|
|
|
|
result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity"); |
|
|
|
|
result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical > low"); |
|
|
|
|
result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical > low"); |
|
|
|
|
|
|
|
|
|
// char bat_message[60];
|
|
|
|
|
// snprintf(bat_message,60,"电池压差过大,%d号与%d号电芯,压差 %f V",cell_max_index,cell_min_index,dropout_voltage/1000.0);
|
|
|
|
|
if (dropout_voltage == 0xeeee) |
|
|
|
|
{ |
|
|
|
|
result = result && update_check(buflen, buffer, fs_cells_dropout_voltage, "电池电芯可能异常"); |
|
|
|
|
} |
|
|
|
|
else |
|
|
|
|
{ |
|
|
|
|
result = result && update_check(buflen, buffer, fs_cells_dropout_voltage, "电池压差过大"); |
|
|
|
|
} |
|
|
|
|
return result; |
|
|
|
|
} |
|
|
|
|
|
|
|
|
@ -262,7 +270,7 @@ bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_inde
@@ -262,7 +270,7 @@ bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_inde
|
|
|
|
|
uint16_t cell_max = 0; |
|
|
|
|
for (int k = 0; k < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; k++) |
|
|
|
|
{
|
|
|
|
|
if ((_state.cell_voltages.cells[k]!=65535) && _state.cell_voltages.cells[k] != 0) //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) |
|
|
|
|
{ |
|
|
|
@ -282,6 +290,10 @@ bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_inde
@@ -282,6 +290,10 @@ bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_inde
|
|
|
|
|
{ |
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
if(cell_max ==0&&cell_min==0){ |
|
|
|
|
dropout_voltage= 0xeeee; //
|
|
|
|
|
return true; |
|
|
|
|
} |
|
|
|
|
} |
|
|
|
|
return false; |
|
|
|
|
} |
|
|
|
|