aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Baro: ICM20789: example fixes

Browse Source
This commit is contained in:
Peter Barker 2018-01-02 12:02:46 +11:00 committed by Andrew Tridgell
parent bf95be96f3
commit 3a0c5bed85
1 changed files with 14 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

23
libraries/AP_Baro/examples/ICM20789/ICM20789.cpp
View File

@ -13,7 +13,7 @@ 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;
static AP_HAL::OwnPtr<AP_HAL::Device> dev;
// SPI registers
#define MPUREG_WHOAMI 0x75
@ -215,7 +215,7 @@ static bool read_calibration_data(void)
// initialise baro on i2c
static void i2c_init(void)
{
if (!i2c_dev->get_semaphore()->take(0)) {
if (!i2c_dev->get_semaphore()->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
AP_HAL::panic("Failed to get baro semaphore");
}
@ -242,18 +242,18 @@ static void i2c_init(void)
i2c_dev->get_semaphore()->give();
printf("checking lidar\n");
if (!lidar_dev->get_semaphore()->take(0)) {
AP_HAL::panic("Failed to get lidar semaphore");
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;
lidar_dev->read_registers(regs[i], &v, 1);
dev->read_registers(regs[i], &v, 1);
printf("0x%02x : 0x%02x\n", regs[i], v);
}
lidar_dev->get_semaphore()->give();
dev->get_semaphore()->give();
}
void setup()
@ -267,19 +267,24 @@ void setup()
#ifdef HAL_INS_MPU60x0_NAME
spi_dev = std::move(hal.spi->get_device(HAL_INS_MPU60x0_NAME));
if (!spi_dev->get_semaphore()->take(0)) {
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));
lidar_dev = std::move(hal.i2c_mgr->get_device(1, 0x29));
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
}