zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
34824 Commits
10 Branches
0 Tags
227 MiB
 
 
 
 
 
 
Tree: b346d82f96
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'b346d82f96'
${ noResults }
zr-v4/libraries/AP_Baro/examples/ICM20789/ICM20789.cpp

303 lines
8.0 KiB
Raw Blame History

/*
minimal test program for ICM20789 baro with IMU on SPI and baro on I2C
*/
#include <AP_HAL/AP_HAL.h>
#include <AP_HAL/I2CDevice.h>
#include <AP_Baro/AP_Baro.h>
#include <AP_BoardConfig/AP_BoardConfig.h>
#include <utility>
#include <stdio.h>
const AP_HAL::HAL& hal = AP_HAL::get_HAL();
static AP_HAL::OwnPtr<AP_HAL::Device> i2c_dev;
static AP_HAL::OwnPtr<AP_HAL::Device> spi_dev;
static AP_HAL::OwnPtr<AP_HAL::Device> dev;
// SPI registers
#define MPUREG_WHOAMI 0x75
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_INT_PIN_CFG 0x37
# define BIT_BYPASS_EN 0x02
# define BIT_INT_RD_CLEAR 0x10 // clear the interrupt when any read occurs
# define BIT_LATCH_INT_EN 0x20 // latch data ready pin
#define BIT_USER_CTRL_I2C_MST_EN 0x20 // Enable MPU to act as the I2C Master to external slave sensors
#define BIT_PWR_MGMT_1_DEVICE_RESET 0x80 // reset entire device
#define BIT_USER_CTRL_I2C_IF_DIS 0x10 // Disable primary I2C interface and enable hal.spi->interface
#define BIT_PWR_MGMT_1_CLK_XGYRO 0x01 // PLL with X axis gyroscope reference
#define BIT_PWR_MGMT_1_CLK_ZGYRO 0x03 // PLL with Z axis gyroscope reference
// baro commands
#define CMD_SOFT_RESET 0x805D
#define CMD_READ_ID 0xEFC8
void setup(void);
void loop(void);
#ifdef HAL_INS_MPU60x0_NAME
static void spi_init()
{
// SPI reads have flag 0x80 set
spi_dev->set_read_flag(0x80);
// run the SPI bus at low speed
spi_dev->set_speed(AP_HAL::Device::SPEED_LOW);
uint8_t whoami = 0;
uint8_t v;
spi_dev->write_register(0x6B, 0x01);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x6A, 0x10);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6C, 0x3f);
spi_dev->write_register(0xF5, 0x00);
spi_dev->write_register(0x19, 0x09);
spi_dev->write_register(0xEA, 0x00);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x6A, 0x10);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x23, 0x00);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x1D, 0xC0);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x1A, 0xC0);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x38, 0x01);
spi_dev->read_registers(0x6A, &v, 1);
printf("reg 0x6A=0x%02x\n", v);
spi_dev->read_registers(0x6B, &v, 1);
printf("reg 0x6B=0x%02x\n", v);
hal.scheduler->delay(1);
spi_dev->write_register(0x6A, 0x10);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x23, 0x00);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x41);
spi_dev->write_register(0x6C, 0x3f);
spi_dev->write_register(0x6B, 0x41);
spi_dev->read_registers(0x6A, &v, 1);
printf("reg 0x6A=0x%02x\n", v);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x6A, 0x10);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x23, 0x00);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->read_registers(0x6A, &v, 1);
printf("reg 0x6A=0x%02x\n", v);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x6A, 0x10);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x23, 0x00);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x41);
spi_dev->write_register(0x6C, 0x3f);
spi_dev->write_register(0x6B, 0x41);
spi_dev->read_registers(0x6A, &v, 1);
printf("reg 0x6A=0x%02x\n", v);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x6A, 0x10);
spi_dev->write_register(0x6B, 0x41);
hal.scheduler->delay(1);
spi_dev->write_register(0x6B, 0x01);
hal.scheduler->delay(1);
spi_dev->write_register(0x23, 0x00);
spi_dev->write_register(0x6B, 0x41);
// print the WHOAMI
spi_dev->read_registers(MPUREG_WHOAMI, &whoami, 1);
printf("20789 SPI whoami: 0x%02x\n", whoami);
// wait for sensor to settle
hal.scheduler->delay(100);
// dump registers
printf("ICM20789 registers\n");
#if 0
for (uint8_t reg = 0; reg<0x80; reg++) {
v=0;
spi_dev->read_registers(reg, &v, 1);
printf("%02x:%02x ", (unsigned)reg, (unsigned)v);
if ((reg+1) % 16 == 0) {
printf("\n");
}
}
printf("\n");
#endif
spi_dev->get_semaphore()->give();
}
#endif
/*
send a 16 bit command to the baro
*/
static bool send_cmd16(uint16_t cmd)
{
uint8_t cmd_b[2] = { uint8_t(cmd >> 8), uint8_t(cmd & 0xFF) };
return i2c_dev->transfer(cmd_b, 2, nullptr, 0);
}
/*
read baro calibration data
*/
static bool read_calibration_data(void)
{
// setup for OTP read
const uint8_t cmd[5] = { 0xC5, 0x95, 0x00, 0x66, 0x9C };
if (!i2c_dev->transfer(cmd, sizeof(cmd), nullptr, 0)) {
return false;
}
for (uint8_t i=0; i<4; i++) {
if (!send_cmd16(0xC7F7)) {
return false;
}
uint8_t d[3];
if (!i2c_dev->transfer(nullptr, 0, d, sizeof(d))) {
return false;
}
uint16_t c = int16_t((d[0]<<8) | d[1]);
printf("sensor_constants[%u]=%d\n", i, c);
}
return true;
}
#ifdef HAL_INS_MPU60x0_NAME
// initialise baro on i2c
static void i2c_init(void)
{
if (!i2c_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("Failed to get baro semaphore");
}
if (send_cmd16(CMD_READ_ID)) {
printf("ICM20789: read ID ******\n");
uint8_t id[3] {};
if (!i2c_dev->transfer(nullptr, 0, id, 3)) {
printf("ICM20789: failed to read ID\n");
}
printf("ICM20789: ID %02x %02x %02x\n", id[0], id[1], id[2]);
} else {
printf("ICM20789 read ID failed\n");
}
if (send_cmd16(CMD_SOFT_RESET)) {
printf("ICM20789: reset OK ************###########*********!!!!!!!!\n");
} else {
printf("ICM20789 baro reset failed\n");
}
hal.scheduler->delay(1);
read_calibration_data();
i2c_dev->get_semaphore()->give();
printf("checking baro\n");
if (!dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("Failed to get device semaphore");
}
uint8_t regs[3] = { 0xC0, 0xC1, 0xC2 };
for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {
uint8_t v = 0;
dev->read_registers(regs[i], &v, 1);
printf("0x%02x : 0x%02x\n", regs[i], v);
}
dev->get_semaphore()->give();
}
#endif
void setup()
{
printf("ICM20789 test\n");
AP_BoardConfig{}.init();
hal.scheduler->delay(1000);
#ifdef HAL_INS_MPU60x0_NAME
spi_dev = std::move(hal.spi->get_device(HAL_INS_MPU60x0_NAME));
if (!spi_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("Failed to get spi semaphore");
}
i2c_dev = std::move(hal.i2c_mgr->get_device(1, 0x63));
dev = std::move(hal.i2c_mgr->get_device(1, 0x29));
while (true) {
spi_init();
i2c_init();
hal.scheduler->delay(1000);
}
#else
while (true) {
printf("HAL_INS_MPU60x0_NAME not defined for this board\n");
hal.scheduler->delay(1000);
}
#endif
}
void loop()
{
}
AP_HAL_MAIN();