From 9026ce065a0526c281450700f7a05c1faad91d17 Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Thu, 9 May 2024 17:08:01 +0100 Subject: [PATCH] AP_BattMonitor: ESC: add mask --- libraries/AP_BattMonitor/AP_BattMonitor.cpp | 32 +++++++++++++++++ .../AP_BattMonitor_DroneCAN.cpp | 2 +- .../AP_BattMonitor/AP_BattMonitor_ESC.cpp | 34 ++++++++++++++++++- libraries/AP_BattMonitor/AP_BattMonitor_ESC.h | 9 +++-- 4 files changed, 72 insertions(+), 5 deletions(-) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index d929b8182a06d..15f44291aa67b 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -66,6 +66,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: _ // @Path: AP_BattMonitor_INA2xx.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 @@ -87,6 +89,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 2_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 2_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[1], "2_", 42, AP_BattMonitor, backend_var_info[1]), #endif @@ -109,6 +113,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 3_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 3_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[2], "3_", 43, AP_BattMonitor, backend_var_info[2]), #endif @@ -131,6 +137,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 4_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 4_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[3], "4_", 44, AP_BattMonitor, backend_var_info[3]), #endif @@ -153,6 +161,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 5_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 5_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[4], "5_", 45, AP_BattMonitor, backend_var_info[4]), #endif @@ -175,6 +185,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 6_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 6_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[5], "6_", 46, AP_BattMonitor, backend_var_info[5]), #endif @@ -197,6 +209,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 7_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 7_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[6], "7_", 47, AP_BattMonitor, backend_var_info[6]), #endif @@ -219,6 +233,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 8_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 8_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[7], "8_", 48, AP_BattMonitor, backend_var_info[7]), #endif @@ -241,6 +257,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: 9_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: 9_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[8], "9_", 49, AP_BattMonitor, backend_var_info[8]), #endif @@ -263,6 +281,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: A_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: A_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[9], "A_", 50, AP_BattMonitor, backend_var_info[9]), #endif @@ -285,6 +305,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: B_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: B_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[10], "B_", 51, AP_BattMonitor, backend_var_info[10]), #endif @@ -307,6 +329,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: C_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: C_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[11], "C_", 52, AP_BattMonitor, backend_var_info[11]), #endif @@ -329,6 +353,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: D_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: D_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[12], "D_", 53, AP_BattMonitor, backend_var_info[12]), #endif @@ -351,6 +377,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: E_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: E_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[13], "E_", 54, AP_BattMonitor, backend_var_info[13]), #endif @@ -373,6 +401,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: F_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: F_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[14], "F_", 55, AP_BattMonitor, backend_var_info[14]), #endif @@ -395,6 +425,8 @@ const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { // @Path: AP_BattMonitor_Synthetic_Current.cpp // @Group: G_ // @Path: AP_BattMonitor_INA2xx.cpp + // @Group: G_ + // @Path: AP_BattMonitor_ESC.cpp AP_SUBGROUPVARPTR(drivers[15], "G_", 56, AP_BattMonitor, backend_var_info[15]), #endif diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp index 3c7d35fd9e5b9..0af65b951d0a2 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_DroneCAN.cpp @@ -26,7 +26,7 @@ const AP_Param::GroupInfo AP_BattMonitor_DroneCAN::var_info[] = { // @User: Advanced AP_GROUPINFO("CURR_MULT", 30, AP_BattMonitor_DroneCAN, _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 }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp index c633217f531ae..a1c309a6e60ef 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_ESC.cpp @@ -20,6 +20,32 @@ #include "AP_BattMonitor_ESC.h" +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) { } @@ -34,9 +60,15 @@ void AP_BattMonitor_ESC::read(void) 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