Add support for ACS37800 and mRo-M10104

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -29,6 +29,7 @@
 #include "AP_BattMonitor_AD7091R5.h"
 #include "AP_BattMonitor_Scripting.h"
 #include "AP_BattMonitor_TIBQ76952.h"
+#include "AP_BattMonitor_ACS37800.h"
 
 #include <AP_HAL/AP_HAL.h>
 
@@ -713,6 +714,11 @@ AP_BattMonitor::init()
                 drivers[instance] = NEW_NOTHROW AP_BattMonitor_TIBQ76952(*this, state[instance], _params[instance]);
                 break;
 #endif // AP_BATTERY_TIBQ76952_ENABLED
+#if AP_BATTERY_ACS37800_ENABLED
+            case Type::ACS37800_I2C:
+                drivers[instance] = NEW_NOTHROW AP_BattMonitor_ACS37800(*this, state[instance], _params[instance]);
+                break;
+#endif // AP_BATTERY_ACS37800_ENABLED
             case Type::NONE:
             default:
                 break;

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -44,7 +44,7 @@ class AP_BattMonitor_Torqeedo;
 class AP_BattMonitor_FuelLevel_Analog;
 class AP_BattMonitor_EFI;
 class AP_BattMonitor_Scripting;
-
+class AP_BattMonitor_ACS37800;
 
 class AP_BattMonitor
 {
@@ -66,6 +66,7 @@ class AP_BattMonitor
     friend class AP_BattMonitor_LTC2946;
     friend class AP_BattMonitor_AD7091R5;
     friend class AP_BattMonitor_INA3221;
+    friend class AP_BattMonitor_ACS37800;
 
     friend class AP_BattMonitor_Torqeedo;
     friend class AP_BattMonitor_FuelLevel_Analog;

libraries/AP_BattMonitor/AP_BattMonitor_ACS37800.cpp

@@ -0,0 +1,190 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "AP_BattMonitor_ACS37800.h"
+
+#if AP_BATTERY_ACS37800_ENABLED
+
+#include <AP_HAL/AP_HAL.h>
+#include <GCS_MAVLink/GCS.h>
+#include <AP_HAL/utility/sparse-endian.h>
+#include "AP_BattMonitor_ACS37800.h"
+
+extern const AP_HAL::HAL& hal;
+
+typedef struct
+{
+  union
+  {
+    uint32_t all;
+    struct
+    {
+      uint32_t vcodes : 16;
+      uint32_t icodes : 16;
+    } bits;
+  } data;
+} acs378002a_t;
+
+#define RESIST_MULT 202.5
+
+#define REG_ACCESS_CODE 0x2F // access_code
+#define CUSTOMER_CODE 0x4F70656E
+#define REG_C_EEPROM 0x0C
+#define REG_F_SHADOW 0x1F
+#define REG_F_EEPROM 0x0F
+#define CONFIG_C_MASK 0x0000000F
+#define CONFIG_F 0x01FFC000 // bypass_N_en=1, n=1023
+
+#define REG_VRMS_IRMS 0x20 
+#define REG_VRMS_IRMS_ONESEC 0x26
+#define REG_VCODES_ICODES 0x2A
+#define REG_PINSTANT 0x2C
+
+#ifndef HAL_BATTMON_ACS37800_BUS
+#define HAL_BATTMON_ACS37800_BUS 0
+#endif
+#ifndef HAL_BATTMON_ACS37800_ADDR
+#define HAL_BATTMON_ACS37800_ADDR 96
+#endif
+
+// ACS37800-I2C provides voltage, current, and power measurements, however, it does not provide any means for calibration.
+// Thus, added V_FINE_M and C_FINE_M to adjust the voltage and current readings.
+const AP_Param::GroupInfo AP_BattMonitor_ACS37800::var_info[] = {
+
+    // @Param: I2C_BUS
+    // @DisplayName: Battery monitor I2C bus number
+    // @Description: Battery monitor I2C bus number
+    // @Range: 0 4
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("I2C_BUS", 138, AP_BattMonitor_ACS37800, i2c_bus, HAL_BATTMON_ACS37800_BUS),
+
+    // @Param: I2C_ADDR
+    // @DisplayName: Battery monitor I2C address
+    // @Description: Battery monitor I2C address
+    // @Range: 0 127
+    // @User: Advanced
+    // @RebootRequired: True
+    AP_GROUPINFO("I2C_ADDR", 139, AP_BattMonitor_ACS37800, i2c_address, HAL_BATTMON_ACS37800_ADDR),
+
+    // @Param: V_FINE_M
+    // @DisplayName: Voltage fine adjustment
+    // @Description: Fine-adjust the voltage reading. Calculate dividing the indicated measured voltage by the known applied voltage.
+    // @User: Advanced
+    AP_GROUPINFO("V_FINE_M", 8, AP_BattMonitor_ACS37800, volt_fine_mult, 1.0),
+
+    // @Param: C_FINE_M
+    // @DisplayName: Current fine adjustment
+    // @Description: Fine-adjust the current reading. Calculate dividing the indicated measured current by the known current draw.
+    // @User: Advanced
+    AP_GROUPINFO("C_FINE_M", 9, AP_BattMonitor_ACS37800, curr_fine_mult, 1.0),
+
+    AP_GROUPEND
+};
+
+AP_BattMonitor_ACS37800::AP_BattMonitor_ACS37800(AP_BattMonitor &mon, 
+                            AP_BattMonitor::BattMonitor_State &mon_state, 
+                            AP_BattMonitor_Params &params):
+        AP_BattMonitor_Backend(mon, mon_state, params)
+{
+    AP_Param::setup_object_defaults(this, var_info);
+    _state.var_info = var_info;
+}
+
+void AP_BattMonitor_ACS37800::init(void)
+{
+    _dev = hal.i2c_mgr->get_device_ptr(i2c_bus, i2c_address, 100000, false, 20);
+    if (!_dev) {
+        return;
+    }
+    //Device found
+    WITH_SEMAPHORE(_dev->get_semaphore());
+    uint32_t f_config;
+    read_word(REG_F_SHADOW, f_config);
+    // if sensor is already configured, no need to write
+    if (f_config != CONFIG_F) { 
+        // first time using the sensor -> write config to EEPROM
+        uint32_t c_config;
+        // enter "customer mode" by writing the access code - so EEPROM is writeable
+        write_word(REG_ACCESS_CODE, CUSTOMER_CODE);
+        hal.scheduler->delay(50);
+        // write configuration for DC operation 
+        write_word(REG_F_EEPROM, CONFIG_F); 
+        hal.scheduler->delay(90);
+        read_word(REG_C_EEPROM, c_config);
+        c_config |= CONFIG_C_MASK;
+        write_word(REG_C_EEPROM, c_config);
+        hal.scheduler->delay(100);
+    } 
+
+    _dev->register_periodic_callback(25000, FUNCTOR_BIND_MEMBER(&AP_BattMonitor_ACS37800::update, void));
+}
+
+void AP_BattMonitor_ACS37800::read()
+{
+    WITH_SEMAPHORE(accumulate.sem);
+    _state.healthy = accumulate.count > 0;
+    if (!_state.healthy) {
+        return;
+    }
+
+    _state.voltage = accumulate.volt_sum / accumulate.count;
+    _state.current_amps = accumulate.current_sum / accumulate.count;
+    accumulate.volt_sum = 0;
+    accumulate.current_sum = 0;
+    accumulate.count = 0;
+
+    const uint32_t tnow = AP_HAL::micros();
+    const uint32_t dt_us = tnow - _state.last_time_micros;
+
+    // update total current drawn since startup
+    update_consumed(_state, dt_us);
+    _state.last_time_micros = tnow;
+}
+
+void AP_BattMonitor_ACS37800::update()
+{
+    WITH_SEMAPHORE(_dev->get_semaphore());
+    acs378002a_t immediate;
+    if (read_word(REG_VCODES_ICODES, immediate.data.all)) {     
+        WITH_SEMAPHORE(accumulate.sem);
+        accumulate.volt_sum += convert_voltage(immediate.data.bits.vcodes);
+        accumulate.current_sum += convert_current(immediate.data.bits.icodes);
+        accumulate.count++;
+    }
+}
+
+float AP_BattMonitor_ACS37800::convert_voltage(int16_t vcode)
+{
+    return (((((float) vcode)/27536.13)*250)/1000)*RESIST_MULT*volt_fine_mult;
+}
+
+float AP_BattMonitor_ACS37800::convert_current(int16_t icode)
+{
+    return (((float) icode)/32768.0)*90.0*curr_fine_mult;
+}
+
+bool AP_BattMonitor_ACS37800::write_word(const uint8_t reg, const uint32_t data) const
+{
+    const uint8_t b[5] { reg,  uint8_t (data&0xff), uint8_t(data >> 8), uint8_t(data >> 16), uint8_t(data >> 24) };
+    return _dev->transfer(b, sizeof(b), nullptr, 0);
+}
+
+bool AP_BattMonitor_ACS37800::read_word(const uint8_t reg, uint32_t& data) const
+{
+    return _dev->read_registers(reg, (uint8_t *)&data, sizeof(data));
+}
+
+#endif

libraries/AP_BattMonitor/AP_BattMonitor_ACS37800.h

@@ -0,0 +1,62 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include "AP_BattMonitor_config.h"
+
+#if AP_BATTERY_ACS37800_ENABLED
+
+#include <AP_Common/AP_Common.h>
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/I2CDevice.h>
+#include <utility>
+#include "AP_BattMonitor_Backend.h"
+
+class AP_BattMonitor_ACS37800 : public AP_BattMonitor_Backend
+{
+public:
+    AP_BattMonitor_ACS37800(AP_BattMonitor &mon, AP_BattMonitor::BattMonitor_State &mon_state, AP_BattMonitor_Params &params);
+
+    bool has_current() const override { return true; }
+
+    void init(void) override;
+    void read() override;
+    void update();
+
+    static const struct AP_Param::GroupInfo var_info[];
+
+protected:
+    AP_HAL::I2CDevice *_dev;
+
+    AP_Int8 i2c_bus;
+    AP_Int8 i2c_address;
+    AP_Float volt_fine_mult;
+    AP_Float curr_fine_mult;
+
+    struct {
+        uint16_t count;
+        float volt_sum;
+        float current_sum;
+        HAL_Semaphore sem;
+    } accumulate;
+
+    float convert_voltage(int16_t vcode);
+    float convert_current(int16_t icode);
+
+    bool write_word(const uint8_t reg, const uint32_t data) const;
+    bool read_word(const uint8_t reg, uint32_t& data) const;
+};
+
+#endif // AP_BATTERY_ACS37800_ENABLED

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -53,6 +53,7 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @Values: 30:INA3221
     // @Values: 31:Analog Current Only
     // @Values: 32:TIBQ76952-I2C (Periph only)
+    // @Values: 33:ACS37800-I2C
     // @User: Standard
     // @RebootRequired: True
     AP_GROUPINFO_FLAGS("MONITOR", 1, AP_BattMonitor_Params, _type, int8_t(AP_BattMonitor::Type::NONE), AP_PARAM_FLAG_ENABLE),

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -44,6 +44,7 @@ class AP_BattMonitor_Params {
         INA3221                        = 30,
         ANALOG_CURRENT_ONLY            = 31,
         TIBQ76952_I2C                  = 32,
+        ACS37800_I2C                   = 33,
     };
 
     // low voltage sources (used for BATT_LOW_TYPE parameter)

libraries/AP_BattMonitor/AP_BattMonitor_config.h

@@ -108,6 +108,10 @@
 #define AP_BATTERY_AD7091R5_ENABLED AP_BATTERY_BACKEND_DEFAULT_ENABLED && (HAL_PROGRAM_SIZE_LIMIT_KB > 1024)
 #endif
 
+#ifndef AP_BATTERY_ACS37800_ENABLED
+#define AP_BATTERY_ACS37800_ENABLED 0
+#endif
+
 // SMBus-subclass backends:
 #ifndef AP_BATTERY_SMBUS_GENERIC_ENABLED
 #define AP_BATTERY_SMBUS_GENERIC_ENABLED AP_BATTERY_SMBUS_ENABLED

libraries/AP_HAL_ChibiOS/hwdef/mRo-M10104/README.md

@@ -0,0 +1,51 @@
+# mRo CAN Power Board - M10104
+
+The sensor is based on Allegro Micro ACS37800 power monitoring IC, which provides 0.85 mΩ primary conductor resistance for low power loss and high inrush current capabilities. 
+
+## Electrical: 
+
+- 50.4V 12S LiPo
+- 60 Amps**Continuous**- 90 Amps Max Current Sensing
+- 5.3V, 3A ultra low noise power supply
+
+## Mechanical:
+
+- Board Weight: 5.90g (0.208 oz)
+- Board Dimensions:
+  - 26mm x 26mm (1.02"x1.02")
+
+## Parameters:
+
+- Set `BATT_MONITORx=8` (CAN)
+
+NOTE: The module comes with factory calibrated values. End users are encouraged to verify and tune the parameters for the best accuracy.
+`BATT_V_FINE_M` and `BATT_C_FINE_M` are multipliers set to 1.0 by default to fine tune your power readings. Keep a safe margin of battery capacity for your tests until verified.
+
+## Port pinout
+
+### CAN+PWR Connector
+
+6-pin Molex Clik-Mate
+
+| Pin | Color | Signal | TTL/Voltage Level |
+| --- | ----- | ------ | ----------------- |
+| 1   | red   | VCC    | 5.3V              |
+| 2   | black | VCC    | 5.3V              |
+| 3   | black | CAN_H  |                   |
+| 4   | black | CAN_L  |                   |
+| 5   | black | GND    | N/A               |
+| 6   | black | GND    | N/A               |
+
+### CAN
+
+| Pin | Color | Signal | TTL/Voltage Level |
+| --- | ----- | ------ | ----------------- |
+| 1   | red   | VCC    | 5V                |
+| 2   | black | CAN_H     |
+| 3   | black | CAN_L     |
+| 4   | black | GND    | N/A
+
+## More questions?
+Contact us [info@3dr.com](mailto:info@3dr.com)
+
+[Designed and assembled in USA]