diff --git a/libraries/AP_HAL_Linux/examples/BusTest/BusTest.cpp b/libraries/AP_HAL_Linux/examples/BusTest/BusTest.cpp index 4cb0d7dd72..1ca4aced3b 100644 --- a/libraries/AP_HAL_Linux/examples/BusTest/BusTest.cpp +++ b/libraries/AP_HAL_Linux/examples/BusTest/BusTest.cpp @@ -1,5 +1,3 @@ -// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- - // // scan I2C and SPI buses for expected devices // @@ -25,23 +23,25 @@ get_device(const char *name) { AP_HAL::OwnPtr dev; - const char *spi_name = new char; - spi_name = hal.spi->get_device_name(0); + const char *spi_name = hal.spi->get_device_name(0); /* Dummy is the default device at index 0 if there isn't registered device for the target board. */ - const char *dummy = {"**dummy**"}; + const char *dummy = "**dummy**"; if (!strcmp(dummy, spi_name)) { hal.console->printf("No devices, nothing to do.\n"); - while(1) { hal.scheduler->delay(500); }; + while (1) { + hal.scheduler->delay(500); + } } /* We get possible registered device count on the target board */ uint8_t spicount = hal.spi->get_count(); for (uint8_t ref = 0; ref < spicount; ref++) { - /* We get the name from the index and we compare - with our possible devices list. - */ + /* + * We get the name from the index and we compare + * with our possible devices list. + */ spi_name = hal.spi->get_device_name(ref); if (!strcmp(name, spi_name)) { dev = hal.spi->get_device(spi_name); @@ -67,34 +67,36 @@ void loop(void) for (uint8_t i = 0; i < ARRAY_SIZE(whoami_list); i++) { dev = get_device(whoami_list[i].name); - if (dev) { - - if (dev->bus_type == AP_HAL::Device::BusType::BUS_TYPE_SPI) { - bus_type = "SPI"; - } else { - bus_type = "I2C"; - } - - hal.console->printf("Device %s registered on %s bus\n", whoami_list[i].name, bus_type); - hal.console->printf("Trying to send data...\n"); - - spi_sem = dev->get_semaphore(); - if (!spi_sem->take(1000)) { - hal.console->printf("Failed to get SPI semaphore for %s\n", whoami_list[i].name); - break; - } - uint8_t tx[2] = { whoami_list[i].whoami_reg, 0 }; - uint8_t rx[2] = { 0, 0 }; - dev->transfer(tx, sizeof(tx), rx, sizeof(rx)); - if (rx[1] != 0) { - hal.console->printf("WHO_AM_I for %s: 0x%02x\n\n", whoami_list[i].name, (unsigned)rx[1]); - } else { - hal.console->printf("No response %s!\n\n", whoami_list[i].name); - break; - } - spi_sem->give(); + if (!dev) { + continue; + } + + if (dev->bus_type() == AP_HAL::Device::BusType::BUS_TYPE_SPI) { + bus_type = "SPI"; + } else { + bus_type = "I2C"; } + + hal.console->printf("Device %s registered on %s bus\n", whoami_list[i].name, bus_type); + hal.console->printf("Trying to send data...\n"); + + spi_sem = dev->get_semaphore(); + if (!spi_sem->take(1000)) { + hal.console->printf("Failed to get SPI semaphore for %s\n", whoami_list[i].name); + break; + } + uint8_t tx[2] = { whoami_list[i].whoami_reg, 0 }; + uint8_t rx[2] = { 0, 0 }; + dev->transfer(tx, sizeof(tx), rx, sizeof(rx)); + if (rx[1] != 0) { + hal.console->printf("WHO_AM_I for %s: 0x%02x\n\n", whoami_list[i].name, (unsigned)rx[1]); + } else { + hal.console->printf("No response %s!\n\n", whoami_list[i].name); + break; + } + spi_sem->give(); } + hal.scheduler->delay(1000); }