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.
65 lines
1.9 KiB
65 lines
1.9 KiB
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- |
|
|
|
// |
|
// scan I2C and SPI buses for expected devices |
|
// |
|
|
|
#include <AP_Common/AP_Common.h> |
|
#include <AP_Progmem/AP_Progmem.h> |
|
#include <AP_HAL/AP_HAL.h> |
|
#include <AP_HAL_Linux/AP_HAL_Linux.h> |
|
#include <AP_HAL_Empty/AP_HAL_Empty.h> |
|
#include <AP_Math/AP_Math.h> |
|
#include <AP_Param/AP_Param.h> |
|
#include <StorageManager/StorageManager.h> |
|
|
|
const AP_HAL::HAL& hal = AP_HAL_BOARD_DRIVER; |
|
|
|
void setup(void) |
|
{ |
|
hal.console->println("BusTest startup..."); |
|
} |
|
|
|
static struct { |
|
const char *name; |
|
enum AP_HAL::SPIDevice dev; |
|
uint8_t whoami_reg; |
|
} whoami_list[] = { |
|
{ "MS5611", AP_HAL::SPIDevice_MS5611, 0x00 | 0x80 }, |
|
{ "MPU9250", AP_HAL::SPIDevice_MPU9250, 0x75 | 0x80 }, |
|
{ "MPU6000", AP_HAL::SPIDevice_MPU6000, 0x75 | 0x80 }, |
|
{ "FRAM", AP_HAL::SPIDevice_Dataflash, 0x00 | 0x80 }, |
|
{ "LSM9DS0_AM", AP_HAL::SPIDevice_LSM9DS0_AM, 0x0F | 0x80 }, |
|
{ "LSM9DS0_G", AP_HAL::SPIDevice_LSM9DS0_G, 0x0F | 0x80 }, |
|
}; |
|
|
|
void loop(void) |
|
{ |
|
AP_HAL::SPIDeviceDriver *spi; |
|
AP_HAL::Semaphore *spi_sem; |
|
|
|
hal.console->printf("Scanning SPI bus devices\n"); |
|
|
|
for (uint8_t i=0; i < ARRAY_SIZE(whoami_list); i++) { |
|
spi = hal.spi->device(whoami_list[i].dev); |
|
if (spi == NULL) { |
|
hal.console->printf("Failed to get SPI device for %s\n", whoami_list[i].name); |
|
continue; |
|
} |
|
spi_sem = spi->get_semaphore(); |
|
if (!spi_sem->take(1000)) { |
|
hal.console->printf("Failed to get SPI semaphore for %s\n", whoami_list[i].name); |
|
continue; |
|
} |
|
uint8_t tx[2] = { whoami_list[i].whoami_reg, 0 }; |
|
uint8_t rx[2]; |
|
|
|
spi->transaction(tx, rx, 2); |
|
|
|
hal.console->printf("WHO_AM_I for %s: 0x%02x\n", whoami_list[i].name, (unsigned)rx[1]); |
|
spi_sem->give(); |
|
} |
|
hal.scheduler->delay(200); |
|
} |
|
|
|
AP_HAL_MAIN();
|
|
|