diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.cpp b/libraries/AP_BattMonitor/AP_BattMonitor.cpp index f0e2a5b2a8..bdbc640036 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor.cpp @@ -7,157 +7,16 @@ extern const AP_HAL::HAL& hal; const AP_Param::GroupInfo AP_BattMonitor::var_info[] = { - // @Param: _MONITOR - // @DisplayName: Battery monitoring - // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell - // @User: Standard - AP_GROUPINFO("_MONITOR", 0, AP_BattMonitor, _monitoring[0], BattMonitor_TYPE_NONE), + // 0 - 18, 20- 22 used by old parameter indexes - // @Param: _VOLT_PIN - // @DisplayName: Battery Voltage sensing pin - // @Description: Setting this to 0 ~ 13 will enable battery voltage sensing on pins A0 ~ A13. On the PX4-v1 it should be set to 100. On the Pixhawk, Pixracer and NAVIO boards it should be set to 2, Pixhawk2 Power2 is 13. - // @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 13:Pixhawk2_PM2, 100:PX4-v1 - // @User: Standard - AP_GROUPINFO("_VOLT_PIN", 1, AP_BattMonitor, _volt_pin[0], AP_BATT_VOLT_PIN), + // @Group: _ + // @Path: AP_BattMonitor_Params.cpp + AP_SUBGROUPINFO_FLAGS(_params[0], "_", 23, AP_BattMonitor, AP_BattMonitor_Params, AP_PARAM_FLAG_IGNORE_ENABLE), - // @Param: _CURR_PIN - // @DisplayName: Battery Current sensing pin - // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. On the PX4-v1 it should be set to 101. On the Pixhawk, Pixracer and NAVIO boards it should be set to 3, Pixhawk2 Power2 is 14. - // @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 14:Pixhawk2_PM2, 101:PX4-v1 - // @User: Standard - AP_GROUPINFO("_CURR_PIN", 2, AP_BattMonitor, _curr_pin[0], AP_BATT_CURR_PIN), + // @Group: 2_ + // @Path: AP_BattMonitor_Params.cpp + AP_SUBGROUPINFO(_params[1], "2_", 24, AP_BattMonitor, AP_BattMonitor_Params), - // @Param: _VOLT_MULT - // @DisplayName: Voltage Multiplier - // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX4 using the PX4IO power supply this should be set to 1. - // @User: Advanced - AP_GROUPINFO("_VOLT_MULT", 3, AP_BattMonitor, _volt_multiplier[0], AP_BATT_VOLTDIVIDER_DEFAULT), - - // @Param: _AMP_PERVOLT - // @DisplayName: Amps per volt - // @Description: Number of amps that a 1V reading on the current sensor corresponds to. On the APM2 or Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. - // @Units: A/V - // @User: Standard - AP_GROUPINFO("_AMP_PERVOLT", 4, AP_BattMonitor, _curr_amp_per_volt[0], AP_BATT_CURR_AMP_PERVOLT_DEFAULT), - - // @Param: _AMP_OFFSET - // @DisplayName: AMP offset - // @Description: Voltage offset at zero current on current sensor - // @Units: V - // @User: Standard - AP_GROUPINFO("_AMP_OFFSET", 5, AP_BattMonitor, _curr_amp_offset[0], 0), - - // @Param: _CAPACITY - // @DisplayName: Battery capacity - // @Description: Capacity of the battery in mAh when full - // @Units: mA.h - // @Increment: 50 - // @User: Standard - AP_GROUPINFO("_CAPACITY", 6, AP_BattMonitor, _pack_capacity[0], AP_BATT_CAPACITY_DEFAULT), - - // 7 & 8 were used for VOLT2_PIN and VOLT2_MULT - -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) - // @Param: _WATT_MAX - // @DisplayName: Maximum allowed power (Watts) - // @Description: If battery wattage (voltage * current) exceeds this value then the system will reduce max throttle (THR_MAX, TKOFF_THR_MAX and THR_MIN for reverse thrust) to satisfy this limit. This helps limit high current to low C rated batteries regardless of battery voltage. The max throttle will slowly grow back to THR_MAX (or TKOFF_THR_MAX ) and THR_MIN if demanding the current max and under the watt max. Use 0 to disable. - // @Units: W - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("_WATT_MAX", 9, AP_BattMonitor, _watt_max[0], AP_BATT_MAX_WATT_DEFAULT), -#endif - - // @Param: _SERIAL_NUM - // @DisplayName: Battery serial number - // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1 - // @User: Advanced - AP_GROUPINFO("_SERIAL_NUM", 10, AP_BattMonitor, _serial_numbers[0], AP_BATT_SERIAL_NUMBER_DEFAULT), - -#if AP_BATT_MONITOR_MAX_INSTANCES > 1 - // @Param: 2_MONITOR - // @DisplayName: Battery monitoring - // @Description: Controls enabling monitoring of the battery's voltage and current - // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell - // @User: Standard - AP_GROUPINFO("2_MONITOR", 11, AP_BattMonitor, _monitoring[1], BattMonitor_TYPE_NONE), - - // @Param: 2_VOLT_PIN - // @DisplayName: Battery Voltage sensing pin - // @Description: Setting this to 0 ~ 13 will enable battery voltage sensing on pins A0 ~ A13. On the PX4-v1 it should be set to 100. On the Pixhawk, Pixracer and NAVIO boards it should be set to 2, Pixhawk2 Power2 is 13. - // @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 13:Pixhawk2_PM2, 100:PX4-v1 - // @User: Standard - AP_GROUPINFO("2_VOLT_PIN", 12, AP_BattMonitor, _volt_pin[1], AP_BATT_VOLT_PIN), - - // @Param: 2_CURR_PIN - // @DisplayName: Battery Current sensing pin - // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. On the PX4-v1 it should be set to 101. On the Pixhawk, Pixracer and NAVIO boards it should be set to 3, Pixhawk2 Power2 is 14. - // @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 14:Pixhawk2_PM2, 101:PX4-v1 - // @User: Standard - AP_GROUPINFO("2_CURR_PIN", 13, AP_BattMonitor, _curr_pin[1], AP_BATT_CURR_PIN), - - // @Param: 2_VOLT_MULT - // @DisplayName: Voltage Multiplier - // @Description: Used to convert the voltage of the voltage sensing pin (BATT2_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX4 using the PX4IO power supply this should be set to 1. - // @User: Advanced - AP_GROUPINFO("2_VOLT_MULT", 14, AP_BattMonitor, _volt_multiplier[1], AP_BATT_VOLTDIVIDER_DEFAULT), - - // @Param: 2_AMP_PERVOL - // @DisplayName: Amps per volt - // @Description: Number of amps that a 1V reading on the current sensor corresponds to. On the APM2 or Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. - // @Units: A/V - // @User: Standard - AP_GROUPINFO("2_AMP_PERVOL", 15, AP_BattMonitor, _curr_amp_per_volt[1], AP_BATT_CURR_AMP_PERVOLT_DEFAULT), - - // @Param: 2_AMP_OFFSET - // @DisplayName: AMP offset - // @Description: Voltage offset at zero current on current sensor - // @Units: V - // @User: Standard - AP_GROUPINFO("2_AMP_OFFSET", 16, AP_BattMonitor, _curr_amp_offset[1], 0), - - // @Param: 2_CAPACITY - // @DisplayName: Battery capacity - // @Description: Capacity of the battery in mAh when full - // @Units: mA.h - // @Increment: 50 - // @User: Standard - AP_GROUPINFO("2_CAPACITY", 17, AP_BattMonitor, _pack_capacity[1], AP_BATT_CAPACITY_DEFAULT), - - -#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) - // @Param: 2_WATT_MAX - // @DisplayName: Maximum allowed current - // @Description: If battery wattage (voltage * current) exceeds this value then the system will reduce max throttle (THR_MAX, TKOFF_THR_MAX and THR_MIN for reverse thrust) to satisfy this limit. This helps limit high current to low C rated batteries regardless of battery voltage. The max throttle will slowly grow back to THR_MAX (or TKOFF_THR_MAX ) and THR_MIN if demanding the current max and under the watt max. Use 0 to disable. - // @Units: A - // @Increment: 1 - // @User: Advanced - AP_GROUPINFO("2_WATT_MAX", 18, AP_BattMonitor, _watt_max[1], AP_BATT_MAX_WATT_DEFAULT), -#endif - - // @Param: 2_SERIAL_NUM - // @DisplayName: Battery serial number - // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1 - // @User: Advanced - AP_GROUPINFO("2_SERIAL_NUM", 20, AP_BattMonitor, _serial_numbers[1], AP_BATT_SERIAL_NUMBER_DEFAULT), - -#endif // AP_BATT_MONITOR_MAX_INSTANCES > 1 - - // @Param: _LOW_TIMER - // @DisplayName: Low voltage timeout - // @Description: This is the timeout in seconds before a low voltage event will be triggered. For aircraft with low C batteries it may be necessary to raise this in order to cope with low voltage on long takeoffs. A value of zero disables low voltage errors. - // @Units: s - // @Increment: 1 - // @Range: 0 120 - // @User: Advanced - AP_GROUPINFO("_LOW_TIMER", 21, AP_BattMonitor, _low_voltage_timeout, AP_BATT_LOW_VOLT_TIMEOUT_DEFAULT), - - // @Param: _LOW_TYPE - // @DisplayName: Low voltage type - // @Description: Voltage type used for detection of low voltage event - // @Values: 0:Raw Voltage, 1:Sag Compensated Voltage - // @User: Advanced - AP_GROUPINFO("_LOW_TYPE", 22, AP_BattMonitor, _low_voltage_source, BattMonitor_LowVoltageSource_Raw), AP_GROUPEND }; @@ -181,9 +40,11 @@ AP_BattMonitor::init() return; } + convert_params(); + #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO // force monitor for bebop - _monitoring[0] = BattMonitor_TYPE_BEBOP; + _params[0]._type.set(AP_BattMonitor_Params::BattMonitor_TYPE_BEBOP); #endif // create each instance @@ -191,35 +52,33 @@ AP_BattMonitor::init() // clear out the cell voltages memset(&state[instance].cell_voltages, 0xFF, sizeof(cells)); - uint8_t monitor_type = _monitoring[instance]; - switch (monitor_type) { - case BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY: - case BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT: - state[instance].instance = instance; - drivers[instance] = new AP_BattMonitor_Analog(*this, state[instance]); + switch (get_type(instance)) { + case AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY: + case AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT: + drivers[instance] = new AP_BattMonitor_Analog(*this, state[instance], _params[instance]); _num_instances++; break; - case BattMonitor_TYPE_SOLO: - state[instance].instance = instance; - drivers[instance] = new AP_BattMonitor_SMBus_Solo(*this, state[instance], + case AP_BattMonitor_Params::BattMonitor_TYPE_SOLO: + drivers[instance] = new AP_BattMonitor_SMBus_Solo(*this, state[instance], _params[instance], hal.i2c_mgr->get_device(AP_BATTMONITOR_SMBUS_BUS_INTERNAL, AP_BATTMONITOR_SMBUS_I2C_ADDR, 100000, true, 20)); _num_instances++; break; - case BattMonitor_TYPE_MAXELL: - state[instance].instance = instance; - drivers[instance] = new AP_BattMonitor_SMBus_Maxell(*this, state[instance], + case AP_BattMonitor_Params::BattMonitor_TYPE_MAXELL: + drivers[instance] = new AP_BattMonitor_SMBus_Maxell(*this, state[instance], _params[instance], hal.i2c_mgr->get_device(AP_BATTMONITOR_SMBUS_BUS_EXTERNAL, AP_BATTMONITOR_SMBUS_I2C_ADDR, 100000, true, 20)); _num_instances++; break; - case BattMonitor_TYPE_BEBOP: + case AP_BattMonitor_Params::BattMonitor_TYPE_BEBOP: #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - state[instance].instance = instance; - drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance]); + drivers[instance] = new AP_BattMonitor_Bebop(*this, state[instance], _params[instance]); _num_instances++; #endif break; + case AP_BattMonitor_Params::BattMonitor_TYPE_NONE: + default: + break; } // call init function for each backend @@ -229,12 +88,84 @@ AP_BattMonitor::init() } } +void AP_BattMonitor::convert_params(void) { + if (_params[0]._type.configured_in_storage()) { + // _params[0]._type will always be configured in storage after conversion is done the first time + return; + } + + #define SECOND_BATT_CONVERT_MASK 0x80 + const struct ConversionTable { + uint8_t old_element; + uint8_t new_index; // upper bit used to indicate if its the first or second instance + }conversionTable[22] = { + { 0, 0 }, // _MONITOR + { 1, 1 }, // _VOLT_PIN + { 2, 2 }, // _CURR_PIN + { 3, 3 }, // _VOLT_MULT + { 4, 4 }, // _AMP_PERVOLT + { 5, 5 }, // _AMP_OFFSET + { 6, 6 }, // _CAPACITY + { 9, 7 }, // _WATT_MAX + {10, 8 }, // _SERIAL_NUM + {11, (SECOND_BATT_CONVERT_MASK | 0)}, // 2_MONITOR + {12, (SECOND_BATT_CONVERT_MASK | 1)}, // 2_VOLT_PIN + {13, (SECOND_BATT_CONVERT_MASK | 2)}, // 2_CURR_PIN + {14, (SECOND_BATT_CONVERT_MASK | 3)}, // 2_VOLT_MULT + {15, (SECOND_BATT_CONVERT_MASK | 4)}, // 2_AMP_PERVOLT + {16, (SECOND_BATT_CONVERT_MASK | 5)}, // 2_AMP_OFFSET + {17, (SECOND_BATT_CONVERT_MASK | 6)}, // 2_CAPACITY + {18, (SECOND_BATT_CONVERT_MASK | 7)}, // 2_WATT_MAX + {20, (SECOND_BATT_CONVERT_MASK | 8)}, // 2_SERIAL_NUM + {21, 9 }, // _LOW_TIMER + {22, 10 }, // _LOW_TYPE + {21, (SECOND_BATT_CONVERT_MASK | 9)}, // 2_LOW_TIMER + {22, (SECOND_BATT_CONVERT_MASK |10)}, // 2_LOW_TYPE + }; + + + char param_name[17]; + AP_Param::ConversionInfo info; + info.new_name = param_name; + +#if APM_BUILD_TYPE(APM_BUILD_ArduPlane) + info.old_key = 166; +#elif APM_BUILD_TYPE(APM_BUILD_ArduCopter) + info.old_key = 36; +#elif APM_BUILD_TYPE(APM_BUILD_ArduSub) + info.old_key = 33; +#elif APM_BUILD_TYPE(APM_BUILD_APMrover2) + info.old_key = 145; +#else + _params[0]._type.save(true); + return; // no conversion is supported on this platform +#endif + + for (uint8_t i = 0; i < ARRAY_SIZE(conversionTable); i++) { + uint8_t param_instance = conversionTable[i].new_index >> 7; + uint8_t destination_index = 0x7F & conversionTable[i].new_index; + + info.old_group_element = conversionTable[i].old_element; + info.type = (ap_var_type)AP_BattMonitor_Params::var_info[destination_index].type; + if (param_instance) { + hal.util->snprintf(param_name, 17, "BATT2_%s", AP_BattMonitor_Params::var_info[destination_index].name); + } else { + hal.util->snprintf(param_name, 17, "BATT_%s", AP_BattMonitor_Params::var_info[destination_index].name); + } + + AP_Param::convert_old_parameter(&info, 1.0f, 0); + } + + // force _params[0]._type into storage to flag that conversion has been done + _params[0]._type.save(true); +} + // read - read the voltage and current for all instances void AP_BattMonitor::read() { for (uint8_t i=0; i<_num_instances; i++) { - if (drivers[i] != nullptr && _monitoring[i] != BattMonitor_TYPE_NONE) { + if (drivers[i] != nullptr && _params[i].type() != AP_BattMonitor_Params::BattMonitor_TYPE_NONE) { drivers[i]->read(); drivers[i]->update_resistance_estimate(); } @@ -243,13 +174,13 @@ AP_BattMonitor::read() // healthy - returns true if monitor is functioning bool AP_BattMonitor::healthy(uint8_t instance) const { - return instance < _num_instances && _BattMonitor_STATE(instance).healthy; + return instance < _num_instances && state[instance].healthy; } /// has_current - returns true if battery monitor instance provides current info bool AP_BattMonitor::has_current(uint8_t instance) const { - if (instance < _num_instances && drivers[instance] != nullptr && _monitoring[instance] != BattMonitor_TYPE_NONE) { + if (instance < _num_instances && drivers[instance] != nullptr && _params[instance].type() != AP_BattMonitor_Params::BattMonitor_TYPE_NONE) { return drivers[instance]->has_current(); } @@ -261,7 +192,7 @@ bool AP_BattMonitor::has_current(uint8_t instance) const float AP_BattMonitor::voltage(uint8_t instance) const { if (instance < _num_instances) { - return _BattMonitor_STATE(instance).voltage; + return state[instance].voltage; } else { return 0.0f; } @@ -273,7 +204,7 @@ float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const { if (instance < _num_instances) { // resting voltage should always be greater than or equal to the raw voltage - return MAX(_BattMonitor_STATE(instance).voltage, _BattMonitor_STATE(instance).voltage_resting_estimate); + return MAX(state[instance].voltage, state[instance].voltage_resting_estimate); } else { return 0.0f; } @@ -282,7 +213,7 @@ float AP_BattMonitor::voltage_resting_estimate(uint8_t instance) const /// current_amps - returns the instantaneous current draw in amperes float AP_BattMonitor::current_amps(uint8_t instance) const { if (instance < _num_instances) { - return _BattMonitor_STATE(instance).current_amps; + return state[instance].current_amps; } else { return 0.0f; } @@ -291,7 +222,7 @@ float AP_BattMonitor::current_amps(uint8_t instance) const { /// current_total_mah - returns total current drawn since start-up in amp-hours float AP_BattMonitor::current_total_mah(uint8_t instance) const { if (instance < _num_instances) { - return _BattMonitor_STATE(instance).current_total_mah; + return state[instance].current_total_mah; } else { return 0.0f; } @@ -308,14 +239,14 @@ uint8_t AP_BattMonitor::capacity_remaining_pct(uint8_t instance) const } /// pack_capacity_mah - returns the capacity of the battery pack in mAh when the pack is full - int32_t AP_BattMonitor::pack_capacity_mah(uint8_t instance) const - { - if (instance < AP_BATT_MONITOR_MAX_INSTANCES) { - return _pack_capacity[instance]; +int32_t AP_BattMonitor::pack_capacity_mah(uint8_t instance) const +{ + if (instance < AP_BATT_MONITOR_MAX_INSTANCES) { + return _params[instance]._pack_capacity; } else { return 0; } - } +} /// exhausted - returns true if the voltage remains below the low_voltage for 10 seconds or remaining capacity falls below min_capacity_mah bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_capacity_mah) @@ -327,12 +258,12 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca // use voltage or sag compensated voltage float voltage_used; - switch ((enum BattMonitor_LowVoltage_Source)_low_voltage_source.get()) { - case BattMonitor_LowVoltageSource_Raw: + switch (_params[instance].failsafe_voltage_source()) { + case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_Raw: default: voltage_used = state[instance].voltage; break; - case BattMonitor_LowVoltageSource_SagCompensated: + case AP_BattMonitor_Params::BattMonitor_LowVoltageSource_SagCompensated: voltage_used = voltage_resting_estimate(instance); break; } @@ -342,7 +273,8 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca // this is the first time our voltage has dropped below minimum so start timer if (state[instance].low_voltage_start_ms == 0) { state[instance].low_voltage_start_ms = AP_HAL::millis(); - } else if (_low_voltage_timeout > 0 && AP_HAL::millis() - state[instance].low_voltage_start_ms > uint32_t(_low_voltage_timeout.get())*1000U) { + } else if (_params[instance]._low_voltage_timeout > 0 && + AP_HAL::millis() - state[instance].low_voltage_start_ms > uint32_t(_params[instance]._low_voltage_timeout)*1000U) { return true; } } else { @@ -351,7 +283,8 @@ bool AP_BattMonitor::exhausted(uint8_t instance, float low_voltage, float min_ca } // check capacity if current monitoring is enabled - if (has_current(instance) && (min_capacity_mah > 0) && (_pack_capacity[instance] - state[instance].current_total_mah < min_capacity_mah)) { + if (has_current(instance) && (min_capacity_mah > 0) && + (_params[instance]._pack_capacity - state[instance].current_total_mah < min_capacity_mah)) { return true; } @@ -372,9 +305,9 @@ bool AP_BattMonitor::overpower_detected() const bool AP_BattMonitor::overpower_detected(uint8_t instance) const { #if APM_BUILD_TYPE(APM_BUILD_ArduPlane) - if (instance < _num_instances && _watt_max[instance] > 0) { - float power = _BattMonitor_STATE(instance).current_amps * _BattMonitor_STATE(instance).voltage; - return _BattMonitor_STATE(instance).healthy && (power > _watt_max[instance]); + if (instance < _num_instances && _params[instance]._watt_max > 0) { + float power = state[instance].current_amps * state[instance].voltage; + return state[instance].healthy && (power > _params[instance]._watt_max); } return false; #else diff --git a/libraries/AP_BattMonitor/AP_BattMonitor.h b/libraries/AP_BattMonitor/AP_BattMonitor.h index b43b4dc969..cce5603b16 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor.h @@ -4,6 +4,7 @@ #include #include #include +#include "AP_BattMonitor_Params.h" // maximum number of battery monitors #define AP_BATT_MONITOR_MAX_INSTANCES 2 @@ -11,9 +12,6 @@ // first monitor is always the primary monitor #define AP_BATT_PRIMARY_INSTANCE 0 -#define AP_BATT_CAPACITY_DEFAULT 3300 -#define AP_BATT_LOW_VOLT_TIMEOUT_DEFAULT 10 // low voltage of 10 seconds will cause battery_exhausted to return true -#define AP_BATT_MAX_WATT_DEFAULT 0 #define AP_BATT_SERIAL_NUMBER_DEFAULT -1 #define AP_BATT_MONITOR_TIMEOUT 5000 @@ -43,40 +41,23 @@ public: AP_BattMonitor(const AP_BattMonitor &other) = delete; AP_BattMonitor &operator=(const AP_BattMonitor&) = delete; - // Battery monitor driver types - enum BattMonitor_Type { - BattMonitor_TYPE_NONE = 0, - BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY = 3, - BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4, - BattMonitor_TYPE_SOLO = 5, - BattMonitor_TYPE_BEBOP = 6, - BattMonitor_TYPE_MAXELL = 7 - }; - - // low voltage sources (used for BATT_LOW_TYPE parameter) - enum BattMonitor_LowVoltage_Source { - BattMonitor_LowVoltageSource_Raw = 0, - BattMonitor_LowVoltageSource_SagCompensated = 1 - }; - struct cells { uint16_t cells[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; }; // The BattMonitor_State structure is filled in by the backend driver struct BattMonitor_State { - uint8_t instance; // the instance number of this monitor - bool healthy; // battery monitor is communicating correctly + cells cell_voltages; // battery cell voltages in millivolts, 10 cells matches the MAVLink spec float voltage; // voltage in volts float current_amps; // current in amperes float current_total_mah; // total current draw since start-up uint32_t last_time_micros; // time when voltage and current was last read uint32_t low_voltage_start_ms; // time when voltage dropped below the minimum - cells cell_voltages; // battery cell voltages in millivolts, 10 cells matches the MAVLink spec float temperature; // battery temperature in celsius uint32_t temperature_time; // timestamp of the last recieved temperature message float voltage_resting_estimate; // voltage with sag removed based on current and resistance estimate float resistance; // resistance calculated by comparing resting voltage vs in flight voltage + bool healthy; // battery monitor is communicating correctly }; // Return the number of battery monitor instances @@ -88,8 +69,6 @@ public: /// Read the battery voltage and current for all batteries. Should be called at 10hz void read(); -#define _BattMonitor_STATE(instance) state[instance] - // healthy - returns true if monitor is functioning bool healthy(uint8_t instance) const; bool healthy() const { return healthy(AP_BATT_PRIMARY_INSTANCE); } @@ -128,11 +107,11 @@ public: bool exhausted(float low_voltage, float min_capacity_mah) { return exhausted(AP_BATT_PRIMARY_INSTANCE, low_voltage, min_capacity_mah); } /// get_type - returns battery monitor type - enum BattMonitor_Type get_type() { return get_type(AP_BATT_PRIMARY_INSTANCE); } - enum BattMonitor_Type get_type(uint8_t instance) { return (enum BattMonitor_Type)_monitoring[instance].get(); } + enum AP_BattMonitor_Params::BattMonitor_Type get_type() { return get_type(AP_BATT_PRIMARY_INSTANCE); } + enum AP_BattMonitor_Params::BattMonitor_Type get_type(uint8_t instance) { return _params[instance].type(); } /// set_monitoring - sets the monitor type (used for example sketch only) - void set_monitoring(uint8_t instance, uint8_t mon) { _monitoring[instance].set(mon); } + void set_monitoring(uint8_t instance, uint8_t mon) { _params[instance]._type.set(mon); } /// true when (voltage * current) > watt_max bool overpower_detected() const; @@ -157,20 +136,13 @@ public: protected: /// parameters - AP_Int8 _monitoring[AP_BATT_MONITOR_MAX_INSTANCES]; /// 0=disabled, 3=voltage only, 4=voltage and current - AP_Int8 _volt_pin[AP_BATT_MONITOR_MAX_INSTANCES]; /// board pin used to measure battery voltage - AP_Int8 _curr_pin[AP_BATT_MONITOR_MAX_INSTANCES]; /// board pin used to measure battery current - AP_Float _volt_multiplier[AP_BATT_MONITOR_MAX_INSTANCES]; /// voltage on volt pin multiplied by this to calculate battery voltage - AP_Float _curr_amp_per_volt[AP_BATT_MONITOR_MAX_INSTANCES]; /// voltage on current pin multiplied by this to calculate current in amps - AP_Float _curr_amp_offset[AP_BATT_MONITOR_MAX_INSTANCES]; /// offset voltage that is subtracted from current pin before conversion to amps - AP_Int32 _pack_capacity[AP_BATT_MONITOR_MAX_INSTANCES]; /// battery pack capacity less reserve in mAh - AP_Int16 _watt_max[AP_BATT_MONITOR_MAX_INSTANCES]; /// max battery power allowed. Reduce max throttle to reduce current to satisfy this limit - AP_Int32 _serial_numbers[AP_BATT_MONITOR_MAX_INSTANCES]; /// battery serial number, automatically filled in on SMBus batteries - AP_Int8 _low_voltage_timeout; /// timeout in seconds before a low voltage event will be triggered - AP_Int8 _low_voltage_source; /// voltage type used for detection of low voltage event + AP_BattMonitor_Params _params[AP_BATT_MONITOR_MAX_INSTANCES]; private: BattMonitor_State state[AP_BATT_MONITOR_MAX_INSTANCES]; AP_BattMonitor_Backend *drivers[AP_BATT_MONITOR_MAX_INSTANCES]; uint8_t _num_instances; /// number of monitors + + void convert_params(void); + }; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp index f29c3f8db6..29959cec30 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.cpp @@ -7,11 +7,13 @@ extern const AP_HAL::HAL& hal; /// Constructor -AP_BattMonitor_Analog::AP_BattMonitor_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state) : - AP_BattMonitor_Backend(mon, mon_state) +AP_BattMonitor_Analog::AP_BattMonitor_Analog(AP_BattMonitor &mon, + AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms) : + AP_BattMonitor_Backend(mon, mon_state, params) { - _volt_pin_analog_source = hal.analogin->channel(mon._volt_pin[_state.instance]); - _curr_pin_analog_source = hal.analogin->channel(mon._curr_pin[_state.instance]); + _volt_pin_analog_source = hal.analogin->channel(_params._volt_pin); + _curr_pin_analog_source = hal.analogin->channel(_params._curr_pin); // always healthy _state.healthy = true; @@ -22,10 +24,10 @@ void AP_BattMonitor_Analog::read() { // this copes with changing the pin at runtime - _volt_pin_analog_source->set_pin(_mon._volt_pin[_state.instance]); + _volt_pin_analog_source->set_pin(_params._volt_pin); // get voltage - _state.voltage = _volt_pin_analog_source->voltage_average() * _mon._volt_multiplier[_state.instance]; + _state.voltage = _volt_pin_analog_source->voltage_average() * _params._volt_multiplier; // read current if (has_current()) { @@ -34,10 +36,10 @@ AP_BattMonitor_Analog::read() float dt = tnow - _state.last_time_micros; // this copes with changing the pin at runtime - _curr_pin_analog_source->set_pin(_mon._curr_pin[_state.instance]); + _curr_pin_analog_source->set_pin(_params._curr_pin); // read current - _state.current_amps = (_curr_pin_analog_source->voltage_average()-_mon._curr_amp_offset[_state.instance])*_mon._curr_amp_per_volt[_state.instance]; + _state.current_amps = (_curr_pin_analog_source->voltage_average()-_params._curr_amp_offset)*_params._curr_amp_per_volt; // update total current drawn since startup if (_state.last_time_micros != 0 && dt < 2000000.0f) { @@ -53,5 +55,5 @@ AP_BattMonitor_Analog::read() /// return true if battery provides current info bool AP_BattMonitor_Analog::has_current() const { - return (_mon.get_type(_state.instance) == AP_BattMonitor::BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT); + return (_params.type() == AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT); } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h index 295ca3f8ae..b44efc4f79 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Analog.h @@ -104,7 +104,7 @@ class AP_BattMonitor_Analog : public AP_BattMonitor_Backend public: /// Constructor - AP_BattMonitor_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state); + AP_BattMonitor_Analog(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms); /// Read the battery voltage and current. Should be called at 10hz void read(); diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp index 7686992386..24f1f1484d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp @@ -22,29 +22,25 @@ base class constructor. This incorporates initialisation as well. */ -AP_BattMonitor_Backend::AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state) : +AP_BattMonitor_Backend::AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms) : _mon(mon), - _state(mon_state) + _state(mon_state), + _params(params) { } /// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100) uint8_t AP_BattMonitor_Backend::capacity_remaining_pct() const { - float mah_remaining = _mon._pack_capacity[_state.instance] - _state.current_total_mah; - if ( _mon._pack_capacity[_state.instance] > 10 ) { // a very very small battery - return (100 * (mah_remaining) / _mon._pack_capacity[_state.instance]); + float mah_remaining = _params._pack_capacity - _state.current_total_mah; + if ( _params._pack_capacity > 10 ) { // a very very small battery + return (100 * (mah_remaining) / _params._pack_capacity); } else { return 0; } } -/// get capacity for this instance -int32_t AP_BattMonitor_Backend::get_capacity() const -{ - return _mon.pack_capacity_mah(_state.instance); -} - // update battery resistance estimate // faster rates of change of the current and voltage readings cause faster updates to the resistance estimate // the battery resistance is calculated by comparing the latest current and voltage readings to a low-pass filtered current and voltage diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h index 29c3d468e8..3aa99b6b4d 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Backend.h @@ -22,7 +22,7 @@ class AP_BattMonitor_Backend { public: // constructor. This incorporates initialisation as well. - AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state); + AP_BattMonitor_Backend(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms); // we declare a virtual destructor so that BattMonitor driver can // override with a custom destructor if need be @@ -43,15 +43,13 @@ public: /// capacity_remaining_pct - returns the % battery capacity remaining (0 ~ 100) uint8_t capacity_remaining_pct() const; - /// get capacity for this instance - int32_t get_capacity() const; - // update battery resistance estimate and voltage_resting_estimate void update_resistance_estimate(); protected: AP_BattMonitor &_mon; // reference to front-end AP_BattMonitor::BattMonitor_State &_state; // reference to this instances state (held in the front-end) + AP_BattMonitor_Params &_params; // reference to this instances parameters (held in the front-end) private: // resistance estimate diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp index 08d3cf8b47..ee224c7e5c 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.cpp @@ -208,7 +208,7 @@ void AP_BattMonitor_Bebop::read(void) /* compute remaining battery percent and get battery capacity */ remaining = _compute_battery_percentage(vbat); - capacity = (float) get_capacity(); + capacity = (float) _params._pack_capacity; /* fillup battery state */ _state.voltage = vbat; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h index 531409a22b..66ae455a45 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Bebop.h @@ -22,8 +22,8 @@ class AP_BattMonitor_Bebop :public AP_BattMonitor_Backend { public: // constructor. This incorporates initialisation as well. - AP_BattMonitor_Bebop(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state): - AP_BattMonitor_Backend(mon, mon_state), + AP_BattMonitor_Bebop(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params ¶ms): + AP_BattMonitor_Backend(mon, mon_state, params), _prev_vbat_raw(0.0f), _prev_vbat(0.0f), _battery_voltage_max(0.0f) diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp new file mode 100644 index 0000000000..37060b1088 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp @@ -0,0 +1,92 @@ +#include +#include +#include "AP_BattMonitor_Params.h" +#include "AP_BattMonitor_Analog.h" + +const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = { + // @Param: MONITOR + // @DisplayName: Battery monitoring + // @Description: Controls enabling monitoring of the battery's voltage and current + // @Values: 0:Disabled,3:Analog Voltage Only,4:Analog Voltage and Current,5:Solo,6:Bebop,7:SMBus-Maxell + // @User: Standard + AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, BattMonitor_TYPE_NONE, AP_PARAM_FLAG_ENABLE), + + // @Param: VOLT_PIN + // @DisplayName: Battery Voltage sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery voltage sensing on pins A0 ~ A13. On the PX4-v1 it should be set to 100. On the Pixhawk, Pixracer and NAVIO boards it should be set to 2, Pixhawk2 Power2 is 13. + // @Values: -1:Disabled, 0:A0, 1:A1, 2:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 13:Pixhawk2_PM2, 100:PX4-v1 + // @User: Standard + AP_GROUPINFO("VOLT_PIN", 2, AP_BattMonitor_Params, _volt_pin, AP_BATT_VOLT_PIN), + + // @Param: CURR_PIN + // @DisplayName: Battery Current sensing pin + // @Description: Setting this to 0 ~ 13 will enable battery current sensing on pins A0 ~ A13. On the PX4-v1 it should be set to 101. On the Pixhawk, Pixracer and NAVIO boards it should be set to 3, Pixhawk2 Power2 is 14. + // @Values: -1:Disabled, 1:A1, 2:A2, 3:Pixhawk/Pixracer/Navio2/Pixhawk2_PM1, 14:Pixhawk2_PM2, 101:PX4-v1 + // @User: Standard + AP_GROUPINFO("CURR_PIN", 3, AP_BattMonitor_Params, _curr_pin, AP_BATT_CURR_PIN), + + // @Param: VOLT_MULT + // @DisplayName: Voltage Multiplier + // @Description: Used to convert the voltage of the voltage sensing pin (BATT_VOLT_PIN) to the actual battery's voltage (pin_voltage * VOLT_MULT). For the 3DR Power brick on APM2 or Pixhawk, this should be set to 10.1. For the Pixhawk with the 3DR 4in1 ESC this should be 12.02. For the PX using the PX4IO power supply this should be set to 1. + // @User: Advanced + AP_GROUPINFO("VOLT_MULT", 4, AP_BattMonitor_Params, _volt_multiplier, AP_BATT_VOLTDIVIDER_DEFAULT), + + // @Param: AMP_PERVLT + // @DisplayName: Amps per volt + // @Description: Number of amps that a 1V reading on the current sensor corresponds to. On the APM2 or Pixhawk using the 3DR Power brick this should be set to 17. For the Pixhawk with the 3DR 4in1 ESC this should be 17. + // @Units: A/V + // @User: Standard + AP_GROUPINFO("AMP_PERVLT", 5, AP_BattMonitor_Params, _curr_amp_per_volt, AP_BATT_CURR_AMP_PERVOLT_DEFAULT), + + // @Param: AMP_OFFSET + // @DisplayName: AMP offset + // @Description: Voltage offset at zero current on current sensor + // @Units: V + // @User: Standard + AP_GROUPINFO("AMP_OFFSET", 6, AP_BattMonitor_Params, _curr_amp_offset, 0), + + // @Param: CAPACITY + // @DisplayName: Battery capacity + // @Description: Capacity of the battery in mAh when full + // @Units: mA.h + // @Increment: 50 + // @User: Standard + AP_GROUPINFO("CAPACITY", 7, AP_BattMonitor_Params, _pack_capacity, 3300), + + // @Param: WATT_MAX + // @DisplayName: Maximum allowed power (Watts) + // @Description: If battery wattage (voltage * current) exceeds this value then the system will reduce max throttle (THR_MAX, TKOFF_THR_MAX and THR_MIN for reverse thrust) to satisfy this limit. This helps limit high current to low C rated batteries regardless of battery voltage. The max throttle will slowly grow back to THR_MAX (or TKOFF_THR_MAX ) and THR_MIN if demanding the current max and under the watt max. Use 0 to disable. + // @Units: W + // @Increment: 1 + // @User: Advanced + AP_GROUPINFO_FRAME("WATT_MAX", 8, AP_BattMonitor_Params, _watt_max, 0, AP_PARAM_FRAME_PLANE), + + // @Param: SERIAL_NUM + // @DisplayName: Battery serial number + // @Description: Battery serial number, automatically filled in for SMBus batteries, otherwise will be -1 + // @User: Advanced + AP_GROUPINFO("SERIAL_NUM", 9, AP_BattMonitor_Params, _serial_number, AP_BATT_SERIAL_NUMBER_DEFAULT), + + // @Param: LOW_TIMER + // @DisplayName: Low voltage timeout + // @Description: This is the timeout in seconds before a low voltage event will be triggered. For aircraft with low C batteries it may be necessary to raise this in order to cope with low voltage on long takeoffs. A value of zero disables low voltage errors. + // @Units: s + // @Increment: 1 + // @Range: 0 120 + // @User: Advanced + AP_GROUPINFO("LOW_TIMER", 10, AP_BattMonitor_Params, _low_voltage_timeout, 10), + + // @Param: FS_VOLTSRC + // @DisplayName: Failsafe voltage source + // @Description: Voltage type used for detection of low voltage event + // @Values: 0:Raw Voltage, 1:Sag Compensated Voltage + // @User: Advanced + AP_GROUPINFO("FS_VOLTSRC", 11, AP_BattMonitor_Params, _failsafe_voltage_source, BattMonitor_LowVoltageSource_Raw), + + AP_GROUPEND + +}; + +AP_BattMonitor_Params::AP_BattMonitor_Params(void) { + AP_Param::setup_object_defaults(this, var_info); +} diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_Params.h b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h new file mode 100644 index 0000000000..260327cc07 --- /dev/null +++ b/libraries/AP_BattMonitor/AP_BattMonitor_Params.h @@ -0,0 +1,46 @@ +#pragma once + +#include + +class AP_BattMonitor_Params { +public: + static const struct AP_Param::GroupInfo var_info[]; + + AP_BattMonitor_Params(void); + + /* Do not allow copies */ + AP_BattMonitor_Params(const AP_BattMonitor_Params &other) = delete; + AP_BattMonitor_Params &operator=(const AP_BattMonitor_Params&) = delete; + + // Battery monitor driver types + enum BattMonitor_Type { + BattMonitor_TYPE_NONE = 0, + BattMonitor_TYPE_ANALOG_VOLTAGE_ONLY = 3, + BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT = 4, + BattMonitor_TYPE_SOLO = 5, + BattMonitor_TYPE_BEBOP = 6, + BattMonitor_TYPE_MAXELL = 7 + }; + + // low voltage sources (used for BATT_LOW_TYPE parameter) + enum BattMonitor_LowVoltage_Source { + BattMonitor_LowVoltageSource_Raw = 0, + BattMonitor_LowVoltageSource_SagCompensated = 1 + }; + + BattMonitor_Type type(void) const { return (enum BattMonitor_Type)_type.get(); } + BattMonitor_LowVoltage_Source failsafe_voltage_source(void) { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); } + + AP_Int8 _type; /// 0=disabled, 3=voltage only, 4=voltage and current + AP_Int8 _volt_pin; /// board pin used to measure battery voltage + AP_Int8 _curr_pin; /// board pin used to measure battery current + AP_Float _volt_multiplier; /// voltage on volt pin multiplied by this to calculate battery voltage + AP_Float _curr_amp_per_volt; /// voltage on current pin multiplied by this to calculate current in amps + AP_Float _curr_amp_offset; /// offset voltage that is subtracted from current pin before conversion to amps + AP_Int32 _pack_capacity; /// battery pack capacity less reserve in mAh + AP_Int16 _watt_max; /// max battery power allowed. Reduce max throttle to reduce current to satisfy t his limit + AP_Int32 _serial_number; /// battery serial number, automatically filled in on SMBus batteries + AP_Int8 _low_voltage_timeout; /// timeout in seconds before a low voltage event will be triggered + AP_Int8 _failsafe_voltage_source; /// voltage type used for detection of low voltage event + +}; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp index 0f6e917949..ba3f4c0178 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.cpp @@ -9,12 +9,13 @@ AP_BattMonitor_SMBus::AP_BattMonitor_SMBus(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, AP_HAL::OwnPtr dev) - : AP_BattMonitor_Backend(mon, mon_state), + : AP_BattMonitor_Backend(mon, mon_state, params), _dev(std::move(dev)) { - _mon._serial_numbers[_state.instance] = AP_BATT_SERIAL_NUMBER_DEFAULT; - _mon._pack_capacity[_state.instance] = 0; + _params._serial_number = AP_BATT_SERIAL_NUMBER_DEFAULT; + _params._pack_capacity = 0; } void AP_BattMonitor_SMBus::init(void) { @@ -29,12 +30,12 @@ void AP_BattMonitor_SMBus::read(void) // nothing to be done here for actually interacting with the battery // however we can use this to set any parameters that need to be set - if (_serial_number != _mon._serial_numbers[_state.instance]) { - _mon._serial_numbers[_state.instance].set_and_notify(_serial_number); + if (_serial_number != _params._serial_number) { + _params._serial_number.set_and_notify(_serial_number); } - if (_full_charge_capacity != _mon._pack_capacity[_state.instance]) { - _mon._pack_capacity[_state.instance].set_and_notify(_full_charge_capacity); + if (_full_charge_capacity != _params._pack_capacity) { + _params._pack_capacity.set_and_notify(_full_charge_capacity); } } @@ -58,7 +59,7 @@ bool AP_BattMonitor_SMBus::read_full_charge_capacity(void) // we know the full charge capacity bool AP_BattMonitor_SMBus::read_remaining_capacity(void) { - int32_t capacity = get_capacity(); + int32_t capacity = _params._pack_capacity; if (capacity > 0) { uint16_t data; diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h index 77fa13233f..41b6093b92 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus.h @@ -18,6 +18,7 @@ public: /// Constructor AP_BattMonitor_SMBus(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, AP_HAL::OwnPtr dev); // virtual destructor to reduce compiler warnings diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp index a9855cd72a..5d04793298 100755 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.cpp @@ -38,8 +38,9 @@ uint8_t maxell_cell_ids[] = { 0x3f, // cell 1 // Constructor AP_BattMonitor_SMBus_Maxell::AP_BattMonitor_SMBus_Maxell(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, AP_HAL::OwnPtr dev) - : AP_BattMonitor_SMBus(mon, mon_state, std::move(dev)) + : AP_BattMonitor_SMBus(mon, mon_state, params, std::move(dev)) {} void AP_BattMonitor_SMBus_Maxell::timer() diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h index d0b4904d71..f3fefada9e 100755 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Maxell.h @@ -13,6 +13,7 @@ public: // Constructor AP_BattMonitor_SMBus_Maxell(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, AP_HAL::OwnPtr dev); private: diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp index 0aab01f2d5..3fb8805db7 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.cpp @@ -29,8 +29,9 @@ // Constructor AP_BattMonitor_SMBus_Solo::AP_BattMonitor_SMBus_Solo(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, AP_HAL::OwnPtr dev) - : AP_BattMonitor_SMBus(mon, mon_state, std::move(dev)) + : AP_BattMonitor_SMBus(mon, mon_state, params, std::move(dev)) { _pec_supported = true; } diff --git a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h index 8a4dd7861b..5eba0436ea 100644 --- a/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h +++ b/libraries/AP_BattMonitor/AP_BattMonitor_SMBus_Solo.h @@ -13,6 +13,7 @@ public: // Constructor AP_BattMonitor_SMBus_Solo(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, + AP_BattMonitor_Params ¶ms, AP_HAL::OwnPtr dev); private: diff --git a/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/AP_BattMonitor_test.cpp b/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/AP_BattMonitor_test.cpp index c35c8ebca7..aff28f874d 100644 --- a/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/AP_BattMonitor_test.cpp +++ b/libraries/AP_BattMonitor/examples/AP_BattMonitor_test/AP_BattMonitor_test.cpp @@ -18,7 +18,7 @@ void setup() { hal.console->printf("Battery monitor library test\n"); // set battery monitor to smbus - battery_mon.set_monitoring(0, AP_BattMonitor::BattMonitor_TYPE_SOLO); + battery_mon.set_monitoring(0, AP_BattMonitor_Params::BattMonitor_TYPE_SOLO); // initialise the battery monitor battery_mon.init();