mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-02-06 07:53:57 -04:00
AP_Baro: implement ICM20789 barometer
This commit is contained in:
Andrew Tridgell
parent
5a4f0fed2d
commit
bf67153791
@ -35,6 +35,7 @@
|
||||
#include "AP_Baro_KellerLD.h"
|
||||
#include "AP_Baro_MS5611.h"
|
||||
#include "AP_Baro_LPS25H.h"
|
||||
#include "AP_Baro_ICM20789.h"
|
||||
#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_QFLIGHT
|
||||
#include "AP_Baro_qflight.h"
|
||||
#endif
|
||||
|
||||
318
libraries/AP_Baro/AP_Baro_ICM20789.cpp
Normal file
318
libraries/AP_Baro/AP_Baro_ICM20789.cpp
Normal file
@ -0,0 +1,318 @@
|
||||
/*
|
||||
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_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/I2CDevice.h>
|
||||
#include <utility>
|
||||
|
||||
#include <AP_Common/AP_Common.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_BoardConfig/AP_BoardConfig.h>
|
||||
#include "AP_Baro_ICM20789.h"
|
||||
|
||||
#include <utility>
|
||||
#include <stdio.h>
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <DataFlash/DataFlash.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
/*
|
||||
CMD_READ options. The draft datasheet doesn't specify, but it seems
|
||||
Mode_1 has a conversion interval of 2ms. Mode_3 has a conversion
|
||||
interval of 20ms. Both seem to produce equally as smooth results, so
|
||||
presumably Mode_3 is doing internal averaging
|
||||
*/
|
||||
#define CMD_READ_PT_MODE_1 0x401A
|
||||
#define CMD_READ_PT_MODE_3 0x5059
|
||||
#define CMD_READ_TP_MODE_1 0x609C
|
||||
#define CMD_READ_TP_MODE_3 0x70DF
|
||||
|
||||
#define CONVERSION_INTERVAL_MODE_1 2000
|
||||
#define CONVERSION_INTERVAL_MODE_3 20000
|
||||
|
||||
// setup for Mode_3
|
||||
#define CMD_READ_PT CMD_READ_PT_MODE_3
|
||||
#define CONVERSION_INTERVAL CONVERSION_INTERVAL_MODE_3
|
||||
|
||||
#define CMD_SOFT_RESET 0x805D
|
||||
#define CMD_READ_ID 0xEFC8
|
||||
|
||||
#define debug(fmt, args...) hal.console->printf(fmt, ##args)
|
||||
|
||||
/*
|
||||
constructor
|
||||
*/
|
||||
AP_Baro_ICM20789::AP_Baro_ICM20789(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> _dev)
|
||||
: AP_Baro_Backend(baro)
|
||||
, dev(std::move(_dev))
|
||||
{
|
||||
}
|
||||
|
||||
AP_Baro_Backend *AP_Baro_ICM20789::probe(AP_Baro &baro,
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev)
|
||||
{
|
||||
debug("Probing for ICM20789 baro\n");
|
||||
if (!dev) {
|
||||
return nullptr;
|
||||
}
|
||||
AP_Baro_ICM20789 *sensor = new AP_Baro_ICM20789(baro, std::move(dev));
|
||||
if (!sensor || !sensor->init()) {
|
||||
delete sensor;
|
||||
return nullptr;
|
||||
}
|
||||
return sensor;
|
||||
}
|
||||
|
||||
|
||||
bool AP_Baro_ICM20789::spi_init(void)
|
||||
{
|
||||
dev_icm->set_read_flag(0x80);
|
||||
|
||||
dev_icm->set_speed(AP_HAL::Device::SPEED_LOW);
|
||||
uint8_t whoami = 0;
|
||||
uint8_t v;
|
||||
|
||||
dev_icm->read_registers(0x6A, &v, 1);
|
||||
dev_icm->write_register(0x6B, 0x01);
|
||||
|
||||
hal.scheduler->delay(1);
|
||||
dev_icm->write_register(0x6A, 0x10);
|
||||
dev_icm->write_register(0x6B, 0x41);
|
||||
|
||||
hal.scheduler->delay(1);
|
||||
dev_icm->write_register(0x6B, 0x01);
|
||||
|
||||
hal.scheduler->delay(1);
|
||||
dev_icm->write_register(0x23, 0x00);
|
||||
dev_icm->write_register(0x6B, 0x41);
|
||||
|
||||
dev_icm->read_registers(0x75, &whoami, 1);
|
||||
|
||||
// wait for sensor to settle
|
||||
hal.scheduler->delay(100);
|
||||
|
||||
dev_icm->read_registers(0x75, &whoami, 1);
|
||||
|
||||
dev_icm->write_register(0x37, 0x00);
|
||||
dev_icm->write_register(0x6A, 0x10);
|
||||
return true;
|
||||
}
|
||||
|
||||
bool AP_Baro_ICM20789::init()
|
||||
{
|
||||
if (!dev) {
|
||||
return false;
|
||||
}
|
||||
|
||||
debug("Looking for 20789 baro\n");
|
||||
|
||||
if (!dev->get_semaphore()->take(0)) {
|
||||
AP_HAL::panic("PANIC: AP_Baro_ICM20789: failed to take serial semaphore for init");
|
||||
}
|
||||
|
||||
#ifdef HAL_INS_MPU60x0_NAME
|
||||
/*
|
||||
Pressure sensor data can be accessed in the following mode:
|
||||
Bypass Mode: Set register INT_PIN_CFG (Address: 55 (Decimal); 37 (Hex)) bit 1 to value 1
|
||||
and I2C_MST_EN bit is 0 Address: 106 (Decimal); 6A (Hex)).
|
||||
Pressure sensor data can then be accessed using the procedure described in Section 10.
|
||||
*/
|
||||
debug("Setting up IMU\n");
|
||||
dev_icm = hal.spi->get_device(HAL_INS_MPU60x0_NAME);
|
||||
|
||||
if (!dev_icm->get_semaphore()->take(0)) {
|
||||
AP_HAL::panic("PANIC: AP_Baro_ICM20789: failed to take serial semaphore ICM");
|
||||
}
|
||||
|
||||
if (!spi_init()) {
|
||||
debug("ICM20789: failed to initialise SPI device\n");
|
||||
goto failed;
|
||||
}
|
||||
|
||||
dev_icm->get_semaphore()->give();
|
||||
#endif // HAL_INS_MPU60x0_NAME
|
||||
|
||||
if (!send_cmd16(CMD_SOFT_RESET)) {
|
||||
debug("ICM20789: reset failed\n");
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// wait for sensor to settle
|
||||
hal.scheduler->delay(10);
|
||||
|
||||
if (!read_calibration_data()) {
|
||||
debug("ICM20789: read_calibration_data failed\n");
|
||||
goto failed;
|
||||
}
|
||||
|
||||
// start a reading
|
||||
if (!send_cmd16(CMD_READ_PT)) {
|
||||
debug("ICM20789: start read failed\n");
|
||||
goto failed;
|
||||
}
|
||||
|
||||
dev->set_retries(0);
|
||||
|
||||
instance = _frontend.register_sensor();
|
||||
|
||||
dev->get_semaphore()->give();
|
||||
|
||||
debug("ICM20789: startup OK\n");
|
||||
|
||||
// use 10ms to ensure we don't lose samples, with max lag of 10ms
|
||||
dev->register_periodic_callback(CONVERSION_INTERVAL/2, FUNCTOR_BIND_MEMBER(&AP_Baro_ICM20789::timer, void));
|
||||
|
||||
return true;
|
||||
|
||||
failed:
|
||||
dev_icm->get_semaphore()->give();
|
||||
dev->get_semaphore()->give();
|
||||
return false;
|
||||
}
|
||||
|
||||
bool AP_Baro_ICM20789::send_cmd16(uint16_t cmd)
|
||||
{
|
||||
uint8_t cmd_b[2] = { uint8_t(cmd >> 8), uint8_t(cmd & 0xFF) };
|
||||
return dev->transfer(cmd_b, 2, nullptr, 0);
|
||||
}
|
||||
|
||||
bool AP_Baro_ICM20789::read_calibration_data(void)
|
||||
{
|
||||
// setup for OTP read
|
||||
const uint8_t cmd[5] = { 0xC5, 0x95, 0x00, 0x66, 0x9C };
|
||||
if (!dev->transfer(cmd, sizeof(cmd), nullptr, 0)) {
|
||||
debug("ICM20789: read cal1 failed\n");
|
||||
return false;
|
||||
}
|
||||
for (uint8_t i=0; i<4; i++) {
|
||||
if (!send_cmd16(0xC7F7)) {
|
||||
debug("ICM20789: read cal2[%u] failed\n", i);
|
||||
return false;
|
||||
}
|
||||
uint8_t d[3];
|
||||
if (!dev->transfer(nullptr, 0, d, sizeof(d))) {
|
||||
debug("ICM20789: read cal3[%u] failed\n", i);
|
||||
return false;
|
||||
}
|
||||
sensor_constants[i] = int16_t((d[0]<<8) | d[1]);
|
||||
debug("sensor_constants[%u]=%d\n", i, sensor_constants[i]);
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
void AP_Baro_ICM20789::calculate_conversion_constants(const float p_Pa[3], const float p_LUT[3],
|
||||
float &A, float &B, float &C)
|
||||
{
|
||||
C = (p_LUT[0] * p_LUT[1] * (p_Pa[0] - p_Pa[1]) +
|
||||
p_LUT[1] * p_LUT[2] * (p_Pa[1] - p_Pa[2]) +
|
||||
p_LUT[2] * p_LUT[0] * (p_Pa[2] - p_Pa[0])) /
|
||||
(p_LUT[2] * (p_Pa[0] - p_Pa[1]) +
|
||||
p_LUT[0] * (p_Pa[1] - p_Pa[2]) +
|
||||
p_LUT[1] * (p_Pa[2] - p_Pa[0]));
|
||||
A = (p_Pa[0] * p_LUT[0] - p_Pa[1] * p_LUT[1] - (p_Pa[1] - p_Pa[0]) * C) / (p_LUT[0] - p_LUT[1]);
|
||||
B = (p_Pa[0] - A) * (p_LUT[0] + C);
|
||||
}
|
||||
|
||||
/*
|
||||
Convert an output from a calibrated sensor to a pressure in Pa.
|
||||
Arguments:
|
||||
p_LSB -- Raw pressure data from sensor
|
||||
T_LSB -- Raw temperature data from sensor
|
||||
*/
|
||||
float AP_Baro_ICM20789::get_pressure(uint32_t p_LSB, uint32_t T_LSB)
|
||||
{
|
||||
float t = T_LSB - 32768.0;
|
||||
float s[3];
|
||||
s[0] = LUT_lower + float(sensor_constants[0] * t * t) * quadr_factor;
|
||||
s[1] = offst_factor * sensor_constants[3] + float(sensor_constants[1] * t * t) * quadr_factor;
|
||||
s[2] = LUT_upper + float(sensor_constants[2] * t * t) * quadr_factor;
|
||||
float A, B, C;
|
||||
calculate_conversion_constants(p_Pa_calib, s, A, B, C);
|
||||
return A + B / (C + p_LSB);
|
||||
}
|
||||
|
||||
|
||||
static struct {
|
||||
uint32_t Praw, Traw;
|
||||
float T, P;
|
||||
} dd;
|
||||
|
||||
void AP_Baro_ICM20789::convert_data(uint32_t Praw, uint32_t Traw)
|
||||
{
|
||||
// temperature is easy
|
||||
float T = -45 + (175.0 / (1U<<16)) * Traw;
|
||||
|
||||
// pressure involves a few more calculations
|
||||
float P = get_pressure(Praw, Traw);
|
||||
|
||||
if (isinf(P) || isnan(P)) {
|
||||
// really bad data
|
||||
return;
|
||||
}
|
||||
|
||||
if (_sem->take(0)) {
|
||||
dd.Praw = Praw;
|
||||
dd.Traw = Traw;
|
||||
dd.P = P;
|
||||
dd.T = T;
|
||||
|
||||
accum.psum += P;
|
||||
accum.tsum += T;
|
||||
accum.count++;
|
||||
_sem->give();
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Baro_ICM20789::timer(void)
|
||||
{
|
||||
uint8_t d[9] {};
|
||||
if (dev->transfer(nullptr, 0, d, sizeof(d))) {
|
||||
// ignore CRC bytes for now
|
||||
uint32_t Praw = (uint32_t(d[0]) << 16) | (uint32_t(d[1]) << 8) | d[3];
|
||||
uint32_t Traw = (uint32_t(d[6]) << 8) | d[7];
|
||||
|
||||
convert_data(Praw, Traw);
|
||||
send_cmd16(CMD_READ_PT);
|
||||
last_measure_us = AP_HAL::micros();
|
||||
} else {
|
||||
if (AP_HAL::micros() - last_measure_us > CONVERSION_INTERVAL*3) {
|
||||
// lost a sample
|
||||
send_cmd16(CMD_READ_PT);
|
||||
last_measure_us = AP_HAL::micros();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void AP_Baro_ICM20789::update()
|
||||
{
|
||||
if (_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
|
||||
if (accum.count > 0) {
|
||||
_copy_to_frontend(instance, accum.psum/accum.count, accum.tsum/accum.count);
|
||||
accum.psum = accum.tsum = 0;
|
||||
accum.count = 0;
|
||||
}
|
||||
_sem->give();
|
||||
#if 0
|
||||
// useful for debugging
|
||||
DataFlash_Class::instance()->Log_Write("ICMB", "TimeUS,Traw,Praw,P,T", "QIIff",
|
||||
AP_HAL::micros64(),
|
||||
dd.Traw, dd.Praw, dd.P, dd.T);
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
65
libraries/AP_Baro/AP_Baro_ICM20789.h
Normal file
65
libraries/AP_Baro/AP_Baro_ICM20789.h
Normal file
@ -0,0 +1,65 @@
|
||||
#pragma once
|
||||
|
||||
#include "AP_Baro_Backend.h"
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_HAL/Semaphores.h>
|
||||
#include <AP_HAL/Device.h>
|
||||
|
||||
#ifndef HAL_BARO_ICM20789_I2C_ADDR
|
||||
// the baro is on a separate I2C address from the IMU, even though in
|
||||
// the same package
|
||||
#define HAL_BARO_ICM20789_I2C_ADDR 0x63
|
||||
#endif
|
||||
|
||||
class AP_Baro_ICM20789 : public AP_Baro_Backend
|
||||
{
|
||||
public:
|
||||
void update();
|
||||
|
||||
static AP_Baro_Backend *probe(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
|
||||
|
||||
private:
|
||||
AP_Baro_ICM20789(AP_Baro &baro, AP_HAL::OwnPtr<AP_HAL::Device> dev);
|
||||
virtual ~AP_Baro_ICM20789(void) {};
|
||||
|
||||
bool init();
|
||||
bool send_cmd16(uint16_t cmd);
|
||||
|
||||
bool read_calibration_data(void);
|
||||
|
||||
void convert_data(uint32_t Praw, uint32_t Traw);
|
||||
|
||||
void calculate_conversion_constants(const float p_Pa[3], const float p_LUT[3],
|
||||
float &A, float &B, float &C);
|
||||
float get_pressure(uint32_t p_LSB, uint32_t T_LSB);
|
||||
|
||||
bool spi_init(void);
|
||||
|
||||
void timer(void);
|
||||
|
||||
// calibration data
|
||||
int16_t sensor_constants[4];
|
||||
|
||||
uint8_t instance;
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev;
|
||||
AP_HAL::OwnPtr<AP_HAL::Device> dev_icm;
|
||||
|
||||
// time last read command was sent
|
||||
uint32_t last_measure_us;
|
||||
|
||||
// accumulation structure, protected by _sem
|
||||
struct {
|
||||
float tsum;
|
||||
float psum;
|
||||
uint32_t count;
|
||||
} accum;
|
||||
|
||||
// conversion constants. Thanks to invensense for including python
|
||||
// sample code in the datasheet!
|
||||
const float p_Pa_calib[3] = {45000.0, 80000.0, 105000.0};
|
||||
const float LUT_lower = 3.5 * (1U<<20);
|
||||
const float LUT_upper = 11.5 * (1U<<20);
|
||||
const float quadr_factor = 1 / 16777216.0;
|
||||
const float offst_factor = 2048.0;
|
||||
};
|
||||
291
libraries/AP_Baro/examples/ICM20789/ICM20789.cpp
Normal file
291
libraries/AP_Baro/examples/ICM20789/ICM20789.cpp
Normal file
@ -0,0 +1,291 @@
|
||||
/*
|
||||
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> lidar_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
|
||||
|
||||
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();
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
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;
|
||||
}
|
||||
|
||||
// initialise baro on i2c
|
||||
static void i2c_init(void)
|
||||
{
|
||||
if (!i2c_dev->get_semaphore()->take(0)) {
|
||||
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 lidar\n");
|
||||
if (!lidar_dev->get_semaphore()->take(0)) {
|
||||
AP_HAL::panic("Failed to get lidar semaphore");
|
||||
}
|
||||
|
||||
uint8_t regs[3] = { 0xC0, 0xC1, 0xC2 };
|
||||
for (uint8_t i=0; i<ARRAY_SIZE(regs); i++) {
|
||||
uint8_t v = 0;
|
||||
lidar_dev->read_registers(regs[i], &v, 1);
|
||||
printf("0x%02x : 0x%02x\n", regs[i], v);
|
||||
}
|
||||
lidar_dev->get_semaphore()->give();
|
||||
}
|
||||
|
||||
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(0)) {
|
||||
AP_HAL::panic("Failed to get spi semaphore");
|
||||
}
|
||||
|
||||
|
||||
i2c_dev = std::move(hal.i2c_mgr->get_device(1, 0x63));
|
||||
lidar_dev = std::move(hal.i2c_mgr->get_device(1, 0x29));
|
||||
|
||||
while (true) {
|
||||
spi_init();
|
||||
i2c_init();
|
||||
hal.scheduler->delay(1000);
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
void loop()
|
||||
{
|
||||
}
|
||||
|
||||
AP_HAL_MAIN();
|
||||
7
libraries/AP_Baro/examples/ICM20789/wscript
Normal file
7
libraries/AP_Baro/examples/ICM20789/wscript
Normal file
@ -0,0 +1,7 @@
|
||||
#!/usr/bin/env python
|
||||
# encoding: utf-8
|
||||
|
||||
def build(bld):
|
||||
bld.ap_example(
|
||||
use='ap',
|
||||
)
|
||||
Loading…
Reference in New Issue
Block a user