diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index dd307ecb19..e2a024e3f8 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -53,6 +53,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: _ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: _ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[0], "_", 41, AP_BattMonitor, backend_var_info[0]), #if AP_BATT_MONITOR_MAX_INSTANCES > 1 @@ -70,6 +72,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 2_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 2_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]), #endif @@ -88,6 +92,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 3_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 3_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]), #endif @@ -106,6 +112,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 4_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 4_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]), #endif @@ -124,6 +132,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 5_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 5_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]), #endif @@ -142,6 +152,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 6_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 6_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]), #endif @@ -160,6 +172,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 7_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 7_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]), #endif @@ -178,6 +192,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 8_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 8_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]), #endif @@ -196,6 +212,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_UAVCAN.cpp // @Group: 9_ // @Path: AP_BattMonitor_FuelLevel_Analog.cpp + // @Group: 9_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]), #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp index 713561ecb9..e4b8f71803 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp @@ -21,6 +21,32 @@ extern const AP_HAL::HAL &hal; +const AP_Param::GroupInfo AP_BattMonitor_ESC::var_info[] = { + + // Param indexes must be between 36 and 39 to avoid conflict with other battery monitor param tables loaded by pointer + + // @Param: ESC_MASK + // @DisplayName: ESC mask + // @Description: If 0 all connected ESCs will be used. If non-zero, only those selected in will be used. + // @Bitmask: 0: ESC 1, 1: ESC 2, 2: ESC 3, 3: ESC 4, 4: ESC 5, 5: ESC 6, 6: ESC 7, 7: ESC 8, 8: ESC 9, 9: ESC 10, 10: ESC 11, 11: ESC 12, 12: ESC 13, 13: ESC 14, 14: ESC 15, 15: ESC 16, 16: ESC 17, 17: ESC 18, 18: ESC 19, 19: ESC 20, 20: ESC 21, 21: ESC 22, 22: ESC 23, 23: ESC 24, 24: ESC 25, 25: ESC 26, 26: ESC 27, 27: ESC 28, 28: ESC 29, 29: ESC 30, 30: ESC 31, 31: ESC 32 + // @User: Standard + AP_GROUPINFO("ESC_MASK", 36, AP_BattMonitor_ESC, _mask, 0), + + // Param indexes must be between 36 and 39 to avoid conflict with other battery monitor param tables loaded by pointer + + AP_GROUPEND +}; + +// constructor. This incorporates initialisation as well. +AP_BattMonitor_ESC::AP_BattMonitor_ESC(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms): + AP_BattMonitor_Backend(mon, mon_state, params) +{ + AP_Param::setup_object_defaults(this, var_info); + _state.var_info = var_info; +}; + void AP_BattMonitor_ESC::init(void) { } @@ -32,12 +58,19 @@ void AP_BattMonitor_ESC::read(void) uint8_t voltage_escs = 0; // number of ESCs with valid voltage uint8_t temperature_escs = 0; // number of ESCs with valid temperature float voltage_sum = 0; + float voltage_min = 0; float current_sum = 0; float temperature_sum = 0; uint32_t highest_ms = 0; - _state.consumed_mah = delta_mah; + _state.consumed_mah = delta_mah; + const bool all_enabled = _mask == 0; for (uint8_t i=0; i voltage) { + voltage_min = voltage; + } voltage_sum += voltage; voltage_escs++; } @@ -69,7 +105,11 @@ void AP_BattMonitor_ESC::read(void) } if (voltage_escs > 0) { - _state.voltage = voltage_sum / voltage_escs; + if (uint32_t(_params._options.get()) & uint32_t(AP_BattMonitor_Params::Options::Minimum_Voltage)) { + _state.voltage = voltage_min; + } else { + _state.voltage = voltage_sum / voltage_escs; + } _state.healthy = true; } else { _state.voltage = 0; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h index 0c7113993b..e135d331da 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.h @@ -25,9 +25,7 @@ class AP_BattMonitor_ESC :public AP_BattMonitor_Backend { public: // constructor. This incorporates initialisation as well. - AP_BattMonitor_ESC(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms): - AP_BattMonitor_Backend(mon, mon_state, params) - {}; + AP_BattMonitor_ESC(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms); virtual ~AP_BattMonitor_ESC(void) {}; @@ -46,7 +44,12 @@ class AP_BattMonitor_ESC :public AP_BattMonitor_Backend // reset remaining percentage to given value virtual bool reset_remaining(float percentage) override; + static const struct AP_Param::GroupInfo var_info[]; + private: + + AP_Int32 _mask; + bool have_current; bool have_temperature; float delta_mah; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp index de38fd9f4d..6cefbf6bcb 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -143,7 +143,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { // @Param: OPTIONS // @DisplayName: Battery monitor options // @Description: This sets options to change the behaviour of the battery monitor - // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS + // @Bitmask: 0:Ignore DroneCAN SoC, 1:MPPT reports input voltage and current, 2:MPPT Powered off when disarmed, 3:MPPT Powered on when armed, 4:MPPT Powered off at boot, 5:MPPT Powered on at boot, 6:Send resistance compensated voltage to GCS, 7:Aggregate monitor reports minimum voltage instead of average, 8:Aggregate monitor reports maximum current instead of average, 9:Aggregate monitor reports maximum temperature instead of average, 10:Aggregate monitor reports minimum temperature instead of average // @User: Advanced AP_GROUPINFO("OPTIONS", 21, AP_BattMonitor_Params, _options, 0), diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h index 32ae410223..de2decc68d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -17,7 +17,7 @@ class AP_BattMonitor_Params { BattMonitor_LowVoltageSource_Raw = 0, BattMonitor_LowVoltageSource_SagCompensated = 1 }; - enum class Options : uint8_t { + enum class Options : uint16_t { Ignore_UAVCAN_SoC = (1U<<0), // Ignore UAVCAN State-of-Charge (charge %) supplied value from the device and use the internally calculated one MPPT_Use_Input_Value = (1U<<1), // MPPT reports voltage and current from Input (usually solar panel) instead of the output MPPT_Power_Off_At_Disarm = (1U<<2), // MPPT Disabled when vehicle is disarmed, if HW supports it @@ -25,6 +25,8 @@ class AP_BattMonitor_Params { MPPT_Power_Off_At_Boot = (1U<<4), // MPPT Disabled at startup (aka boot), if HW supports it MPPT_Power_On_At_Boot = (1U<<5), // MPPT Enabled at startup (aka boot), if HW supports it. If Power_Off_at_Boot is also set, the behavior is Power_Off_at_Boot GCS_Resting_Voltage = (1U<<6), // send resistance resting voltage to GCS + // AllowSplitAuxInfo = (1U<<7), // allow different node to provide aux info for DroneCAN (Reserved for ArduPilot 4.5+) + Minimum_Voltage = (1U<<8), // aggregate monitors report minimum voltage rather than average }; BattMonitor_LowVoltage_Source failsafe_voltage_source(void) const { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp index 7ce61484f8..fd4ebbda0b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Sum.cpp @@ -45,6 +45,7 @@ void AP_BattMonitor_Sum::read() { float voltage_sum = 0; + float voltage_min = 0; uint8_t voltage_count = 0; float current_sum = 0; uint8_t current_count = 0; @@ -65,6 +66,9 @@ AP_BattMonitor_Sum::read() if (!_mon.healthy(i)) { continue; } + if (voltage_count == 0 || _mon.voltage(i) < voltage_min) { + voltage_min = _mon.voltage(i); + } voltage_sum += _mon.voltage(i); voltage_count++; float current; @@ -77,7 +81,11 @@ AP_BattMonitor_Sum::read() const uint32_t dt_us = tnow_us - _state.last_time_micros; if (voltage_count > 0) { - _state.voltage = voltage_sum / voltage_count; + if (uint32_t(_params._options.get()) & uint32_t(AP_BattMonitor_Params::Options::Minimum_Voltage)) { + _state.voltage = voltage_min; + } else { + _state.voltage = voltage_sum / voltage_count; + } } if (current_count > 0) { _state.current_amps = current_sum; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp index 3544c87c3f..9a5571acc5 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_UAVCAN.cpp @@ -30,7 +30,7 @@ const AP_Param::GroupInfo AP_BattMonitor_UAVCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_UAVCAN, _curr_mult, 1.0), - // Param indexes must be between 30 and 39 to avoid conflict with other battery monitor param tables loaded by pointer + // Param indexes must be between 30 and 35 to avoid conflict with other battery monitor param tables loaded by pointer AP_GROUPEND };