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_Baro/examples/ICM20789/ICM20789.cpp
Views: 1800
1
/*

2
  minimal test program for ICM20789 baro with IMU on SPI and baro on I2C

3
 */

4


5
#include <AP_HAL/AP_HAL.h>

6
#include <AP_HAL/I2CDevice.h>

7
#include <AP_Baro/AP_Baro.h>

8
#include <AP_BoardConfig/AP_BoardConfig.h>

9
#include <utility>

10
#include <stdio.h>

11


12
const AP_HAL::HAL& hal = AP_HAL::get_HAL();

13


14
static AP_HAL::OwnPtr<AP_HAL::Device> i2c_dev;

15
static AP_HAL::OwnPtr<AP_HAL::Device> spi_dev;

16
static AP_HAL::OwnPtr<AP_HAL::Device> dev;

17


18
// SPI registers

19
#define MPUREG_WHOAMI                           0x75

20
#define MPUREG_USER_CTRL                        0x6A

21
#define MPUREG_PWR_MGMT_1                       0x6B

22


23
#define MPUREG_INT_PIN_CFG                      0x37

24
#       define BIT_BYPASS_EN                        0x02

25
#       define BIT_INT_RD_CLEAR                     0x10    // clear the interrupt when any read occurs

26
#       define BIT_LATCH_INT_EN                     0x20    // latch data ready pin

27


28
#define BIT_USER_CTRL_I2C_MST_EN             0x20 // Enable MPU to act as the I2C Master to external slave sensors

29
#define BIT_PWR_MGMT_1_DEVICE_RESET          0x80 // reset entire device

30
#define BIT_USER_CTRL_I2C_IF_DIS             0x10 // Disable primary I2C interface and enable hal.spi->interface

31
#define BIT_PWR_MGMT_1_CLK_XGYRO             0x01 // PLL with X axis gyroscope reference

32
#define BIT_PWR_MGMT_1_CLK_ZGYRO             0x03 // PLL with Z axis gyroscope reference

33


34
// baro commands

35
#define CMD_SOFT_RESET     0x805D

36
#define CMD_READ_ID        0xEFC8

37


38
void setup(void);

39
void loop(void);

40


41


42
#ifdef HAL_INS_MPU60x0_NAME

43
static void spi_init()

44
{

45
    // SPI reads have flag 0x80 set

46
    spi_dev->set_read_flag(0x80);

47


48
    // run the SPI bus at low speed

49
    spi_dev->set_speed(AP_HAL::Device::SPEED_LOW);

50


51
    uint8_t whoami = 0;

52
    uint8_t v;

53


54
    spi_dev->write_register(0x6B, 0x01);

55
    spi_dev->write_register(0x6B, 0x01);

56


57
    hal.scheduler->delay(1);

58
    spi_dev->write_register(0x6A, 0x10);

59
    spi_dev->write_register(0x6B, 0x41);

60


61
    hal.scheduler->delay(1);

62
    spi_dev->write_register(0x6C, 0x3f);

63
    spi_dev->write_register(0xF5, 0x00);

64
    spi_dev->write_register(0x19, 0x09);

65
    spi_dev->write_register(0xEA, 0x00);

66
    spi_dev->write_register(0x6B, 0x01);

67


68
    hal.scheduler->delay(1);

69
    spi_dev->write_register(0x6A, 0x10);

70
    spi_dev->write_register(0x6B, 0x41);

71


72
    hal.scheduler->delay(1);

73
    spi_dev->write_register(0x6B, 0x01);

74


75
    hal.scheduler->delay(1);

76
    spi_dev->write_register(0x23, 0x00);

77
    spi_dev->write_register(0x6B, 0x41);

78


79
    hal.scheduler->delay(1);

80
    spi_dev->write_register(0x1D, 0xC0);

81
    spi_dev->write_register(0x6B, 0x01);

82


83
    hal.scheduler->delay(1);

84
    spi_dev->write_register(0x1A, 0xC0);

85
    spi_dev->write_register(0x6B, 0x41);

86


87
    hal.scheduler->delay(1);

88
    spi_dev->write_register(0x38, 0x01);

89


90
    spi_dev->read_registers(0x6A, &v, 1);

91
    printf("reg 0x6A=0x%02x\n", v);

92
    spi_dev->read_registers(0x6B, &v, 1);

93
    printf("reg 0x6B=0x%02x\n", v);

94


95
    hal.scheduler->delay(1);

96
    spi_dev->write_register(0x6A, 0x10);

97
    spi_dev->write_register(0x6B, 0x41);

98


99
    hal.scheduler->delay(1);

100
    spi_dev->write_register(0x6B, 0x01);

101


102
    hal.scheduler->delay(1);

103
    spi_dev->write_register(0x23, 0x00);

104
    spi_dev->write_register(0x6B, 0x41);

105


106
    hal.scheduler->delay(1);

107
    spi_dev->write_register(0x6B, 0x41);

108
    spi_dev->write_register(0x6C, 0x3f);

109
    spi_dev->write_register(0x6B, 0x41);

110


111
    spi_dev->read_registers(0x6A, &v, 1);

112
    printf("reg 0x6A=0x%02x\n", v);

113
    spi_dev->write_register(0x6B, 0x01);

114


115
    hal.scheduler->delay(1);

116
    spi_dev->write_register(0x6A, 0x10);

117
    spi_dev->write_register(0x6B, 0x41);

118


119
    hal.scheduler->delay(1);

120
    spi_dev->write_register(0x6B, 0x01);

121


122
    hal.scheduler->delay(1);

123
    spi_dev->write_register(0x23, 0x00);

124
    spi_dev->write_register(0x6B, 0x41);

125


126
    hal.scheduler->delay(1);

127
    spi_dev->read_registers(0x6A, &v, 1);

128
    printf("reg 0x6A=0x%02x\n", v);

129
    spi_dev->write_register(0x6B, 0x01);

130


131
    hal.scheduler->delay(1);

132
    spi_dev->write_register(0x6A, 0x10);

133
    spi_dev->write_register(0x6B, 0x41);

134


135
    hal.scheduler->delay(1);

136
    spi_dev->write_register(0x6B, 0x01);

137


138
    hal.scheduler->delay(1);

139
    spi_dev->write_register(0x23, 0x00);

140
    spi_dev->write_register(0x6B, 0x41);

141


142
    hal.scheduler->delay(1);

143
    spi_dev->write_register(0x6B, 0x41);

144
    spi_dev->write_register(0x6C, 0x3f);

145
    spi_dev->write_register(0x6B, 0x41);

146


147
    spi_dev->read_registers(0x6A, &v, 1);

148
    printf("reg 0x6A=0x%02x\n", v);

149
    spi_dev->write_register(0x6B, 0x01);

150


151
    hal.scheduler->delay(1);

152
    spi_dev->write_register(0x6A, 0x10);

153
    spi_dev->write_register(0x6B, 0x41);

154


155
    hal.scheduler->delay(1);

156
    spi_dev->write_register(0x6B, 0x01);

157


158
    hal.scheduler->delay(1);

159
    spi_dev->write_register(0x23, 0x00);

160
    spi_dev->write_register(0x6B, 0x41);

161


162
    // print the WHOAMI

163
    spi_dev->read_registers(MPUREG_WHOAMI, &whoami, 1);

164
    printf("20789 SPI whoami: 0x%02x\n", whoami);

165


166
    // wait for sensor to settle

167
    hal.scheduler->delay(100);

168


169
    // dump registers

170
    printf("ICM20789 registers\n");

171
#if 0

172
    for (uint8_t reg = 0; reg<0x80; reg++) {

173
        v=0;

174
        spi_dev->read_registers(reg, &v, 1);

175
        printf("%02x:%02x ", (unsigned)reg, (unsigned)v);

176
        if ((reg+1) % 16 == 0) {

177
            printf("\n");

178
        }

179
    }

180
    printf("\n");

181
#endif

182


183
    spi_dev->get_semaphore()->give();

184
}

185
#endif

186


187
#ifdef HAL_INS_MPU60x0_NAME

188
/*

189
  send a 16 bit command to the baro

190
 */

191
static bool send_cmd16(uint16_t cmd)

192
{

193
    uint8_t cmd_b[2] = { uint8_t(cmd >> 8), uint8_t(cmd & 0xFF) };

194
    return i2c_dev->transfer(cmd_b, 2, nullptr, 0);

195
}

196


197
/*

198
  read baro calibration data

199
 */

200
static bool read_calibration_data(void)

201
{

202
    // setup for OTP read

203
    const uint8_t cmd[5] = { 0xC5, 0x95, 0x00, 0x66, 0x9C };

204
    if (!i2c_dev->transfer(cmd, sizeof(cmd), nullptr, 0)) {

205
        return false;

206
    }

207
    for (uint8_t i=0; i<4; i++) {

208
        if (!send_cmd16(0xC7F7)) {

209
            return false;

210
        }

211
        uint8_t d[3];

212
        if (!i2c_dev->transfer(nullptr, 0, d, sizeof(d))) {

213
            return false;

214
        }

215
        uint16_t c = int16_t((d[0]<<8) | d[1]);

216
        printf("sensor_constants[%u]=%d\n", i, c);

217
    }

218
    return true;

219
}

220


221
// initialise baro on i2c

222
static void i2c_init(void)

223
{

224
    i2c_dev->get_semaphore()->take_blocking();

225


226
    if (send_cmd16(CMD_READ_ID)) {

227
        printf("ICM20789: read ID ******\n");

228
        uint8_t id[3] {};

229
        if (!i2c_dev->transfer(nullptr, 0, id, 3)) {

230
            printf("ICM20789: failed to read ID\n");

231
        }

232
        printf("ICM20789: ID %02x %02x %02x\n", id[0], id[1], id[2]);

233
    } else {

234
        printf("ICM20789 read ID failed\n");

235
    }

236


237
    if (send_cmd16(CMD_SOFT_RESET)) {

238
        printf("ICM20789: reset OK ************###########*********!!!!!!!!\n");

239
    } else {

240
        printf("ICM20789 baro reset failed\n");

241
    }

242
    hal.scheduler->delay(1);

243


244


245
    read_calibration_data();

246


247
    i2c_dev->get_semaphore()->give();

248


249
    printf("checking baro\n");

250
    dev->get_semaphore()->take_blocking();

251


252
    uint8_t regs[3] = { 0xC0, 0xC1, 0xC2 };

253
    for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {

254
        uint8_t v = 0;

255
        dev->read_registers(regs[i], &v, 1);

256
        printf("0x%02x : 0x%02x\n", regs[i], v);

257
    }

258
    dev->get_semaphore()->give();

259
}

260
#endif

261


262
void setup()

263
{

264
    printf("ICM20789 test\n");

265


266
    AP_BoardConfig{}.init();

267


268
    hal.scheduler->delay(1000);

269


270
#ifdef HAL_INS_MPU60x0_NAME

271
    spi_dev = std::move(hal.spi->get_device(HAL_INS_MPU60x0_NAME));

272


273
    spi_dev->get_semaphore()->take_blocking();

274


275
    i2c_dev = std::move(hal.i2c_mgr->get_device(1, 0x63));

276
    dev = std::move(hal.i2c_mgr->get_device(1, 0x29));

277


278
    while (true) {

279
        spi_init();

280
        i2c_init();

281
        hal.scheduler->delay(1000);

282
    }

283
#else

284
    while (true) {

285
        printf("HAL_INS_MPU60x0_NAME not defined for this board\n");

286
        hal.scheduler->delay(1000);

287
    }

288
#endif

289
}

290


291


292
void loop()

293
{

294
}

295


296
AP_HAL_MAIN();

297


298