CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutSign UpSign In
Ardupilot

Real-time collaboration for Jupyter Notebooks, Linux Terminals, LaTeX, VS Code, R IDE, and more,
all in one place. Commercial Alternative to JupyterHub.

GitHub Repository: Ardupilot/ardupilot
 Path: blob/master/libraries/AP_BattMonitor/AP_BattMonitor_INA3221.cpp
Views: 1798
1
#include "AP_BattMonitor_config.h"

2


3
#if AP_BATTERY_INA3221_ENABLED

4


5
#include "AP_BattMonitor_INA3221.h"

6


7
#include <AP_HAL/utility/sparse-endian.h>

8


9
#include <AP_HAL/AP_HAL.h>

10
#include <AP_HAL/I2CDevice.h>

11
#include <GCS_MAVLink/GCS.h>

12


13
#define INA3221_DEBUGGING 0

14


15
#if INA3221_DEBUGGING

16
#include <stdio.h>

17
#define debug(fmt, args ...)  do {printf("INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)

18
// #define debug(fmt, args ...)  do {gcs().send_text(MAV_SEVERITY_INFO, "INA3221: %s:%d: " fmt "\n", __FUNCTION__, __LINE__, ## args); } while(0)

19
#else

20
#define debug(fmt, args ...)

21
#endif

22


23
#ifndef HAL_BATTMON_INA3221_BUS

24
#define HAL_BATTMON_INA3221_BUS 0

25
#endif

26


27
#ifndef HAL_BATTMON_INA3221_ADDR

28
#define HAL_BATTMON_INA3221_ADDR 64

29
#endif

30


31
#ifndef HAL_BATTMON_INA3221_SHUNT_OHMS

32
#define HAL_BATTMON_INA3221_SHUNT_OHMS 0.001

33
#endif

34


35
#define HAL_BATTMON_INA3221_CONV_TIME_140US 0b000

36
#define HAL_BATTMON_INA3221_CONV_TIME_204US 0b001

37
#define HAL_BATTMON_INA3221_CONV_TIME_332US 0b010

38
#define HAL_BATTMON_INA3221_CONV_TIME_588US 0b011

39
#define HAL_BATTMON_INA3221_CONV_TIME_1100US 0b100

40
#define HAL_BATTMON_INA3221_CONV_TIME_2116US 0b101

41
#define HAL_BATTMON_INA3221_CONV_TIME_4156US 0b110

42
#define HAL_BATTMON_INA3221_CONV_TIME_8244US 0b111

43


44
#define HAL_BATTMON_INA3221_AVG_MODE_1 0b000

45
#define HAL_BATTMON_INA3221_AVG_MODE_4 0b001

46
#define HAL_BATTMON_INA3221_AVG_MODE_16 0b010

47
#define HAL_BATTMON_INA3221_AVG_MODE_64 0b011

48
#define HAL_BATTMON_INA3221_AVG_MODE_128 0b100

49
#define HAL_BATTMON_INA3221_AVG_MODE_256 0b101

50
#define HAL_BATTMON_INA3221_AVG_MODE_512 0b110

51
#define HAL_BATTMON_INA3221_AVG_MODE_1024 0b111

52


53
#ifndef HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL

54
#define HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US

55
#endif

56


57
#ifndef HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL

58
#define HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL HAL_BATTMON_INA3221_CONV_TIME_8244US

59
#endif

60


61
#ifndef HAL_BATTMON_INA3221_AVG_MODE_SEL

62
#define HAL_BATTMON_INA3221_AVG_MODE_SEL HAL_BATTMON_INA3221_AVG_MODE_1024

63
#endif

64


65
struct AP_BattMonitor_INA3221::AddressDriver AP_BattMonitor_INA3221::address_driver[HAL_BATTMON_INA3221_MAX_DEVICES];

66
uint8_t AP_BattMonitor_INA3221::address_driver_count;

67


68
const AP_Param::GroupInfo AP_BattMonitor_INA3221::var_info[] = {

69


70
    // @Param: I2C_BUS

71
    // @DisplayName: Battery monitor I2C bus number

72
    // @Description: Battery monitor I2C bus number

73
    // @Range: 0 3

74
    // @User: Advanced

75
    // @RebootRequired: True

76
    AP_GROUPINFO("I2C_BUS", 22, AP_BattMonitor_INA3221, i2c_bus, HAL_BATTMON_INA3221_BUS),

77


78
    // @Param: I2C_ADDR

79
    // @DisplayName: Battery monitor I2C address

80
    // @Description: Battery monitor I2C address. If this is zero then probe list of supported addresses

81
    // @Range: 0 127

82
    // @User: Advanced

83
    // @RebootRequired: True

84
    AP_GROUPINFO("I2C_ADDR", 23, AP_BattMonitor_INA3221, i2c_address, HAL_BATTMON_INA3221_ADDR),

85


86
    // @Param: CHANNEL

87
    // @DisplayName: INA3221 channel

88
    // @Description: INA3221 channel to return data for

89
    // @Range: 1 3

90
    // @User: Advanced

91
    // @RebootRequired: True

92
    AP_GROUPINFO("CHANNEL", 24, AP_BattMonitor_INA3221, channel, 1),

93


94
    // CHECK/UPDATE INDEX TABLE IN AP_BattMonitor_Backend.cpp WHEN CHANGING OR ADDING PARAMETERS

95


96
    AP_GROUPEND

97
};

98


99
extern const AP_HAL::HAL &hal;

100


101
AP_BattMonitor_INA3221::AP_BattMonitor_INA3221(

102
    AP_BattMonitor &mon,

103
    AP_BattMonitor::BattMonitor_State &mon_state,

104
    AP_BattMonitor_Params &params) :

105
    AP_BattMonitor_Backend(mon, mon_state, params)

106
{

107
    AP_Param::setup_object_defaults(this, var_info);

108
    _state.var_info = var_info;

109
}

110


111
bool AP_BattMonitor_INA3221::AddressDriver::read_register(uint8_t addr, uint16_t &ret)

112
{

113
    if (!dev->transfer(&addr, 1, (uint8_t*)&ret, 2)) {

114
        return false;

115
    }

116
    ret = be16toh(ret);

117
    return true;

118
}

119


120
bool AP_BattMonitor_INA3221::AddressDriver::write_register(uint8_t addr, uint16_t val)

121
{

122
    uint8_t buf[3] { addr, uint8_t(val >> 8), uint8_t(val & 0xFF) };

123


124
    return dev->transfer(buf, sizeof(buf), nullptr, 0);

125
}

126


127
#define REG_CONFIGURATION 0x00

128
#define REG_MANUFACTURER_ID 0xFE

129
#define REG_DIE_ID 0xFF

130


131
bool AP_BattMonitor_INA3221::AddressDriver::write_config(void)

132
{

133
    // update device configuration

134
    union {

135
        struct PACKED {

136
            uint16_t mode : 3;

137
            uint16_t shunt_voltage_conversiontime : 3;

138
            uint16_t bus_voltage_conversiontime : 3;

139
            uint16_t averaging_mode : 3;

140
            uint16_t ch1_enable : 1;

141
            uint16_t ch2_enable : 1;

142
            uint16_t ch3_enable : 1;

143
            uint16_t reset : 1;

144
        } bits;

145
        uint16_t word;

146
    } configuration {{

147
        0b111, // continuous operation

148
        HAL_BATTMON_INA3221_SHUNT_CONV_TIME_SEL,

149
        HAL_BATTMON_INA3221_BUS_CONV_TIME_SEL,

150
        HAL_BATTMON_INA3221_AVG_MODE_SEL,

151
        // dynamically enable channels to not waste time converting unused data

152
        (channel_mask & (1 << 1)) != 0, // enable ch1?

153
        (channel_mask & (1 << 2)) != 0, // enable ch2?

154
        (channel_mask & (1 << 3)) != 0, // enable ch3?

155
        0b0,  // don't reset...

156
    }};

157


158
    if (!write_register(REG_CONFIGURATION, configuration.word)) {

159
        return false;

160
    }

161


162
    dev_channel_mask = channel_mask; // what's actually in the device now

163


164
    return true;

165
}

166


167
void AP_BattMonitor_INA3221::init()

168
{

169
    uint8_t dev_address = i2c_address.get();

170
    uint8_t dev_bus = i2c_bus.get();

171
    uint8_t dev_channel = channel.get();

172


173
    if ((dev_channel < 1) || (dev_channel > 3)) {

174
        debug("INA3221: nonexistent channel");

175
        return;

176
    }

177


178
    debug("INA3221: probe ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus);

179


180
    // check to see if we already have the underlying driver reading the bus:

181
    for (uint8_t i=0; i<address_driver_count; i++) {

182
        AddressDriver *d = &address_driver[i];

183
        if (!d->dev) {

184
            continue;

185
        }

186
        if (d->address != dev_address) {

187
            continue;

188
        }

189
        if (d->bus != dev_bus) {

190
            continue;

191
        }

192
        debug("Reusing INA3221 driver @0x%02x on bus %u", dev_address, dev_bus);

193
        address_driver_state = NEW_NOTHROW AddressDriver::StateList;

194
        if (address_driver_state == nullptr) {

195
            return;

196
        }

197
        address_driver_state->channel = dev_channel;

198
        address_driver_state->next = d->statelist;

199
        d->statelist = address_driver_state;

200
        d->channel_mask |= (1 << dev_channel);

201
        return;

202
    }

203


204
    if (address_driver_count == ARRAY_SIZE(address_driver)) {

205
        debug("INA3221: out of address drivers");

206
        return;

207
    }

208


209
    AddressDriver *d = &address_driver[address_driver_count];

210
    d->dev = std::move(hal.i2c_mgr->get_device(i2c_bus, i2c_address, 100000, true, 20));

211
    if (!d->dev) {

212
        return;

213
    }

214
    d->bus = i2c_bus;

215
    d->address = i2c_address;

216


217
    WITH_SEMAPHORE(d->dev->get_semaphore());

218


219
    // check manufacturer_id

220
    uint16_t manufacturer_id;

221
    if (!d->read_register(REG_MANUFACTURER_ID, manufacturer_id)) {

222
        debug("read register (%u (0x%02x)) failed", REG_MANUFACTURER_ID, REG_MANUFACTURER_ID);

223
        return;

224
    }

225
    if (manufacturer_id != 0x5449) {  // 8.6.1 p24

226
        debug("Bad manufacturer_id: 0x%02x", manufacturer_id);

227
        return;

228
    }

229


230
    uint16_t die_id;

231
    if (!d->read_register(REG_DIE_ID, die_id)) {

232
        debug("Bad die: 0x%02x", manufacturer_id);

233
        return;

234
    }

235
    if (die_id != 0x3220) {  // 8.6.1 p24

236
        return;

237
    }

238


239
    d->channel_mask = (1 << dev_channel);

240
    if (!d->write_config()) {

241
        return;

242
    }

243


244
    address_driver_state = NEW_NOTHROW AddressDriver::StateList;

245
    if (address_driver_state == nullptr) {

246
        return;

247
    }

248
    address_driver_state->channel = dev_channel;

249
    address_driver_state->next = d->statelist;

250
    d->statelist = address_driver_state;

251


252
    debug("Found INA3221 ch%d@0x%02x on bus %u", dev_channel, dev_address, dev_bus);

253


254
    address_driver_count++;

255


256
    d->register_timer();

257
}

258


259
void AP_BattMonitor_INA3221::AddressDriver::register_timer(void)

260
{

261
    dev->register_periodic_callback(

262
        100000,

263
        FUNCTOR_BIND_MEMBER(&AP_BattMonitor_INA3221::AddressDriver::timer, void));

264
}

265


266
void AP_BattMonitor_INA3221::AddressDriver::timer(void)

267
{

268
    bool healthy = true;

269


270
    if (channel_mask != dev_channel_mask) {

271
        if (write_config()) { // update enabled channels

272
            return; // data is now out of date, read it next time

273
        }

274
        // continue on to reading if update failed so health gets cleared

275
        healthy = false;

276
    }

277


278
    for (uint8_t i=1; i<=3; i++) {

279
        if ((channel_mask & (1U<<i)) == 0) {

280
            continue;

281
        }

282
        const uint8_t channel_offset = (i-1)*2;

283
        const uint8_t reg_shunt = 1 + channel_offset;

284
        const uint8_t reg_bus = 2 + channel_offset;

285


286
        uint16_t shunt_val;

287
        if (!read_register(reg_shunt, shunt_val)) {

288
            healthy = false;

289
            shunt_val = 0;

290
        }

291
        uint16_t bus_val;

292
        if (!read_register(reg_bus, bus_val)) {

293
            healthy = false;

294
            bus_val = 0;

295
        }

296


297
        // 2s complement number with 3 lowest bits not used, 1 count is 8mV

298
        const float bus_voltage = ((int16_t)bus_val >> 3)*8e-3;

299
        // 2s complement number with 3 lowest bits not used, 1 count is 40uV

300
        const float shunt_voltage = ((int16_t)shunt_val >> 3)*40e-6;

301
        const float shunt_resistance = HAL_BATTMON_INA3221_SHUNT_OHMS;

302
        const float shunt_current = shunt_voltage/shunt_resistance; // I = V/R

303


304
        // transfer readings to state

305
        for (auto *state = statelist; state != nullptr; state = state->next) {

306
            if (state->channel != i) {

307
                continue;

308
            }

309
            WITH_SEMAPHORE(state->sem);

310


311
            // calculate time since last data read

312
            const uint32_t tnow = AP_HAL::micros();

313
            const uint32_t dt_us = tnow - state->last_time_micros;

314


315
            state->healthy = healthy;

316
            state->voltage = bus_voltage;

317
            state->current_amps = shunt_current;

318


319
            // update current drawn since last reading for read to accumulate

320
            if (state->last_time_micros != 0 && dt_us < 2000000) {

321
                const float mah = calculate_mah(state->current_amps, dt_us);

322
                state->delta_mah += mah;

323
                state->delta_wh  += 0.001 * mah * state->voltage;

324
            }

325


326
            state->last_time_micros = tnow;

327
        }

328
    }

329
}

330


331
void AP_BattMonitor_INA3221::read()

332
{

333
    if (address_driver_state == nullptr) {

334
        return;

335
    }

336


337
    WITH_SEMAPHORE(address_driver_state->sem);

338
    // copy state data to front-end under semaphore

339
    _state.healthy = address_driver_state->healthy;

340
    _state.voltage = address_driver_state->voltage;

341
    _state.current_amps = address_driver_state->current_amps;

342
    _state.consumed_mah += address_driver_state->delta_mah;

343
    _state.consumed_wh += address_driver_state->delta_wh;

344
    _state.last_time_micros = address_driver_state->last_time_micros;

345


346
    address_driver_state->delta_mah = 0;

347
    address_driver_state->delta_wh = 0;

348
}

349


350
#endif  // AP_BATTERY_INA3221_ENABLED

351


352